pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
constrained-dynamics-derivatives.hpp
1//
2// Copyright (c) 2020-2022 CNRS INRIA
3//
4
5#ifndef __pinocchio_algorithm_constrained_dynamics_derivatives_hpp__
6#define __pinocchio_algorithm_constrained_dynamics_derivatives_hpp__
7
8#include "pinocchio/algorithm/contact-info.hpp"
9#include "pinocchio/algorithm/proximal.hpp"
10
11namespace pinocchio
12{
13
14 template<
15 typename Scalar,
16 int Options,
17 template<typename, int> class JointCollectionTpl,
18 class ConstraintModelAllocator,
19 class ConstraintDataAllocator,
20 typename MatrixType1,
21 typename MatrixType2,
22 typename MatrixType3,
23 typename MatrixType4,
24 typename MatrixType5,
25 typename MatrixType6>
26 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
27 inline void computeConstraintDynamicsDerivatives(
28 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
29 DataTpl<Scalar, Options, JointCollectionTpl> & data,
30 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
31 contact_models,
32 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
33 const ProximalSettingsTpl<Scalar> & settings,
34 const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq,
35 const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv,
36 const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau,
37 const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq,
38 const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv,
39 const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau);
40
41 template<
42 typename Scalar,
43 int Options,
44 template<typename, int> class JointCollectionTpl,
45 class ConstraintModelAllocator,
46 class ConstraintDataAllocator,
47 typename MatrixType1,
48 typename MatrixType2,
49 typename MatrixType3,
50 typename MatrixType4,
51 typename MatrixType5,
52 typename MatrixType6>
53 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
54 inline void computeConstraintDynamicsDerivatives(
55 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
56 DataTpl<Scalar, Options, JointCollectionTpl> & data,
57 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
58 contact_models,
59 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
60 const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq,
61 const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv,
62 const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau,
63 const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq,
64 const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv,
65 const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau)
66 {
67 ProximalSettingsTpl<Scalar> settings;
68 computeConstraintDynamicsDerivatives(
69 model, data, contact_models, contact_data, settings, ddq_partial_dq.const_cast_derived(),
70 ddq_partial_dv.const_cast_derived(), ddq_partial_dtau.const_cast_derived(),
71 lambda_partial_dq.const_cast_derived(), lambda_partial_dv.const_cast_derived(),
72 lambda_partial_dtau.const_cast_derived());
73 }
74
75 template<
76 typename Scalar,
77 int Options,
78 template<typename, int> class JointCollectionTpl,
79 class ConstraintModelAllocator,
80 class ConstraintDataAllocator>
81 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
82 inline void computeConstraintDynamicsDerivatives(
83 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
84 DataTpl<Scalar, Options, JointCollectionTpl> & data,
85 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
86 contact_models,
87 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
88 const ProximalSettingsTpl<Scalar> & settings)
89 {
90 computeConstraintDynamicsDerivatives(
91 model, data, contact_models, contact_data, settings, data.ddq_dq, data.ddq_dv, data.ddq_dtau,
92 data.dlambda_dq, data.dlambda_dv, data.dlambda_dtau);
93 };
94
95 template<
96 typename Scalar,
97 int Options,
98 template<typename, int> class JointCollectionTpl,
99 class ConstraintModelAllocator,
100 class ConstraintDataAllocator>
101 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
102 inline void computeConstraintDynamicsDerivatives(
103 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
104 DataTpl<Scalar, Options, JointCollectionTpl> & data,
105 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
106 contact_models,
107 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data)
108 {
109 ProximalSettingsTpl<Scalar> settings;
110 computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data, settings);
111 };
112
113} // namespace pinocchio
114
115#include "pinocchio/algorithm/constrained-dynamics-derivatives.hxx"
116
117#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
118 #include "pinocchio/algorithm/constrained-dynamics-derivatives.txx"
119#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
120
121#endif // ifndef __pinocchio_algorithm_constrained_dynamics_derivatives_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::ProximalSettingsTpl
Structure containing all the settings parameters for the proximal algorithms.
Definition proximal.hpp:25