GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh, CTU, INRIA,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			namespace crocoddyl {
11
12			template <typename Scalar>
13		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
14			DifferentialActionModelContactFwdDynamicsTpl(
15			std::shared_ptr<StateMultibody> state,
16			std::shared_ptr<ActuationModelAbstract> actuation,
17			std::shared_ptr<ContactModelMultiple> contacts,
18			std::shared_ptr<CostModelSum> costs, const Scalar JMinvJt_damping,
19			const bool enable_force)
20			: Base(state, actuation->get_nu(), costs->get_nr(), 0, 0),
21		✗	actuation_(actuation),
22		✗	contacts_(contacts),
23		✗	costs_(costs),
24		✗	constraints_(nullptr),
25		✗	pinocchio_(state->get_pinocchio().get()),
26		✗	with_armature_(true),
27		✗	armature_(VectorXs::Zero(state->get_nv())),
28		✗	JMinvJt_damping_(fabs(JMinvJt_damping)),
29		✗	enable_force_(enable_force) {
30		✗	init();
31		✗	}
32
33			template <typename Scalar>
34		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
35			DifferentialActionModelContactFwdDynamicsTpl(
36			std::shared_ptr<StateMultibody> state,
37			std::shared_ptr<ActuationModelAbstract> actuation,
38			std::shared_ptr<ContactModelMultiple> contacts,
39			std::shared_ptr<CostModelSum> costs,
40			std::shared_ptr<ConstraintModelManager> constraints,
41			const Scalar JMinvJt_damping, const bool enable_force)
42			: Base(state, actuation->get_nu(), costs->get_nr(), constraints->get_ng(),
43			constraints->get_nh(), constraints->get_ng_T(),
44			constraints->get_nh_T()),
45		✗	actuation_(actuation),
46		✗	contacts_(contacts),
47		✗	costs_(costs),
48		✗	constraints_(constraints),
49		✗	pinocchio_(state->get_pinocchio().get()),
50		✗	with_armature_(true),
51		✗	armature_(VectorXs::Zero(state->get_nv())),
52		✗	JMinvJt_damping_(fabs(JMinvJt_damping)),
53		✗	enable_force_(enable_force) {
54		✗	init();
55		✗	}
56
57			template <typename Scalar>
58		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::init() {
59		✗	if (JMinvJt_damping_ < Scalar(0.)) {
60		✗	JMinvJt_damping_ = Scalar(0.);
61		✗	throw_pretty("Invalid argument: "
62			<< "The damping factor has to be positive, set to 0");
63			}
64		✗	if (contacts_->get_nu() != nu_) {
65		✗	throw_pretty(
66			"Invalid argument: "
67			<< "Contacts doesn't have the same control dimension (it should be " +
68			std::to_string(nu_) + ")");
69			}
70		✗	if (costs_->get_nu() != nu_) {
71		✗	throw_pretty(
72			"Invalid argument: "
73			<< "Costs doesn't have the same control dimension (it should be " +
74			std::to_string(nu_) + ")");
75			}
76
77		✗	Base::set_u_lb(Scalar(-1.) * pinocchio_->effortLimit.tail(nu_));
78		✗	Base::set_u_ub(Scalar(+1.) * pinocchio_->effortLimit.tail(nu_));
79		✗	}
80
81			template <typename Scalar>
82		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
83			const std::shared_ptr<DifferentialActionDataAbstract>& data,
84			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
85		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
86		✗	throw_pretty(
87			"Invalid argument: " << "x has wrong dimension (it should be " +
88			std::to_string(state_->get_nx()) + ")");
89			}
90		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
91		✗	throw_pretty(
92			"Invalid argument: " << "u has wrong dimension (it should be " +
93			std::to_string(nu_) + ")");
94			}
95
96		✗	const std::size_t nc = contacts_->get_nc();
97		✗	Data* d = static_cast<Data*>(data.get());
98			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
99		✗	x.head(state_->get_nq());
100			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
101		✗	x.tail(state_->get_nv());
102
103			// Computing the forward dynamics with the holonomic constraints defined by
104			// the contact model
105		✗	pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
106		✗	pinocchio::computeCentroidalMomentum(*pinocchio_, d->pinocchio);
107
108		✗	if (!with_armature_) {
109		✗	d->pinocchio.M.diagonal() += armature_;
110			}
111		✗	actuation_->calc(d->multibody.actuation, x, u);
112		✗	contacts_->calc(d->multibody.contacts, x);
113
114			#ifndef NDEBUG
115		✗	Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.contacts->Jc.topRows(nc));
116
117		✗	if (Jc_lu.rank() < d->multibody.contacts->Jc.topRows(nc).rows() &&
118		✗	JMinvJt_damping_ == Scalar(0.)) {
119		✗	throw_pretty(
120			"A damping factor is needed as the contact Jacobian is not full-rank");
121			}
122			#endif
123
124		✗	pinocchio::forwardDynamics(
125		✗	*pinocchio_, d->pinocchio, d->multibody.actuation->tau,
126		✗	d->multibody.contacts->Jc.topRows(nc), d->multibody.contacts->a0.head(nc),
127			JMinvJt_damping_);
128		✗	d->xout = d->pinocchio.ddq;
129		✗	contacts_->updateAcceleration(d->multibody.contacts, d->pinocchio.ddq);
130		✗	contacts_->updateForce(d->multibody.contacts, d->pinocchio.lambda_c);
131		✗	d->multibody.joint->a = d->pinocchio.ddq;
132		✗	d->multibody.joint->tau = u;
133		✗	costs_->calc(d->costs, x, u);
134		✗	d->cost = d->costs->cost;
135		✗	if (constraints_ != nullptr) {
136		✗	d->constraints->resize(this, d);
137		✗	constraints_->calc(d->constraints, x, u);
138			}
139		✗	}
140
141			template <typename Scalar>
142		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
143			const std::shared_ptr<DifferentialActionDataAbstract>& data,
144			const Eigen::Ref<const VectorXs>& x) {
145		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
146		✗	throw_pretty(
147			"Invalid argument: " << "x has wrong dimension (it should be " +
148			std::to_string(state_->get_nx()) + ")");
149			}
150
151		✗	Data* d = static_cast<Data*>(data.get());
152			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
153		✗	x.head(state_->get_nq());
154			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
155		✗	x.tail(state_->get_nv());
156
157		✗	pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
158		✗	pinocchio::computeCentroidalMomentum(*pinocchio_, d->pinocchio);
159		✗	costs_->calc(d->costs, x);
160		✗	d->cost = d->costs->cost;
161		✗	if (constraints_ != nullptr) {
162		✗	d->constraints->resize(this, d, false);
163		✗	constraints_->calc(d->constraints, x);
164			}
165		✗	}
166
167			template <typename Scalar>
168		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
169			const std::shared_ptr<DifferentialActionDataAbstract>& data,
170			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
171		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
172		✗	throw_pretty(
173			"Invalid argument: " << "x has wrong dimension (it should be " +
174			std::to_string(state_->get_nx()) + ")");
175			}
176		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
177		✗	throw_pretty(
178			"Invalid argument: " << "u has wrong dimension (it should be " +
179			std::to_string(nu_) + ")");
180			}
181
182		✗	const std::size_t nv = state_->get_nv();
183		✗	const std::size_t nc = contacts_->get_nc();
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(nv);
188
189		✗	Data* d = static_cast<Data*>(data.get());
190
191			// Computing the dynamics derivatives
192			// We resize the Kinv matrix because Eigen cannot call block operations
193			// recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
194			// it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
195		✗	d->Kinv.resize(nv + nc, nv + nc);
196		✗	pinocchio::computeRNEADerivatives(*pinocchio_, d->pinocchio, q, v, d->xout,
197		✗	d->multibody.contacts->fext);
198		✗	contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
199		✗	pinocchio::getKKTContactDynamicMatrixInverse(
200		✗	*pinocchio_, d->pinocchio, d->multibody.contacts->Jc.topRows(nc),
201		✗	d->Kinv);
202
203		✗	actuation_->calcDiff(d->multibody.actuation, x, u);
204		✗	contacts_->calcDiff(d->multibody.contacts, x);
205
206		✗	const Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
207		✗	const Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, nc);
208			const Eigen::Block<MatrixXs> f_partial_dtau =
209		✗	d->Kinv.bottomLeftCorner(nc, nv);
210		✗	const Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);
211
212		✗	d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
213		✗	d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
214		✗	d->Fx.noalias() -= a_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
215		✗	d->Fx.noalias() += a_partial_dtau * d->multibody.actuation->dtau_dx;
216		✗	d->Fu.noalias() = a_partial_dtau * d->multibody.actuation->dtau_du;
217		✗	d->multibody.joint->da_dx = d->Fx;
218		✗	d->multibody.joint->da_du = d->Fu;
219
220			// Computing the cost derivatives
221		✗	if (enable_force_) {
222		✗	d->df_dx.topLeftCorner(nc, nv).noalias() =
223		✗	f_partial_dtau * d->pinocchio.dtau_dq;
224		✗	d->df_dx.topRightCorner(nc, nv).noalias() =
225		✗	f_partial_dtau * d->pinocchio.dtau_dv;
226		✗	d->df_dx.topRows(nc).noalias() +=
227		✗	f_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
228		✗	d->df_dx.topRows(nc).noalias() -=
229		✗	f_partial_dtau * d->multibody.actuation->dtau_dx;
230		✗	d->df_du.topRows(nc).noalias() =
231		✗	-f_partial_dtau * d->multibody.actuation->dtau_du;
232		✗	contacts_->updateAccelerationDiff(d->multibody.contacts,
233		✗	d->Fx.bottomRows(nv));
234		✗	contacts_->updateForceDiff(d->multibody.contacts, d->df_dx.topRows(nc),
235		✗	d->df_du.topRows(nc));
236			}
237		✗	costs_->calcDiff(d->costs, x, u);
238		✗	if (constraints_ != nullptr) {
239		✗	constraints_->calcDiff(d->constraints, x, u);
240			}
241		✗	}
242
243			template <typename Scalar>
244		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
245			const std::shared_ptr<DifferentialActionDataAbstract>& data,
246			const Eigen::Ref<const VectorXs>& x) {
247		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
248		✗	throw_pretty(
249			"Invalid argument: " << "x has wrong dimension (it should be " +
250			std::to_string(state_->get_nx()) + ")");
251			}
252		✗	Data* d = static_cast<Data*>(data.get());
253		✗	costs_->calcDiff(d->costs, x);
254		✗	if (constraints_ != nullptr) {
255		✗	constraints_->calcDiff(d->constraints, x);
256			}
257		✗	}
258
259			template <typename Scalar>
260			std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
261		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::createData() {
262		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
263			}
264
265			template <typename Scalar>
266			template <typename NewScalar>
267			DifferentialActionModelContactFwdDynamicsTpl<NewScalar>
268		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::cast() const {
269			typedef DifferentialActionModelContactFwdDynamicsTpl<NewScalar> ReturnType;
270			typedef StateMultibodyTpl<NewScalar> StateType;
271			typedef ContactModelMultipleTpl<NewScalar> ContactType;
272			typedef CostModelSumTpl<NewScalar> CostType;
273			typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
274		✗	if (constraints_) {
275		✗	ReturnType ret(
276		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
277		✗	actuation_->template cast<NewScalar>(),
278		✗	std::make_shared<ContactType>(contacts_->template cast<NewScalar>()),
279		✗	std::make_shared<CostType>(costs_->template cast<NewScalar>()),
280			std::make_shared<ConstraintType>(
281		✗	constraints_->template cast<NewScalar>()),
282		✗	scalar_cast<NewScalar>(JMinvJt_damping_), enable_force_);
283		✗	return ret;
284		✗	} else {
285		✗	ReturnType ret(
286		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
287		✗	actuation_->template cast<NewScalar>(),
288		✗	std::make_shared<ContactType>(contacts_->template cast<NewScalar>()),
289		✗	std::make_shared<CostType>(costs_->template cast<NewScalar>()),
290		✗	scalar_cast<NewScalar>(JMinvJt_damping_), enable_force_);
291		✗	return ret;
292		✗	}
293			}
294
295			template <typename Scalar>
296		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::quasiStatic(
297			const std::shared_ptr<DifferentialActionDataAbstract>& data,
298			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
299			Scalar) {
300		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
301		✗	throw_pretty(
302			"Invalid argument: " << "u has wrong dimension (it should be " +
303			std::to_string(nu_) + ")");
304			}
305		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
306		✗	throw_pretty(
307			"Invalid argument: " << "x has wrong dimension (it should be " +
308			std::to_string(state_->get_nx()) + ")");
309			}
310			// Static casting the data
311			DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
312			static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*>(
313		✗	data.get());
314			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
315		✗	x.head(state_->get_nq());
316
317		✗	const std::size_t nq = state_->get_nq();
318		✗	const std::size_t nv = state_->get_nv();
319		✗	const std::size_t nc = contacts_->get_nc();
320
321		✗	d->tmp_xstatic.head(nq) = q;
322		✗	d->tmp_xstatic.tail(nv).setZero();
323		✗	u.setZero();
324
325		✗	pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q,
326		✗	d->tmp_xstatic.tail(nv));
327		✗	pinocchio::computeJointJacobians(*pinocchio_, d->pinocchio, q);
328		✗	pinocchio::rnea(*pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
329		✗	d->tmp_xstatic.tail(nv));
330		✗	actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
331		✗	actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
332		✗	contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
333
334			// Allocates memory
335		✗	d->tmp_Jstatic.conservativeResize(nv, nu_ + nc);
336		✗	d->tmp_Jstatic.leftCols(nu_) = d->multibody.actuation->dtau_du;
337		✗	d->tmp_Jstatic.rightCols(nc) =
338		✗	d->multibody.contacts->Jc.topRows(nc).transpose();
339		✗	u.noalias() = (pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau).head(nu_);
340		✗	d->pinocchio.tau.setZero();
341		✗	}
342
343			template <typename Scalar>
344		✗	bool DifferentialActionModelContactFwdDynamicsTpl<Scalar>::checkData(
345			const std::shared_ptr<DifferentialActionDataAbstract>& data) {
346		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
347		✗	if (d != NULL) {
348		✗	return true;
349			} else {
350		✗	return false;
351			}
352		✗	}
353
354			template <typename Scalar>
355		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::print(
356			std::ostream& os) const {
357		✗	os << "DifferentialActionModelContactFwdDynamics {nx=" << state_->get_nx()
358		✗	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_
359		✗	<< ", nc=" << contacts_->get_nc() << "}";
360		✗	}
361
362			template <typename Scalar>
363		✗	std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_ng()
364			const {
365		✗	if (constraints_ != nullptr) {
366		✗	return constraints_->get_ng();
367			} else {
368		✗	return Base::get_ng();
369			}
370			}
371
372			template <typename Scalar>
373		✗	std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_nh()
374			const {
375		✗	if (constraints_ != nullptr) {
376		✗	return constraints_->get_nh();
377			} else {
378		✗	return Base::get_nh();
379			}
380			}
381
382			template <typename Scalar>
383		✗	std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_ng_T()
384			const {
385		✗	if (constraints_ != nullptr) {
386		✗	return constraints_->get_ng_T();
387			} else {
388		✗	return Base::get_ng_T();
389			}
390			}
391
392			template <typename Scalar>
393		✗	std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_nh_T()
394			const {
395		✗	if (constraints_ != nullptr) {
396		✗	return constraints_->get_nh_T();
397			} else {
398		✗	return Base::get_nh_T();
399			}
400			}
401
402			template <typename Scalar>
403			const typename MathBaseTpl<Scalar>::VectorXs&
404		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_g_lb() const {
405		✗	if (constraints_ != nullptr) {
406		✗	return constraints_->get_lb();
407			} else {
408		✗	return g_lb_;
409			}
410			}
411
412			template <typename Scalar>
413			const typename MathBaseTpl<Scalar>::VectorXs&
414		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_g_ub() const {
415		✗	if (constraints_ != nullptr) {
416		✗	return constraints_->get_ub();
417			} else {
418		✗	return g_lb_;
419			}
420			}
421
422			template <typename Scalar>
423			pinocchio::ModelTpl<Scalar>&
424		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_pinocchio() const {
425		✗	return *pinocchio_;
426			}
427
428			template <typename Scalar>
429			const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
430		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_actuation() const {
431		✗	return actuation_;
432			}
433
434			template <typename Scalar>
435			const std::shared_ptr<ContactModelMultipleTpl<Scalar> >&
436		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_contacts() const {
437		✗	return contacts_;
438			}
439
440			template <typename Scalar>
441			const std::shared_ptr<CostModelSumTpl<Scalar> >&
442		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_costs() const {
443		✗	return costs_;
444			}
445
446			template <typename Scalar>
447			const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
448		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_constraints() const {
449		✗	return constraints_;
450			}
451
452			template <typename Scalar>
453			const typename MathBaseTpl<Scalar>::VectorXs&
454		✗	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_armature() const {
455		✗	return armature_;
456			}
457
458			template <typename Scalar>
459		✗	const Scalar DifferentialActionModelContactFwdDynamicsTpl<
460			Scalar>::get_damping_factor() const {
461		✗	return JMinvJt_damping_;
462			}
463
464			template <typename Scalar>
465		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_armature(
466			const VectorXs& armature) {
467		✗	if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
468		✗	throw_pretty("Invalid argument: "
469			<< "The armature dimension is wrong (it should be " +
470			std::to_string(state_->get_nv()) + ")");
471			}
472		✗	armature_ = armature;
473		✗	with_armature_ = false;
474		✗	}
475
476			template <typename Scalar>
477		✗	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_damping_factor(
478			const Scalar damping) {
479		✗	if (damping < 0.) {
480		✗	throw_pretty(
481			"Invalid argument: " << "The damping factor has to be positive");
482			}
483		✗	JMinvJt_damping_ = damping;
484		✗	}
485
486			} // namespace crocoddyl
487