5 #ifndef __pinocchio_algorithm_contact_inverse_dynamics__hpp__
6 #define __pinocchio_algorithm_contact_inverse_dynamics__hpp__
12 #include <boost/optional/optional.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>
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;
81 check_expression_if_real<Scalar>(settings.mu >
Scalar(0)),
"mu has to be strictly positive");
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();
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>::
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;
220 c_ref.noalias() =
J *
v_ref;
221 boost::optional<VectorXs> impulse_guess = boost::none;
224 data.impulse_c = lambda_guess.get();
226 impulse_guess = boost::make_optional(
data.impulse_c);
233 for (
int i = 0;
i <
model.njoints;
i++)
237 for (
int i = 0;
i < n_contacts;
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);
261 #endif // ifndef __pinocchio_algorithm_contact_inverse_dynamics_hpp__