constrained-dynamics.hpp

//
// Copyright (c) 2019-2021 INRIA
//

#pragma once

// IWYU pragma: begin_keep
#include <cassert>
#include <cstddef>
#include <vector>

#include <Eigen/Core>

#include <boost/fusion/container/vector.hpp>

#include "pinocchio/config.hpp"
#include "pinocchio/context.hpp"
#include "pinocchio/unsupported.hpp"
#include "pinocchio/macros.hpp"

#include "pinocchio/utils/check.hpp"

#include "pinocchio/math.hpp"

#include "pinocchio/spatial.hpp"

#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"

#include "pinocchio/algorithm/fwd.hpp"
#include "pinocchio/algorithm/proximal.hpp"
#include "pinocchio/algorithm/check-model.hpp"
#include "pinocchio/constraints.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/constraint-cholesky.hpp"
// IWYU pragma: end_keep

namespace pinocchio
{

 template<
 typename Scalar,
 int Options,
 template<typename, int> class JointCollectionTpl,
 class ConstraintModel,
 class ConstraintModelAllocator,
 class ConstraintData,
 class ConstraintDataAllocator>
 inline void initConstraintDynamics(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 DataTpl<Scalar, Options, JointCollectionTpl> & data,
 const std::vector<ConstraintModel, ConstraintModelAllocator> & contact_models,
 const std::vector<ConstraintData, ConstraintDataAllocator> & contact_datas);


 template<
 typename Scalar,
 int Options,
 template<typename, int> class JointCollectionTpl,
 typename ConfigVectorType,
 typename TangentVectorType1,
 typename TangentVectorType2,
 class ConstraintModelAllocator,
 class ConstraintDataAllocator>
 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
 inline const
 typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 DataTpl<Scalar, Options, JointCollectionTpl> & data,
 const Eigen::MatrixBase<ConfigVectorType> & q,
 const Eigen::MatrixBase<TangentVectorType1> & v,
 const Eigen::MatrixBase<TangentVectorType2> & tau,
 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
 contact_models,
 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
 ProximalSettingsTpl<Scalar> & settings);


 template<
 typename Scalar,
 int Options,
 template<typename, int> class JointCollectionTpl,
 typename ConfigVectorType,
 typename TangentVectorType1,
 typename TangentVectorType2,
 class ConstraintModelAllocator,
 class ConstraintDataAllocator>
 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
 inline const
 typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 DataTpl<Scalar, Options, JointCollectionTpl> & data,
 const Eigen::MatrixBase<ConfigVectorType> & q,
 const Eigen::MatrixBase<TangentVectorType1> & v,
 const Eigen::MatrixBase<TangentVectorType2> & tau,
 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
 contact_models,
 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas)
 {
 ProximalSettingsTpl<Scalar> settings;
 return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, settings);
 }

 template<
 typename Scalar,
 int Options,
 template<typename, int> class JointCollectionTpl,
 typename ConfigVectorType,
 typename TangentVectorType1,
 typename TangentVectorType2,
 class ModelAllocator,
 class DataAllocator>
 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
 inline const
 typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & contactABA(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 DataTpl<Scalar, Options, JointCollectionTpl> & data,
 const Eigen::MatrixBase<ConfigVectorType> & q,
 const Eigen::MatrixBase<TangentVectorType1> & v,
 const Eigen::MatrixBase<TangentVectorType2> & tau,
 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
 std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data)
 {
 ProximalSettingsTpl<Scalar> settings = ProximalSettingsTpl<Scalar>();
 return contactABA(
 model, data, q.derived(), v.derived(), tau.derived(), contact_models, contact_data, settings);
 }

} // namespace pinocchio

// IWYU pragma: begin_exports
#include "pinocchio/src/algorithm/constrained-dynamics.hxx"
// IWYU pragma: end_exports