5 #ifndef __pinocchio_algorithm_contact_inverse_dynamics__hpp__
6 #define __pinocchio_algorithm_contact_inverse_dynamics__hpp__
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"
17 #include <boost/optional.hpp>
46 template<
typename,
int>
47 class JointCollectionTpl,
49 class ConstraintModelAllocator,
50 class ConstraintDataAllocator,
51 class CoulombFrictionConelAllocator,
53 typename VectorLikeGamma,
54 typename VectorLikeImp>
56 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
58 const
ModelTpl<Scalar, Options, JointCollectionTpl> & model,
59 DataTpl<Scalar, Options, JointCollectionTpl> & data,
60 const Eigen::MatrixBase<VectorLikeC> & c_ref,
65 const Eigen::MatrixBase<VectorLikeR> & R,
66 const Eigen::MatrixBase<VectorLikeGamma> & constraint_correction,
68 const boost::optional<VectorLikeImp> & impulse_guess = boost::none)
71 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
72 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
73 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
75 int problem_size = R.size();
76 int n_contacts = (int)problem_size / 3;
77 PINOCCHIO_CHECK_ARGUMENT_SIZE(constraint_correction.size(), problem_size);
78 PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_models.size(), n_contacts);
79 PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_datas.size(), n_contacts);
80 PINOCCHIO_CHECK_INPUT_ARGUMENT(
81 check_expression_if_real<Scalar>(settings.mu > Scalar(0)),
"mu has to be strictly positive");
82 MatrixXs J = MatrixXs::Zero(problem_size, model.nv);
84 VectorXs c_ref_cor, desaxce_correction, R_prox, impulse_c_prev, dimpulse_c;
85 R_prox = R + VectorXs::Constant(problem_size, settings.mu);
86 c_ref_cor = c_ref + constraint_correction;
89 data.impulse_c = impulse_guess.get();
90 PINOCCHIO_CHECK_ARGUMENT_SIZE(data.impulse_c.size(), problem_size);
94 data.impulse_c.setZero();
96 Scalar impulse_c_prev_norm_inf = data.impulse_c.template lpNorm<Eigen::Infinity>();
97 Scalar complementarity, dual_feasibility;
98 bool abs_prec_reached =
false, rel_prec_reached =
false;
99 const size_t nc = cones.size();
101 for (; settings.iter <= settings.max_iter; ++settings.iter)
103 impulse_c_prev = data.impulse_c;
104 for (
size_t cone_id = 0; cone_id < nc; ++cone_id)
106 const Eigen::DenseIndex row_id = 3 * cone_id;
107 const auto & cone = cones[cone_id];
108 auto impulse_segment = data.impulse_c.template segment<3>(row_id);
109 auto impulse_prev_segment = impulse_c_prev.template segment<3>(row_id);
110 auto R_prox_segment = R_prox.template segment<3>(row_id);
113 auto c_ref_segment = c_ref.template segment<3>(row_id);
114 Vector3 desaxce_segment = cone.computeNormalCorrection(
116 + (R.template segment<3>(row_id).array() * impulse_segment.array()).matrix());
118 -((c_ref_segment + desaxce_segment - settings.mu * impulse_prev_segment).array()
119 / R_prox_segment.array())
121 impulse_segment = cone.weightedProject(impulse_segment, R_prox_segment);
123 dimpulse_c = data.impulse_c - impulse_c_prev;
124 settings.relative_residual = dimpulse_c.template lpNorm<Eigen::Infinity>();
134 const Scalar impulse_c_norm_inf = data.impulse_c.template lpNorm<Eigen::Infinity>();
135 if (check_expression_if_real<Scalar, false>(
136 settings.relative_residual
137 <= settings.relative_accuracy * math::max(impulse_c_norm_inf, impulse_c_prev_norm_inf)))
138 rel_prec_reached =
true;
140 rel_prec_reached =
false;
142 if (abs_prec_reached || rel_prec_reached)
145 impulse_c_prev_norm_inf = impulse_c_norm_inf;
147 return data.impulse_c;
179 template<
typename,
int>
180 class JointCollectionTpl,
181 typename ConfigVectorType,
182 typename TangentVectorType1,
183 typename TangentVectorType2,
184 class ConstraintModelAllocator,
185 class ConstraintDataAllocator,
186 class CoulombFrictionConelAllocator,
187 typename VectorLikeR,
188 typename VectorLikeGamma,
189 typename VectorLikeLam>
191 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
193 const
ModelTpl<Scalar, Options, JointCollectionTpl> & model,
194 DataTpl<Scalar, Options, JointCollectionTpl> & data,
195 const Eigen::MatrixBase<ConfigVectorType> & q,
196 const Eigen::MatrixBase<TangentVectorType1> & v,
197 const Eigen::MatrixBase<TangentVectorType2> & a,
203 const Eigen::MatrixBase<VectorLikeR> & R,
204 const Eigen::MatrixBase<VectorLikeGamma> & constraint_correction,
206 const boost::optional<VectorLikeLam> & lambda_guess = boost::none)
209 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
210 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
214 int problem_size = R.size();
215 int n_contacts = (int)problem_size / 3;
216 MatrixXs J = MatrixXs::Zero(problem_size, model.nv);
218 VectorXs v_ref, c_ref, tau_c;
220 c_ref.noalias() = J * v_ref;
221 boost::optional<VectorXs> impulse_guess = boost::none;
224 data.impulse_c = lambda_guess.get();
225 data.impulse_c *= dt;
226 impulse_guess = boost::make_optional(data.impulse_c);
229 model, data, c_ref, contact_models, contact_datas, cones, R, constraint_correction, settings,
231 data.lambda_c = data.impulse_c / dt;
232 container::aligned_vector<Force> fext(model.njoints);
233 for (
int i = 0; i < model.njoints; i++)
235 fext[i] = Force::Zero();
237 for (
int i = 0; i < n_contacts; i++)
239 const auto & cmodel = contact_models[i];
240 const Eigen::DenseIndex row_id = 3 * i;
241 auto lambda_segment = data.lambda_c.template segment<3>(row_id);
242 typename RigidConstraintData::Matrix6 actInv_transpose1 =
243 cmodel.joint1_placement.toActionMatrixInverse();
244 actInv_transpose1.transposeInPlace();
245 fext[cmodel.joint1_id] +=
Force(actInv_transpose1.template leftCols<3>() * lambda_segment);
246 typename RigidConstraintData::Matrix6 actInv_transpose2 =
247 cmodel.joint2_placement.toActionMatrixInverse();
248 actInv_transpose2.transposeInPlace();
249 fext[cmodel.joint2_id] +=
Force(actInv_transpose2.template leftCols<3>() * lambda_segment);
251 rnea(model, data, q, v, a, fext);
Main pinocchio namespace.
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...
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 & 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 & 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.
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...
Structure containing all the settings parameters for the proximal algorithms.