pinocchio  3.2.0
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>
33  class JointCollectionTpl,
34  class Allocator>
35  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
36  inline void initConstraintDynamics(
37  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
38  DataTpl<Scalar, Options, JointCollectionTpl> & data,
39  const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models);
40 
80  template<
81  typename Scalar,
82  int Options,
83  template<typename, int>
84  class JointCollectionTpl,
85  typename ConfigVectorType,
86  typename TangentVectorType1,
87  typename TangentVectorType2,
88  class ConstraintModelAllocator,
89  class ConstraintDataAllocator>
90  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
91  inline const
92  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
93  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
94  DataTpl<Scalar, Options, JointCollectionTpl> & data,
95  const Eigen::MatrixBase<ConfigVectorType> & q,
96  const Eigen::MatrixBase<TangentVectorType1> & v,
97  const Eigen::MatrixBase<TangentVectorType2> & tau,
98  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
99  contact_models,
100  std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
101  ProximalSettingsTpl<Scalar> & settings);
102 
138  template<
139  typename Scalar,
140  int Options,
141  template<typename, int>
142  class JointCollectionTpl,
143  typename ConfigVectorType,
144  typename TangentVectorType1,
145  typename TangentVectorType2,
146  class ConstraintModelAllocator,
147  class ConstraintDataAllocator>
148  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
149  inline const
150  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
151  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
152  DataTpl<Scalar, Options, JointCollectionTpl> & data,
153  const Eigen::MatrixBase<ConfigVectorType> & q,
154  const Eigen::MatrixBase<TangentVectorType1> & v,
155  const Eigen::MatrixBase<TangentVectorType2> & tau,
156  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
157  contact_models,
158  std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas)
159  {
160  ProximalSettingsTpl<Scalar> settings;
161  return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, settings);
162  }
163 
164  template<
165  typename Scalar,
166  int Options,
167  template<typename, int>
168  class JointCollectionTpl,
169  typename ConfigVectorType,
170  typename TangentVectorType1,
171  typename TangentVectorType2,
172  class ModelAllocator,
173  class DataAllocator>
174  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
175  inline const
176  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & contactABA(
177  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
178  DataTpl<Scalar, Options, JointCollectionTpl> & data,
179  const Eigen::MatrixBase<ConfigVectorType> & q,
180  const Eigen::MatrixBase<TangentVectorType1> & v,
181  const Eigen::MatrixBase<TangentVectorType2> & tau,
182  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
183  std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data)
184  {
185  ProximalSettingsTpl<Scalar> settings = ProximalSettingsTpl<Scalar>();
186  return contactABA(
187  model, data, q.derived(), v.derived(), tau.derived(), contact_models, contact_data, settings);
188  }
189 
190 } // namespace pinocchio
191 
192 #include "pinocchio/algorithm/constrained-dynamics.hxx"
193 
194 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
195  #include "pinocchio/algorithm/constrained-dynamics.txx"
196 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
197 
198 #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