$darkmode
pinocchio 4.0.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 #pragma once
6 
7 // IWYU pragma: begin_keep
8 #include <cassert>
9 #include <cstddef>
10 #include <vector>
11 
12 #include <Eigen/Core>
13 
14 #include <boost/fusion/container/vector.hpp>
15 
16 #include "pinocchio/config.hpp"
17 #include "pinocchio/context.hpp"
18 #include "pinocchio/unsupported.hpp"
19 #include "pinocchio/macros.hpp"
20 
21 #include "pinocchio/utils/check.hpp"
22 
23 #include "pinocchio/math.hpp"
24 
25 #include "pinocchio/spatial.hpp"
26 
27 #include "pinocchio/multibody.hpp"
28 #include "pinocchio/multibody/joint.hpp"
29 
30 #include "pinocchio/algorithm/fwd.hpp"
31 #include "pinocchio/algorithm/proximal.hpp"
32 #include "pinocchio/algorithm/check-model.hpp"
33 #include "pinocchio/constraints.hpp"
34 #include "pinocchio/algorithm/crba.hpp"
35 #include "pinocchio/algorithm/cholesky.hpp"
36 #include "pinocchio/algorithm/constraint-cholesky.hpp"
37 // IWYU pragma: end_keep
38 
39 namespace pinocchio
40 {
41 
57  template<
58  typename Scalar,
59  int Options,
60  template<typename, int> class JointCollectionTpl,
61  class ConstraintModel,
62  class ConstraintModelAllocator,
63  class ConstraintData,
64  class ConstraintDataAllocator>
65  inline void initConstraintDynamics(
66  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
67  DataTpl<Scalar, Options, JointCollectionTpl> & data,
68  const std::vector<ConstraintModel, ConstraintModelAllocator> & contact_models,
69  const std::vector<ConstraintData, ConstraintDataAllocator> & contact_datas);
70 
77 
111  template<
112  typename Scalar,
113  int Options,
114  template<typename, int> class JointCollectionTpl,
115  typename ConfigVectorType,
116  typename TangentVectorType1,
117  typename TangentVectorType2,
118  class ConstraintModelAllocator,
119  class ConstraintDataAllocator>
120  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
121  inline const
122  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
123  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
124  DataTpl<Scalar, Options, JointCollectionTpl> & data,
125  const Eigen::MatrixBase<ConfigVectorType> & q,
126  const Eigen::MatrixBase<TangentVectorType1> & v,
127  const Eigen::MatrixBase<TangentVectorType2> & tau,
128  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
129  contact_models,
130  std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
131  ProximalSettingsTpl<Scalar> & settings);
132 
139 
170  template<
171  typename Scalar,
172  int Options,
173  template<typename, int> class JointCollectionTpl,
174  typename ConfigVectorType,
175  typename TangentVectorType1,
176  typename TangentVectorType2,
177  class ConstraintModelAllocator,
178  class ConstraintDataAllocator>
179  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
180  inline const
181  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
182  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
183  DataTpl<Scalar, Options, JointCollectionTpl> & data,
184  const Eigen::MatrixBase<ConfigVectorType> & q,
185  const Eigen::MatrixBase<TangentVectorType1> & v,
186  const Eigen::MatrixBase<TangentVectorType2> & tau,
187  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
188  contact_models,
189  std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas)
190  {
191  ProximalSettingsTpl<Scalar> settings;
192  return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, settings);
193  }
194 
195  template<
196  typename Scalar,
197  int Options,
198  template<typename, int> class JointCollectionTpl,
199  typename ConfigVectorType,
200  typename TangentVectorType1,
201  typename TangentVectorType2,
202  class ModelAllocator,
203  class DataAllocator>
204  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
205  inline const
206  typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & contactABA(
207  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
208  DataTpl<Scalar, Options, JointCollectionTpl> & data,
209  const Eigen::MatrixBase<ConfigVectorType> & q,
210  const Eigen::MatrixBase<TangentVectorType1> & v,
211  const Eigen::MatrixBase<TangentVectorType2> & tau,
212  const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
213  std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data)
214  {
215  ProximalSettingsTpl<Scalar> settings = ProximalSettingsTpl<Scalar>();
216  return contactABA(
217  model, data, q.derived(), v.derived(), tau.derived(), contact_models, contact_data, settings);
218  }
219 
220 } // namespace pinocchio
221 
222 // IWYU pragma: begin_exports
223 #include "pinocchio/src/algorithm/constrained-dynamics.hxx"
224 // IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
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< ConstraintModel, ConstraintModelAllocator > &contact_models, const std::vector< ConstraintData, ConstraintDataAllocator > &contact_datas)
Init the forward dynamics data according to the contact information contained in contact_models.