pinocchio 4.1.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#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/fwd.hpp>
17#include <pinocchio/context.hpp>
18#include <pinocchio/config.hpp>
19#include <pinocchio/macros.hpp>
20#include <pinocchio/unsupported.hpp>
21
22#include "pinocchio/utils/check.hpp"
23
24#include "pinocchio/math.hpp"
25
26#include "pinocchio/spatial.hpp"
27
28#include "pinocchio/multibody.hpp"
29#include "pinocchio/multibody/joint.hpp"
30
31#include "pinocchio/constraints.hpp"
32#include "pinocchio/algorithm/proximal.hpp"
33#include "pinocchio/algorithm/check-model.hpp"
34#include "pinocchio/algorithm/frames.hpp"
35#include "pinocchio/algorithm/rnea-derivatives.hpp"
36#include "pinocchio/algorithm/frames-derivatives.hpp"
37#include "pinocchio/algorithm/kinematics-derivatives.hpp"
38#include "pinocchio/algorithm/constraint-cholesky.hpp"
39// IWYU pragma: end_keep
40
41namespace pinocchio
42{
43
44 template<
45 typename Scalar,
46 int Options,
47 template<typename, int> class JointCollectionTpl,
48 class ConstraintModelAllocator,
49 class ConstraintDataAllocator,
50 typename MatrixType1,
51 typename MatrixType2,
52 typename MatrixType3,
53 typename MatrixType4,
54 typename MatrixType5,
55 typename MatrixType6>
56 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
57 inline void computeConstraintDynamicsDerivatives(
58 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
59 DataTpl<Scalar, Options, JointCollectionTpl> & data,
60 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
61 contact_models,
62 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
63 const ProximalSettingsTpl<Scalar> & settings,
64 const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq,
65 const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv,
66 const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau,
67 const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq,
68 const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv,
69 const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau);
70
71 template<
72 typename Scalar,
73 int Options,
74 template<typename, int> class JointCollectionTpl,
75 class ConstraintModelAllocator,
76 class ConstraintDataAllocator,
77 typename MatrixType1,
78 typename MatrixType2,
79 typename MatrixType3,
80 typename MatrixType4,
81 typename MatrixType5,
82 typename MatrixType6>
83 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
84 inline void computeConstraintDynamicsDerivatives(
85 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
86 DataTpl<Scalar, Options, JointCollectionTpl> & data,
87 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
88 contact_models,
89 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
90 const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq,
91 const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv,
92 const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau,
93 const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq,
94 const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv,
95 const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau)
96 {
97 ProximalSettingsTpl<Scalar> settings;
98 computeConstraintDynamicsDerivatives(
99 model, data, contact_models, contact_data, settings, ddq_partial_dq.const_cast_derived(),
100 ddq_partial_dv.const_cast_derived(), ddq_partial_dtau.const_cast_derived(),
101 lambda_partial_dq.const_cast_derived(), lambda_partial_dv.const_cast_derived(),
102 lambda_partial_dtau.const_cast_derived());
103 }
104
105 template<
106 typename Scalar,
107 int Options,
108 template<typename, int> class JointCollectionTpl,
109 class ConstraintModelAllocator,
110 class ConstraintDataAllocator>
111 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
112 inline void computeConstraintDynamicsDerivatives(
113 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
114 DataTpl<Scalar, Options, JointCollectionTpl> & data,
115 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
116 contact_models,
117 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
118 const ProximalSettingsTpl<Scalar> & settings)
119 {
120 computeConstraintDynamicsDerivatives(
121 model, data, contact_models, contact_data, settings, data.ddq_dq, data.ddq_dv, data.ddq_dtau,
122 data.dlambda_dq, data.dlambda_dv, data.dlambda_dtau);
123 };
124
125 template<
126 typename Scalar,
127 int Options,
128 template<typename, int> class JointCollectionTpl,
129 class ConstraintModelAllocator,
130 class ConstraintDataAllocator>
131 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
132 inline void computeConstraintDynamicsDerivatives(
133 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
134 DataTpl<Scalar, Options, JointCollectionTpl> & data,
135 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
136 contact_models,
137 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data)
138 {
139 ProximalSettingsTpl<Scalar> settings;
140 computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data, settings);
141 };
142
143} // namespace pinocchio
144
145// IWYU pragma: begin_exports
146#include "pinocchio/src/algorithm/constrained-dynamics-derivatives.hxx"
147// IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11