pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
constrained-dynamics.hpp
1 //
2 // Copyright (c) 2019-2021 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_constrained_dynamics_hpp__
6 #define __pinocchio_algorithm_constrained_dynamics_hpp__
7 
8 #include "pinocchio/algorithm/contact-info.hpp"
9 
10 #include "pinocchio/algorithm/proximal.hpp"
11 
12 namespace pinocchio
13 {
14 
29  template<
30  typename Scalar,
31  int Options,
32  template<typename, int> class JointCollectionTpl,
33  class Allocator>
34  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
35  inline void initConstraintDynamics(
36  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
37  DataTpl<Scalar, Options, JointCollectionTpl> & data,
38  const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models);
39 
79  template<
80  typename Scalar,
81  int Options,
82  template<typename, int> class JointCollectionTpl,
83  typename ConfigVectorType,
84  typename TangentVectorType1,
85  typename TangentVectorType2,
86  class ConstraintModelAllocator,
87  class ConstraintDataAllocator>
88  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
89  inline const
90  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
91  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
92  DataTpl<Scalar, Options, JointCollectionTpl> & data,
93  const Eigen::MatrixBase<ConfigVectorType> & q,
94  const Eigen::MatrixBase<TangentVectorType1> & v,
95  const Eigen::MatrixBase<TangentVectorType2> & tau,
96  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
97  contact_models,
98  std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
99  ProximalSettingsTpl<Scalar> & settings);
100 
136  template<
137  typename Scalar,
138  int Options,
139  template<typename, int> class JointCollectionTpl,
140  typename ConfigVectorType,
141  typename TangentVectorType1,
142  typename TangentVectorType2,
143  class ConstraintModelAllocator,
144  class ConstraintDataAllocator>
145  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
146  inline const
147  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
148  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
149  DataTpl<Scalar, Options, JointCollectionTpl> & data,
150  const Eigen::MatrixBase<ConfigVectorType> & q,
151  const Eigen::MatrixBase<TangentVectorType1> & v,
152  const Eigen::MatrixBase<TangentVectorType2> & tau,
153  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
154  contact_models,
155  std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas)
156  {
157  ProximalSettingsTpl<Scalar> settings;
158  return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, settings);
159  }
160 
161  template<
162  typename Scalar,
163  int Options,
164  template<typename, int> class JointCollectionTpl,
165  typename ConfigVectorType,
166  typename TangentVectorType1,
167  typename TangentVectorType2,
168  class ModelAllocator,
169  class DataAllocator>
170  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
171  inline const
172  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & contactABA(
173  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
174  DataTpl<Scalar, Options, JointCollectionTpl> & data,
175  const Eigen::MatrixBase<ConfigVectorType> & q,
176  const Eigen::MatrixBase<TangentVectorType1> & v,
177  const Eigen::MatrixBase<TangentVectorType2> & tau,
178  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
179  std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data)
180  {
181  ProximalSettingsTpl<Scalar> settings = ProximalSettingsTpl<Scalar>();
182  return contactABA(
183  model, data, q.derived(), v.derived(), tau.derived(), contact_models, contact_data, settings);
184  }
185 
186 } // namespace pinocchio
187 
188 #include "pinocchio/algorithm/constrained-dynamics.hxx"
189 
190 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
191  #include "pinocchio/algorithm/constrained-dynamics.txx"
192 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
193 
194 #endif // ifndef __pinocchio_algorithm_constrained_dynamics_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::PINOCCHIO_UNSUPPORTED_MESSAGE
struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") ContactCholeskyDecompositionTpl
Contact Cholesky decomposition structure. This structure allows to compute in a efficient and parsimo...
Definition: contact-cholesky.hpp:55
pinocchio::constraintDynamics
const 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)
Computes the forward dynamics with contact constraints according to a given list of contact informati...
pinocchio::initConstraintDynamics
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
pinocchio::ProximalSettingsTpl
Structure containing all the settings parameters for the proximal algorithms.
Definition: proximal.hpp:25