pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
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
12namespace 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::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::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::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::ProximalSettingsTpl
Structure containing all the settings parameters for the proximal algorithms.
Definition proximal.hpp:25