GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2025, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/constraints/residual.hpp"
10			#include "crocoddyl/multibody/residuals/contact-force.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::
16			DifferentialActionModelContactInvDynamicsTpl(
17			std::shared_ptr<StateMultibody> state,
18			std::shared_ptr<ActuationModelAbstract> actuation,
19			std::shared_ptr<ContactModelMultiple> contacts,
20			std::shared_ptr<CostModelSum> costs)
21		✗	: Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
22		✗	0, state->get_nv() - actuation->get_nu() + contacts->get_nc_total()),
23		✗	actuation_(actuation),
24		✗	contacts_(contacts),
25		✗	costs_(costs),
26		✗	constraints_(std::make_shared<ConstraintModelManager>(
27		✗	state, state->get_nv() + contacts->get_nc_total())),
28		✗	pinocchio_(state->get_pinocchio().get()) {
29		✗	init(state);
30		✗	}
31
32			template <typename Scalar>
33		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::
34			DifferentialActionModelContactInvDynamicsTpl(
35			std::shared_ptr<StateMultibody> state,
36			std::shared_ptr<ActuationModelAbstract> actuation,
37			std::shared_ptr<ContactModelMultiple> contacts,
38			std::shared_ptr<CostModelSum> costs,
39			std::shared_ptr<ConstraintModelManager> constraints)
40		✗	: Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
41			constraints->get_ng(),
42		✗	state->get_nv() - actuation->get_nu() + contacts->get_nc_total() +
43		✗	constraints->get_nh(),
44			constraints->get_ng_T(), constraints->get_nh_T()),
45		✗	actuation_(actuation),
46		✗	contacts_(contacts),
47		✗	costs_(costs),
48		✗	constraints_(constraints),
49		✗	pinocchio_(state->get_pinocchio().get()) {
50		✗	init(state);
51		✗	}
52
53			template <typename Scalar>
54		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::init(
55			const std::shared_ptr<StateMultibody>& state) {
56		✗	if (contacts_->get_nu() != nu_) {
57		✗	throw_pretty(
58			"Invalid argument: "
59			<< "Contacts doesn't have the same control dimension (it should be " +
60			std::to_string(nu_) + ")");
61			}
62		✗	if (costs_->get_nu() != nu_) {
63		✗	throw_pretty(
64			"Invalid argument: "
65			<< "Costs doesn't have the same control dimension (it should be " +
66			std::to_string(nu_) + ")");
67			}
68		✗	const std::size_t nu = actuation_->get_nu();
69		✗	const std::size_t nc = contacts_->get_nc_total();
70		✗	VectorXs lb =
71		✗	VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
72		✗	VectorXs ub =
73		✗	VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
74		✗	Base::set_u_lb(lb);
75		✗	Base::set_u_ub(ub);
76		✗	contacts_->setComputeAllContacts(true);
77
78		✗	if (state_->get_nv() - actuation_->get_nu() > 0) {
79		✗	constraints_->addConstraint(
80			"tau", std::make_shared<ConstraintModelResidualTpl<Scalar>>(
81		✗	state_,
82			std::make_shared<
83			typename DifferentialActionModelContactInvDynamicsTpl<
84			Scalar>::ResidualModelActuation>(state, nu, nc),
85		✗	false));
86			}
87		✗	if (contacts_->get_nc_total() != 0) {
88		✗	typename ContactModelMultiple::ContactModelContainer contact_list;
89		✗	contact_list = contacts_->get_contacts();
90		✗	typename ContactModelMultiple::ContactModelContainer::iterator it_m, end_m;
91		✗	for (it_m = contact_list.begin(), end_m = contact_list.end(); it_m != end_m;
92		✗	++it_m) {
93		✗	const std::shared_ptr<ContactItem>& contact = it_m->second;
94		✗	const std::string name = contact->name;
95		✗	const pinocchio::FrameIndex id = contact->contact->get_id();
96		✗	const std::size_t nc_i = contact->contact->get_nc();
97		✗	const bool active = contact->active;
98		✗	constraints_->addConstraint(
99			name + "_acc",
100			std::make_shared<ConstraintModelResidualTpl<Scalar>>(
101		✗	state_,
102			std::make_shared<
103			typename DifferentialActionModelContactInvDynamicsTpl<
104			Scalar>::ResidualModelContact>(state, id, nc_i, nc),
105		✗	false),
106			active);
107		✗	constraints_->addConstraint(
108			name + "_force",
109			std::make_shared<ConstraintModelResidualTpl<Scalar>>(
110		✗	state_,
111			std::make_shared<ResidualModelContactForceTpl<Scalar>>(
112		✗	state, id, pinocchio::ForceTpl<Scalar>::Zero(), nc_i, nu_,
113		✗	false),
114		✗	false),
115		✗	!active);
116			}
117		✗	}
118		✗	}
119
120			template <typename Scalar>
121		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calc(
122			const std::shared_ptr<DifferentialActionDataAbstract>& data,
123			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
124		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
125		✗	throw_pretty(
126			"Invalid argument: " << "x has wrong dimension (it should be " +
127			std::to_string(state_->get_nx()) + ")");
128			}
129		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
130		✗	throw_pretty(
131			"Invalid argument: " << "u has wrong dimension (it should be " +
132			std::to_string(nu_) + ")");
133			}
134		✗	Data* d = static_cast<Data*>(data.get());
135		✗	const std::size_t nv = state_->get_nv();
136		✗	const std::size_t nc = contacts_->get_nc_total();
137			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
138		✗	x.head(state_->get_nq());
139			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
140		✗	x.tail(nv);
141			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
142		✗	u.head(nv);
143			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
144		✗	f_ext = u.tail(nc);
145
146		✗	d->xout = a;
147		✗	pinocchio::forwardKinematics(*pinocchio_, d->pinocchio, q, v, a);
148		✗	pinocchio::computeJointJacobians(*pinocchio_, d->pinocchio);
149		✗	contacts_->calc(d->multibody.contacts, x);
150		✗	contacts_->updateForce(d->multibody.contacts, f_ext);
151		✗	pinocchio::rnea(*pinocchio_, d->pinocchio, q, v, a,
152		✗	d->multibody.contacts->fext);
153		✗	pinocchio::updateGlobalPlacements(*pinocchio_, d->pinocchio);
154		✗	pinocchio::centerOfMass(*pinocchio_, d->pinocchio, q, v, a);
155		✗	actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
156		✗	d->multibody.joint->a = a;
157		✗	d->multibody.joint->tau = d->multibody.actuation->u;
158		✗	actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
159		✗	costs_->calc(d->costs, x, u);
160		✗	d->cost = d->costs->cost;
161		✗	for (std::string name : contacts_->get_active_set()) {
162		✗	constraints_->changeConstraintStatus(name + "_acc", true);
163		✗	constraints_->changeConstraintStatus(name + "_force", false);
164			}
165		✗	for (std::string name : contacts_->get_inactive_set()) {
166		✗	constraints_->changeConstraintStatus(name + "_acc", false);
167		✗	constraints_->changeConstraintStatus(name + "_force", true);
168			}
169		✗	d->constraints->resize(this, d);
170		✗	constraints_->calc(d->constraints, x, u);
171		✗	}
172
173			template <typename Scalar>
174		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calc(
175			const std::shared_ptr<DifferentialActionDataAbstract>& data,
176			const Eigen::Ref<const VectorXs>& x) {
177		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
178		✗	throw_pretty(
179			"Invalid argument: " << "x has wrong dimension (it should be " +
180			std::to_string(state_->get_nx()) + ")");
181			}
182
183		✗	Data* d = static_cast<Data*>(data.get());
184			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
185		✗	x.head(state_->get_nq());
186			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
187		✗	x.tail(state_->get_nv());
188
189		✗	pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
190		✗	pinocchio::computeCentroidalMomentum(*pinocchio_, d->pinocchio);
191		✗	costs_->calc(d->costs, x);
192		✗	d->cost = d->costs->cost;
193		✗	d->constraints->resize(this, d, false);
194		✗	constraints_->calc(d->constraints, x);
195		✗	}
196
197			template <typename Scalar>
198		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calcDiff(
199			const std::shared_ptr<DifferentialActionDataAbstract>& data,
200			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
201		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
202		✗	throw_pretty(
203			"Invalid argument: " << "x has wrong dimension (it should be " +
204			std::to_string(state_->get_nx()) + ")");
205			}
206		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
207		✗	throw_pretty(
208			"Invalid argument: " << "u has wrong dimension (it should be " +
209			std::to_string(nu_) + ")");
210			}
211		✗	Data* d = static_cast<Data*>(data.get());
212		✗	const std::size_t nv = state_->get_nv();
213		✗	const std::size_t nc = contacts_->get_nc_total();
214			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
215		✗	x.head(state_->get_nq());
216			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
217		✗	x.tail(nv);
218			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
219		✗	u.head(nv);
220
221		✗	pinocchio::computeRNEADerivatives(*pinocchio_, d->pinocchio, q, v, a,
222		✗	d->multibody.contacts->fext);
223		✗	contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
224		✗	d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
225		✗	d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
226		✗	.transpose();
227		✗	pinocchio::jacobianCenterOfMass(*pinocchio_, d->pinocchio, false);
228		✗	actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
229		✗	actuation_->torqueTransform(d->multibody.actuation, x,
230		✗	d->multibody.joint->tau);
231		✗	d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
232		✗	d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
233		✗	d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
234		✗	d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
235		✗	d->multibody.joint->dtau_du.leftCols(nv).noalias() =
236		✗	d->multibody.actuation->Mtau * d->pinocchio.M;
237		✗	d->multibody.joint->dtau_du.rightCols(nc).noalias() =
238		✗	-d->multibody.actuation->Mtau *
239		✗	d->multibody.contacts->Jc.topRows(nc).transpose();
240		✗	contacts_->calcDiff(d->multibody.contacts, x);
241		✗	costs_->calcDiff(d->costs, x, u);
242		✗	for (std::string name : contacts_->get_active_set()) {
243		✗	constraints_->changeConstraintStatus(name + "_acc", true);
244		✗	constraints_->changeConstraintStatus(name + "_force", false);
245			}
246		✗	for (std::string name : contacts_->get_inactive_set()) {
247		✗	constraints_->changeConstraintStatus(name + "_acc", false);
248		✗	constraints_->changeConstraintStatus(name + "_force", true);
249			}
250		✗	d->constraints->resize(this, d);
251		✗	constraints_->calcDiff(d->constraints, x, u);
252		✗	}
253
254			template <typename Scalar>
255		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calcDiff(
256			const std::shared_ptr<DifferentialActionDataAbstract>& data,
257			const Eigen::Ref<const VectorXs>& x) {
258		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
259		✗	throw_pretty(
260			"Invalid argument: " << "x has wrong dimension (it should be " +
261			std::to_string(state_->get_nx()) + ")");
262			}
263		✗	Data* d = static_cast<Data*>(data.get());
264		✗	costs_->calcDiff(d->costs, x);
265		✗	if (constraints_ != nullptr) {
266		✗	constraints_->calcDiff(d->constraints, x);
267			}
268		✗	}
269
270			template <typename Scalar>
271			std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar>>
272		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::createData() {
273		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
274			}
275
276			template <typename Scalar>
277		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::quasiStatic(
278			const std::shared_ptr<DifferentialActionDataAbstract>& data,
279			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
280			Scalar) {
281		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
282		✗	throw_pretty(
283			"Invalid argument: " << "u has wrong dimension (it should be " +
284			std::to_string(nu_) + ")");
285			}
286		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
287		✗	throw_pretty(
288			"Invalid argument: " << "x has wrong dimension (it should be " +
289			std::to_string(state_->get_nx()) + ")");
290			}
291		✗	Data* d = static_cast<Data*>(data.get());
292		✗	const std::size_t nq = state_->get_nq();
293		✗	const std::size_t nv = state_->get_nv();
294		✗	const std::size_t nu = actuation_->get_nu();
295		✗	std::size_t nc = contacts_->get_nc_total();
296			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
297		✗	x.head(nq);
298		✗	d->tmp_xstatic.head(nq) = q;
299		✗	d->tmp_xstatic.tail(nv).setZero();
300		✗	u.setZero();
301
302		✗	pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q,
303		✗	d->tmp_xstatic.tail(nv));
304		✗	pinocchio::computeJointJacobians(*pinocchio_, d->pinocchio, q);
305		✗	pinocchio::rnea(*pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
306		✗	d->tmp_xstatic.tail(nv));
307		✗	actuation_->calc(d->multibody.actuation, d->tmp_xstatic,
308		✗	d->tmp_xstatic.tail(nu));
309		✗	actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic,
310		✗	d->tmp_xstatic.tail(nu));
311		✗	contacts_->setComputeAllContacts(false);
312		✗	contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
313		✗	contacts_->setComputeAllContacts(true);
314
315		✗	d->tmp_Jstatic.conservativeResize(nv, nu + nc);
316		✗	d->tmp_Jstatic.leftCols(nu) = d->multibody.actuation->dtau_du;
317		✗	d->tmp_Jstatic.rightCols(nc) =
318		✗	d->multibody.contacts->Jc.topRows(nc).transpose();
319		✗	d->tmp_rstatic.noalias() = pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau;
320		✗	if (nc != 0) {
321		✗	nc = 0;
322		✗	std::size_t nc_r = 0;
323		✗	for (typename ContactModelMultiple::ContactModelContainer::const_iterator
324		✗	it_m = contacts_->get_contacts().begin();
325		✗	it_m != contacts_->get_contacts().end(); ++it_m) {
326		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
327		✗	const std::size_t nc_i = m_i->contact->get_nc();
328		✗	if (m_i->active) {
329		✗	u.segment(nv + nc, nc_i) = d->tmp_rstatic.segment(nu + nc_r, nc_i);
330		✗	nc_r += nc_i;
331			} else {
332		✗	u.segment(nv + nc, nc_i).setZero();
333			}
334		✗	nc += nc_i;
335			}
336			}
337		✗	d->pinocchio.tau.setZero();
338		✗	}
339
340			template <typename Scalar>
341			template <typename NewScalar>
342			DifferentialActionModelContactInvDynamicsTpl<NewScalar>
343		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::cast() const {
344			typedef DifferentialActionModelContactInvDynamicsTpl<NewScalar> ReturnType;
345			typedef StateMultibodyTpl<NewScalar> StateType;
346			typedef ContactModelMultipleTpl<NewScalar> ContactType;
347			typedef CostModelSumTpl<NewScalar> CostType;
348			typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
349		✗	if (constraints_) {
350		✗	const std::shared_ptr<ConstraintType>& constraints =
351			std::make_shared<ConstraintType>(
352		✗	constraints_->template cast<NewScalar>());
353		✗	if (state_->get_nv() - actuation_->get_nu() > 0) {
354		✗	constraints->removeConstraint("tau");
355			}
356		✗	if (contacts_->get_nc_total() != 0) {
357		✗	typename ContactModelMultiple::ContactModelContainer contact_list;
358		✗	contact_list = contacts_->get_contacts();
359		✗	typename ContactModelMultiple::ContactModelContainer::iterator it_m,
360		✗	end_m;
361		✗	for (it_m = contact_list.begin(), end_m = contact_list.end();
362		✗	it_m != end_m; ++it_m) {
363		✗	const std::string name = it_m->second->name;
364		✗	constraints->removeConstraint(name + "_acc");
365		✗	constraints->removeConstraint(name + "_force");
366			}
367		✗	}
368		✗	ReturnType ret(
369		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
370		✗	actuation_->template cast<NewScalar>(),
371		✗	std::make_shared<ContactType>(contacts_->template cast<NewScalar>()),
372		✗	std::make_shared<CostType>(costs_->template cast<NewScalar>()),
373			constraints);
374		✗	return ret;
375		✗	} else {
376		✗	ReturnType ret(
377		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
378		✗	actuation_->template cast<NewScalar>(),
379		✗	std::make_shared<ContactType>(contacts_->template cast<NewScalar>()),
380		✗	std::make_shared<CostType>(costs_->template cast<NewScalar>()));
381		✗	return ret;
382		✗	}
383			}
384
385			template <typename Scalar>
386		✗	bool DifferentialActionModelContactInvDynamicsTpl<Scalar>::checkData(
387			const std::shared_ptr<DifferentialActionDataAbstract>& data) {
388		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
389		✗	if (d != NULL) {
390		✗	return true;
391			} else {
392		✗	return false;
393			}
394		✗	}
395
396			template <typename Scalar>
397		✗	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::print(
398			std::ostream& os) const {
399		✗	os << "DifferentialActionModelContactInvDynamics {nx=" << state_->get_nx()
400		✗	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_
401		✗	<< ", nc=" << contacts_->get_nc_total() << "}";
402		✗	}
403
404			template <typename Scalar>
405		✗	std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_ng()
406			const {
407		✗	if (constraints_ != nullptr) {
408		✗	return constraints_->get_ng();
409			} else {
410		✗	return Base::get_ng();
411			}
412			}
413
414			template <typename Scalar>
415		✗	std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_nh()
416			const {
417		✗	if (constraints_ != nullptr) {
418		✗	return constraints_->get_nh();
419			} else {
420		✗	return Base::get_nh();
421			}
422			}
423
424			template <typename Scalar>
425		✗	std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_ng_T()
426			const {
427		✗	if (constraints_ != nullptr) {
428		✗	return constraints_->get_ng_T();
429			} else {
430		✗	return Base::get_ng_T();
431			}
432			}
433
434			template <typename Scalar>
435		✗	std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_nh_T()
436			const {
437		✗	if (constraints_ != nullptr) {
438		✗	return constraints_->get_nh_T();
439			} else {
440		✗	return Base::get_nh_T();
441			}
442			}
443
444			template <typename Scalar>
445			const typename MathBaseTpl<Scalar>::VectorXs&
446		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_g_lb() const {
447		✗	if (constraints_ != nullptr) {
448		✗	return constraints_->get_lb();
449			} else {
450		✗	return g_lb_;
451			}
452			}
453
454			template <typename Scalar>
455			const typename MathBaseTpl<Scalar>::VectorXs&
456		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_g_ub() const {
457		✗	if (constraints_ != nullptr) {
458		✗	return constraints_->get_ub();
459			} else {
460		✗	return g_lb_;
461			}
462			}
463
464			template <typename Scalar>
465			pinocchio::ModelTpl<Scalar>&
466		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_pinocchio() const {
467		✗	return *pinocchio_;
468			}
469
470			template <typename Scalar>
471			const std::shared_ptr<ActuationModelAbstractTpl<Scalar>>&
472		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_actuation() const {
473		✗	return actuation_;
474			}
475
476			template <typename Scalar>
477			const std::shared_ptr<ContactModelMultipleTpl<Scalar>>&
478		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_contacts() const {
479		✗	return contacts_;
480			}
481
482			template <typename Scalar>
483			const std::shared_ptr<CostModelSumTpl<Scalar>>&
484		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_costs() const {
485		✗	return costs_;
486			}
487
488			template <typename Scalar>
489			const std::shared_ptr<ConstraintModelManagerTpl<Scalar>>&
490		✗	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_constraints() const {
491		✗	return constraints_;
492			}
493
494			} // namespace crocoddyl
495