Line |
Branch |
Exec |
Source |
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 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
|
14 |
|
|
template< |
15 |
|
|
typename Scalar, |
16 |
|
|
int Options, |
17 |
|
|
template<typename, int> |
18 |
|
|
class JointCollectionTpl, |
19 |
|
|
class ConstraintModelAllocator, |
20 |
|
|
class ConstraintDataAllocator, |
21 |
|
|
typename MatrixType1, |
22 |
|
|
typename MatrixType2, |
23 |
|
|
typename MatrixType3, |
24 |
|
|
typename MatrixType4, |
25 |
|
|
typename MatrixType5, |
26 |
|
|
typename MatrixType6> |
27 |
|
|
PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") |
28 |
|
|
inline void computeConstraintDynamicsDerivatives( |
29 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
30 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
31 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & |
32 |
|
|
contact_models, |
33 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data, |
34 |
|
|
const ProximalSettingsTpl<Scalar> & settings, |
35 |
|
|
const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq, |
36 |
|
|
const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv, |
37 |
|
|
const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau, |
38 |
|
|
const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq, |
39 |
|
|
const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv, |
40 |
|
|
const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau); |
41 |
|
|
|
42 |
|
|
template< |
43 |
|
|
typename Scalar, |
44 |
|
|
int Options, |
45 |
|
|
template<typename, int> |
46 |
|
|
class JointCollectionTpl, |
47 |
|
|
class ConstraintModelAllocator, |
48 |
|
|
class ConstraintDataAllocator, |
49 |
|
|
typename MatrixType1, |
50 |
|
|
typename MatrixType2, |
51 |
|
|
typename MatrixType3, |
52 |
|
|
typename MatrixType4, |
53 |
|
|
typename MatrixType5, |
54 |
|
|
typename MatrixType6> |
55 |
|
|
PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") |
56 |
|
✗ |
inline void computeConstraintDynamicsDerivatives( |
57 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
58 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
59 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & |
60 |
|
|
contact_models, |
61 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data, |
62 |
|
|
const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq, |
63 |
|
|
const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv, |
64 |
|
|
const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau, |
65 |
|
|
const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq, |
66 |
|
|
const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv, |
67 |
|
|
const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau) |
68 |
|
|
{ |
69 |
|
✗ |
ProximalSettingsTpl<Scalar> settings; |
70 |
|
✗ |
computeConstraintDynamicsDerivatives( |
71 |
|
✗ |
model, data, contact_models, contact_data, settings, ddq_partial_dq.const_cast_derived(), |
72 |
|
✗ |
ddq_partial_dv.const_cast_derived(), ddq_partial_dtau.const_cast_derived(), |
73 |
|
✗ |
lambda_partial_dq.const_cast_derived(), lambda_partial_dv.const_cast_derived(), |
74 |
|
✗ |
lambda_partial_dtau.const_cast_derived()); |
75 |
|
|
} |
76 |
|
|
|
77 |
|
|
template< |
78 |
|
|
typename Scalar, |
79 |
|
|
int Options, |
80 |
|
|
template<typename, int> |
81 |
|
|
class JointCollectionTpl, |
82 |
|
|
class ConstraintModelAllocator, |
83 |
|
|
class ConstraintDataAllocator> |
84 |
|
|
PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") |
85 |
|
21 |
inline void computeConstraintDynamicsDerivatives( |
86 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
87 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
88 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & |
89 |
|
|
contact_models, |
90 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data, |
91 |
|
|
const ProximalSettingsTpl<Scalar> & settings) |
92 |
|
|
{ |
93 |
|
21 |
computeConstraintDynamicsDerivatives( |
94 |
|
21 |
model, data, contact_models, contact_data, settings, data.ddq_dq, data.ddq_dv, data.ddq_dtau, |
95 |
|
21 |
data.dlambda_dq, data.dlambda_dv, data.dlambda_dtau); |
96 |
|
21 |
}; |
97 |
|
|
|
98 |
|
|
template< |
99 |
|
|
typename Scalar, |
100 |
|
|
int Options, |
101 |
|
|
template<typename, int> |
102 |
|
|
class JointCollectionTpl, |
103 |
|
|
class ConstraintModelAllocator, |
104 |
|
|
class ConstraintDataAllocator> |
105 |
|
|
PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") |
106 |
|
✗ |
inline void computeConstraintDynamicsDerivatives( |
107 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
108 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
109 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & |
110 |
|
|
contact_models, |
111 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data) |
112 |
|
|
{ |
113 |
|
✗ |
ProximalSettingsTpl<Scalar> settings; |
114 |
|
✗ |
computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data, settings); |
115 |
|
✗ |
}; |
116 |
|
|
|
117 |
|
|
} // namespace pinocchio |
118 |
|
|
|
119 |
|
|
#include "pinocchio/algorithm/constrained-dynamics-derivatives.hxx" |
120 |
|
|
|
121 |
|
|
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION |
122 |
|
|
#include "pinocchio/algorithm/constrained-dynamics-derivatives.txx" |
123 |
|
|
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION |
124 |
|
|
|
125 |
|
|
#endif // ifndef __pinocchio_algorithm_constrained_dynamics_derivatives_hpp__ |
126 |
|
|
|