pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
contact-inverse-dynamics.hpp
1//
2// Copyright (c) 2024 INRIA
3//
4
5#ifndef __pinocchio_algorithm_contact_inverse_dynamics__hpp__
6#define __pinocchio_algorithm_contact_inverse_dynamics__hpp__
7
8#include "pinocchio/multibody/model.hpp"
9#include "pinocchio/multibody/data.hpp"
10#include "pinocchio/algorithm/constraints/coulomb-friction-cone.hpp"
11#include "pinocchio/algorithm/rnea.hpp"
12#include <boost/optional/optional.hpp>
13#include <pinocchio/algorithm/contact-cholesky.hpp>
14#include <pinocchio/algorithm/contact-jacobian.hpp>
15#include "pinocchio/algorithm/proximal.hpp"
16
17#include <boost/optional.hpp>
18
19namespace pinocchio
20{
21
43 template<
44 typename Scalar,
45 int Options,
46 template<typename, int> class JointCollectionTpl,
47 typename VectorLikeC,
48 class ConstraintModelAllocator,
49 class ConstraintDataAllocator,
50 class CoulombFrictionConelAllocator,
51 typename VectorLikeR,
52 typename VectorLikeGamma,
53 typename VectorLikeImp>
54 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
55 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
56 TangentVectorType & computeContactImpulses(
57 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
58 DataTpl<Scalar, Options, JointCollectionTpl> & data,
60 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
62 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
64 const Eigen::MatrixBase<VectorLikeR> & R,
68 {
69
70 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
71 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
72 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
73
74 int problem_size = R.size();
75 int n_contacts = (int)problem_size / 3;
76 PINOCCHIO_CHECK_ARGUMENT_SIZE(constraint_correction.size(), problem_size);
77 PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_models.size(), n_contacts);
78 PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_datas.size(), n_contacts);
79 PINOCCHIO_CHECK_INPUT_ARGUMENT(
80 check_expression_if_real<Scalar>(settings.mu > Scalar(0)), "mu has to be strictly positive");
81 MatrixXs J = MatrixXs::Zero(problem_size, model.nv); // TODO: malloc
83 VectorXs c_ref_cor, desaxce_correction, R_prox, impulse_c_prev, dimpulse_c; // TODO: malloc
84 R_prox = R + VectorXs::Constant(problem_size, settings.mu);
86 if (impulse_guess)
87 {
88 data.impulse_c = impulse_guess.get();
89 PINOCCHIO_CHECK_ARGUMENT_SIZE(data.impulse_c.size(), problem_size);
90 }
91 else
92 {
93 data.impulse_c.setZero();
94 }
95 Scalar impulse_c_prev_norm_inf = data.impulse_c.template lpNorm<Eigen::Infinity>();
97 bool abs_prec_reached = false, rel_prec_reached = false;
98 const size_t nc = cones.size(); // num constraints
99 settings.iter = 1;
100 for (; settings.iter <= settings.max_iter; ++settings.iter)
101 {
102 impulse_c_prev = data.impulse_c;
103 for (size_t cone_id = 0; cone_id < nc; ++cone_id)
104 {
105 const Eigen::DenseIndex row_id = 3 * cone_id;
106 const auto & cone = cones[cone_id];
107 auto impulse_segment = data.impulse_c.template segment<3>(row_id);
109 auto R_prox_segment = R_prox.template segment<3>(row_id);
110 // Vector3 desaxce_segment;
111 // auto desaxce_segment = desaxce_correction.template segment<3>(row_id);
112 auto c_ref_segment = c_ref.template segment<3>(row_id);
113 Vector3 desaxce_segment = cone.computeNormalCorrection(
115 + (R.template segment<3>(row_id).array() * impulse_segment.array()).matrix());
118 / R_prox_segment.array())
119 .matrix();
121 }
122 dimpulse_c = data.impulse_c - impulse_c_prev;
124
125 // if( check_expression_if_real<Scalar,false>(complementarity <= this->absolute_precision)
126 // && check_expression_if_real<Scalar,false>(dual_feasibility <= this->absolute_precision)
127 // && check_expression_if_real<Scalar,false>(primal_feasibility <=
128 // this->absolute_precision))
129 // abs_prec_reached = true;
130 // else
131 // abs_prec_reached = false;
132
133 const Scalar impulse_c_norm_inf = data.impulse_c.template lpNorm<Eigen::Infinity>();
136 <= settings.relative_accuracy * math::max(impulse_c_norm_inf, impulse_c_prev_norm_inf)))
137 rel_prec_reached = true;
138 else
139 rel_prec_reached = false;
140
142 break;
143
145 }
146 return data.impulse_c;
147 }
148
175 template<
176 typename Scalar,
177 int Options,
178 template<typename, int> class JointCollectionTpl,
179 typename ConfigVectorType,
180 typename TangentVectorType1,
181 typename TangentVectorType2,
182 class ConstraintModelAllocator,
183 class ConstraintDataAllocator,
184 class CoulombFrictionConelAllocator,
185 typename VectorLikeR,
186 typename VectorLikeGamma,
187 typename VectorLikeLam>
188 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
189 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
190 TangentVectorType & contactInverseDynamics(
191 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
192 DataTpl<Scalar, Options, JointCollectionTpl> & data,
193 const Eigen::MatrixBase<ConfigVectorType> & q,
196 Scalar dt,
197 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
199 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
201 const Eigen::MatrixBase<VectorLikeR> & R,
204 const boost::optional<VectorLikeLam> & lambda_guess = boost::none)
205 {
206
207 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
208 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
209 typedef ForceTpl<Scalar, Options> Force;
211
212 int problem_size = R.size();
213 int n_contacts = (int)problem_size / 3;
214 MatrixXs J = MatrixXs::Zero(problem_size, model.nv); // TODO: malloc
216 VectorXs v_ref, c_ref, tau_c;
217 v_ref = v + dt * a;
218 c_ref.noalias() = J * v_ref; // TODO should rather use the displacement
219 boost::optional<VectorXs> impulse_guess = boost::none;
220 if (lambda_guess)
221 {
222 data.impulse_c = lambda_guess.get();
223 data.impulse_c *= dt;
224 impulse_guess = boost::make_optional(data.impulse_c);
225 }
229 data.lambda_c = data.impulse_c / dt;
230 container::aligned_vector<Force> fext(model.njoints);
231 for (int i = 0; i < model.njoints; i++)
232 {
233 fext[i] = Force::Zero();
234 }
235 for (int i = 0; i < n_contacts; i++)
236 {
237 const auto & cmodel = contact_models[i];
238 const Eigen::DenseIndex row_id = 3 * i;
239 auto lambda_segment = data.lambda_c.template segment<3>(row_id);
240 typename RigidConstraintData::Matrix6 actInv_transpose1 =
241 cmodel.joint1_placement.toActionMatrixInverse();
242 actInv_transpose1.transposeInPlace();
243 fext[cmodel.joint1_id] += Force(actInv_transpose1.template leftCols<3>() * lambda_segment);
244 typename RigidConstraintData::Matrix6 actInv_transpose2 =
245 cmodel.joint2_placement.toActionMatrixInverse();
246 actInv_transpose2.transposeInPlace();
247 fext[cmodel.joint2_id] += Force(actInv_transpose2.template leftCols<3>() * lambda_segment);
248 }
249 rnea(model, data, q, v, a, fext);
250 return data.tau;
251 }
252
253} // namespace pinocchio
254
255// #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
256// #include "pinocchio/algorithm/contact-inverse-dynamics.txx"
257// #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
258
259#endif // ifndef __pinocchio_algorithm_contact_inverse_dynamics_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & contactInverseDynamics(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 > &a, Scalar dt, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, const std::vector< CoulombFrictionConeTpl< Scalar >, CoulombFrictionConelAllocator > &cones, const Eigen::MatrixBase< VectorLikeR > &R, const Eigen::MatrixBase< VectorLikeGamma > &constraint_correction, ProximalSettingsTpl< Scalar > &settings, const boost::optional< VectorLikeLam > &lambda_guess=boost::none)
The Contact Inverse Dynamics algorithm. It computes the inverse dynamics in the presence of contacts,...
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
void getConstraintsJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintDataAllocator > &constraint_model, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &constraint_data, const Eigen::MatrixBase< DynamicMatrixLike > &J)
Computes the kinematic Jacobian associatied to a given set of constraint models.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeContactImpulses(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< VectorLikeC > &c_ref, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, const std::vector< CoulombFrictionConeTpl< Scalar >, CoulombFrictionConelAllocator > &cones, const Eigen::MatrixBase< VectorLikeR > &R, const Eigen::MatrixBase< VectorLikeGamma > &constraint_correction, ProximalSettingsTpl< Scalar > &settings, const boost::optional< VectorLikeImp > &impulse_guess=boost::none)
Compute the contact impulses given a target velocity of contact points.
Scalar relative_residual
Relative convergence residual value.
Structure containing all the settings parameters for the proximal algorithms.
Definition proximal.hpp:25