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,
5			// 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		✗	ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(
14			std::shared_ptr<StateMultibody> state, const std::size_t nu)
15		✗	: state_(state),
16		✗	nc_(0),
17		✗	nc_total_(0),
18		✗	nu_(nu),
19		✗	compute_all_contacts_(false) {}
20
21			template <typename Scalar>
22		✗	ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(
23			std::shared_ptr<StateMultibody> state)
24		✗	: state_(state),
25		✗	nc_(0),
26		✗	nc_total_(0),
27		✗	nu_(state->get_nv()),
28		✗	compute_all_contacts_(false) {}
29
30			template <typename Scalar>
31		✗	ContactModelMultipleTpl<Scalar>::~ContactModelMultipleTpl() {}
32
33			template <typename Scalar>
34		✗	void ContactModelMultipleTpl<Scalar>::addContact(
35			const std::string& name, std::shared_ptr<ContactModelAbstract> contact,
36			const bool active) {
37		✗	if (contact->get_nu() != nu_) {
38		✗	throw_pretty("Invalid argument: "
39			<< name
40			<< " contact item doesn't have the same control dimension (" +
41			std::to_string(nu_) + ")");
42			}
43			std::pair<typename ContactModelContainer::iterator, bool> ret =
44		✗	contacts_.insert(std::make_pair(
45			name, std::make_shared<ContactItem>(name, contact, active)));
46		✗	if (ret.second == false) {
47			std::cerr << "Warning: we couldn't add the " << name
48		✗	<< " contact item, it already existed." << std::endl;
49		✗	} else if (active) {
50		✗	nc_ += contact->get_nc();
51		✗	nc_total_ += contact->get_nc();
52		✗	active_set_.insert(name);
53		✗	} else if (!active) {
54		✗	nc_total_ += contact->get_nc();
55		✗	inactive_set_.insert(name);
56			}
57		✗	}
58
59			template <typename Scalar>
60		✗	void ContactModelMultipleTpl<Scalar>::removeContact(const std::string& name) {
61		✗	typename ContactModelContainer::iterator it = contacts_.find(name);
62		✗	if (it != contacts_.end()) {
63		✗	nc_ -= it->second->contact->get_nc();
64		✗	nc_total_ -= it->second->contact->get_nc();
65		✗	contacts_.erase(it);
66		✗	active_set_.erase(name);
67		✗	inactive_set_.erase(name);
68			} else {
69			std::cerr << "Warning: we couldn't remove the " << name
70		✗	<< " contact item, it doesn't exist." << std::endl;
71			}
72		✗	}
73
74			template <typename Scalar>
75		✗	void ContactModelMultipleTpl<Scalar>::changeContactStatus(
76			const std::string& name, const bool active) {
77		✗	typename ContactModelContainer::iterator it = contacts_.find(name);
78		✗	if (it != contacts_.end()) {
79		✗	if (active && !it->second->active) {
80		✗	nc_ += it->second->contact->get_nc();
81		✗	active_set_.insert(name);
82		✗	inactive_set_.erase(name);
83		✗	} else if (!active && it->second->active) {
84		✗	nc_ -= it->second->contact->get_nc();
85		✗	inactive_set_.insert(name);
86		✗	active_set_.erase(name);
87			}
88			// "else" case: Contact status unchanged - already in desired state
89		✗	it->second->active = active;
90			} else {
91			std::cerr << "Warning: we couldn't change the status of the " << name
92		✗	<< " contact item, it doesn't exist." << std::endl;
93			}
94		✗	}
95
96			template <typename Scalar>
97		✗	void ContactModelMultipleTpl<Scalar>::calc(
98			const std::shared_ptr<ContactDataMultiple>& data,
99			const Eigen::Ref<const VectorXs>& x) {
100		✗	if (data->contacts.size() != contacts_.size()) {
101		✗	throw_pretty("Invalid argument: "
102			<< "it doesn't match the number of contact datas and models");
103			}
104
105		✗	std::size_t nc = 0;
106		✗	const std::size_t nv = state_->get_nv();
107		✗	typename ContactModelContainer::iterator it_m, end_m;
108		✗	typename ContactDataContainer::iterator it_d, end_d;
109		✗	if (compute_all_contacts_) {
110		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
111		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
112		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
113		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
114		✗	const std::size_t nc_i = m_i->contact->get_nc();
115		✗	if (m_i->active) {
116		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
117		✗	assert_pretty(
118			it_m->first == it_d->first,
119			"it doesn't match the contact name between model and data ("
120			<< it_m->first << " != " << it_d->first << ")");
121		✗	m_i->contact->calc(d_i, x);
122		✗	data->a0.segment(nc, nc_i) = d_i->a0;
123		✗	data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
124			} else {
125		✗	data->a0.segment(nc, nc_i).setZero();
126		✗	data->Jc.block(nc, 0, nc_i, nv).setZero();
127			}
128		✗	nc += nc_i;
129			}
130			} else {
131		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
132		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
133		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
134		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
135		✗	if (m_i->active) {
136		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
137		✗	assert_pretty(
138			it_m->first == it_d->first,
139			"it doesn't match the contact name between model and data ("
140			<< it_m->first << " != " << it_d->first << ")");
141
142		✗	m_i->contact->calc(d_i, x);
143		✗	const std::size_t nc_i = m_i->contact->get_nc();
144		✗	data->a0.segment(nc, nc_i) = d_i->a0;
145		✗	data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
146		✗	nc += nc_i;
147			}
148			}
149			}
150		✗	}
151
152			template <typename Scalar>
153		✗	void ContactModelMultipleTpl<Scalar>::calcDiff(
154			const std::shared_ptr<ContactDataMultiple>& data,
155			const Eigen::Ref<const VectorXs>& x) {
156		✗	if (data->contacts.size() != contacts_.size()) {
157		✗	throw_pretty("Invalid argument: "
158			<< "it doesn't match the number of contact datas and models");
159			}
160
161		✗	std::size_t nc = 0;
162		✗	const std::size_t ndx = state_->get_ndx();
163		✗	typename ContactModelContainer::iterator it_m, end_m;
164		✗	typename ContactDataContainer::iterator it_d, end_d;
165		✗	if (compute_all_contacts_) {
166		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
167		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
168		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
169		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
170		✗	const std::size_t nc_i = m_i->contact->get_nc();
171		✗	if (m_i->active) {
172		✗	assert_pretty(
173			it_m->first == it_d->first,
174			"it doesn't match the contact name between model and data ("
175			<< it_m->first << " != " << it_d->first << ")");
176		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
177
178		✗	m_i->contact->calcDiff(d_i, x);
179		✗	data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx;
180			} else {
181		✗	data->da0_dx.block(nc, 0, nc_i, ndx).setZero();
182			}
183		✗	nc += nc_i;
184			}
185			} else {
186		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
187		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
188		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
189		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
190		✗	if (m_i->active) {
191		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
192		✗	assert_pretty(
193			it_m->first == it_d->first,
194			"it doesn't match the contact name between model and data ("
195			<< it_m->first << " != " << it_d->first << ")");
196
197		✗	m_i->contact->calcDiff(d_i, x);
198		✗	const std::size_t nc_i = m_i->contact->get_nc();
199		✗	data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx;
200		✗	nc += nc_i;
201			}
202			}
203			}
204		✗	}
205
206			template <typename Scalar>
207		✗	void ContactModelMultipleTpl<Scalar>::updateAcceleration(
208			const std::shared_ptr<ContactDataMultiple>& data,
209			const VectorXs& dv) const {
210		✗	if (static_cast<std::size_t>(dv.size()) != state_->get_nv()) {
211		✗	throw_pretty(
212			"Invalid argument: " << "dv has wrong dimension (it should be " +
213			std::to_string(state_->get_nv()) + ")");
214			}
215		✗	data->dv = dv;
216		✗	}
217
218			template <typename Scalar>
219		✗	void ContactModelMultipleTpl<Scalar>::updateForce(
220			const std::shared_ptr<ContactDataMultiple>& data, const VectorXs& force) {
221		✗	if (static_cast<std::size_t>(force.size()) !=
222		✗	(compute_all_contacts_ ? nc_total_ : nc_)) {
223		✗	throw_pretty(
224			"Invalid argument: "
225			<< "force has wrong dimension (it should be " +
226			std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + ")");
227			}
228		✗	if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
229		✗	throw_pretty("Invalid argument: "
230			<< "it doesn't match the number of contact datas and models");
231			}
232
233		✗	for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
234		✗	*it = pinocchio::ForceTpl<Scalar>::Zero();
235			}
236
237		✗	std::size_t nc = 0;
238		✗	typename ContactModelContainer::iterator it_m, end_m;
239		✗	typename ContactDataContainer::iterator it_d, end_d;
240		✗	if (compute_all_contacts_) {
241		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
242		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
243		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
244		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
245		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
246		✗	assert_pretty(it_m->first == it_d->first,
247			"it doesn't match the contact name between data and model");
248		✗	const std::size_t nc_i = m_i->contact->get_nc();
249		✗	if (m_i->active) {
250			const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
251		✗	force.segment(nc, nc_i);
252		✗	m_i->contact->updateForce(d_i, force_i);
253		✗	const pinocchio::JointIndex joint =
254		✗	state_->get_pinocchio()->frames[d_i->frame].parentJoint;
255		✗	data->fext[joint] = d_i->fext;
256			} else {
257		✗	m_i->contact->setZeroForce(d_i);
258			}
259		✗	nc += nc_i;
260			}
261			} else {
262		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
263		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
264		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
265		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
266		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
267		✗	assert_pretty(it_m->first == it_d->first,
268			"it doesn't match the contact name between data and model");
269		✗	if (m_i->active) {
270		✗	const std::size_t nc_i = m_i->contact->get_nc();
271			const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
272		✗	force.segment(nc, nc_i);
273		✗	m_i->contact->updateForce(d_i, force_i);
274		✗	const pinocchio::JointIndex joint =
275		✗	state_->get_pinocchio()->frames[d_i->frame].parentJoint;
276		✗	data->fext[joint] = d_i->fext;
277		✗	nc += nc_i;
278			} else {
279		✗	m_i->contact->setZeroForce(d_i);
280			}
281			}
282			}
283		✗	}
284
285			template <typename Scalar>
286		✗	void ContactModelMultipleTpl<Scalar>::updateAccelerationDiff(
287			const std::shared_ptr<ContactDataMultiple>& data,
288			const MatrixXs& ddv_dx) const {
289		✗	if (static_cast<std::size_t>(ddv_dx.rows()) != state_->get_nv() ||
290		✗	static_cast<std::size_t>(ddv_dx.cols()) != state_->get_ndx()) {
291		✗	throw_pretty(
292			"Invalid argument: " << "ddv_dx has wrong dimension (it should be " +
293			std::to_string(state_->get_nv()) + "," +
294			std::to_string(state_->get_ndx()) + ")");
295			}
296		✗	data->ddv_dx = ddv_dx;
297		✗	}
298
299			template <typename Scalar>
300		✗	void ContactModelMultipleTpl<Scalar>::updateForceDiff(
301			const std::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx,
302			const MatrixXs& df_du) const {
303		✗	const std::size_t ndx = state_->get_ndx();
304		✗	if (static_cast<std::size_t>(df_dx.rows()) !=
305		✗	(compute_all_contacts_ ? nc_total_ : nc_) ||
306		✗	static_cast<std::size_t>(df_dx.cols()) != ndx) {
307		✗	throw_pretty(
308			"Invalid argument: "
309			<< "df_dx has wrong dimension (it should be " +
310			std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + "," +
311			std::to_string(ndx) + ")");
312			}
313		✗	if (static_cast<std::size_t>(df_du.rows()) !=
314		✗	(compute_all_contacts_ ? nc_total_ : nc_) ||
315		✗	static_cast<std::size_t>(df_du.cols()) != nu_) {
316		✗	throw_pretty(
317			"Invalid argument: "
318			<< "df_du has wrong dimension (it should be " +
319			std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + "," +
320			std::to_string(nu_) + ")");
321			}
322		✗	if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
323		✗	throw_pretty("Invalid argument: "
324			<< "it doesn't match the number of contact datas and models");
325			}
326
327		✗	std::size_t nc = 0;
328		✗	typename ContactModelContainer::const_iterator it_m, end_m;
329		✗	typename ContactDataContainer::const_iterator it_d, end_d;
330		✗	if (compute_all_contacts_) {
331		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
332		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
333		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
334		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
335		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
336		✗	assert_pretty(it_m->first == it_d->first,
337			"it doesn't match the contact name between data and model");
338		✗	const std::size_t nc_i = m_i->contact->get_nc();
339		✗	if (m_i->active) {
340			const Eigen::Block<const MatrixXs> df_dx_i =
341		✗	df_dx.block(nc, 0, nc_i, ndx);
342			const Eigen::Block<const MatrixXs> df_du_i =
343		✗	df_du.block(nc, 0, nc_i, nu_);
344		✗	m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i);
345			} else {
346		✗	m_i->contact->setZeroForceDiff(d_i);
347			}
348		✗	nc += nc_i;
349			}
350			} else {
351		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
352		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
353		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
354		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
355		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
356		✗	assert_pretty(it_m->first == it_d->first,
357			"it doesn't match the contact name between data and model");
358		✗	if (m_i->active) {
359		✗	const std::size_t nc_i = m_i->contact->get_nc();
360			const Eigen::Block<const MatrixXs> df_dx_i =
361		✗	df_dx.block(nc, 0, nc_i, ndx);
362			const Eigen::Block<const MatrixXs> df_du_i =
363		✗	df_du.block(nc, 0, nc_i, nu_);
364		✗	m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i);
365		✗	nc += nc_i;
366			} else {
367		✗	m_i->contact->setZeroForceDiff(d_i);
368			}
369			}
370			}
371		✗	}
372
373			template <typename Scalar>
374		✗	void ContactModelMultipleTpl<Scalar>::updateRneaDiff(
375			const std::shared_ptr<ContactDataMultiple>& data,
376			pinocchio::DataTpl<Scalar>& pinocchio) const {
377		✗	if (static_cast<std::size_t>(data->contacts.size()) !=
378		✗	this->get_contacts().size()) {
379		✗	throw_pretty("Invalid argument: "
380			<< "it doesn't match the number of contact datas and models");
381			}
382		✗	typename ContactModelContainer::const_iterator it_m, end_m;
383		✗	typename ContactDataContainer::const_iterator it_d, end_d;
384		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(),
385		✗	it_d = data->contacts.begin(), end_d = data->contacts.end();
386		✗	it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
387		✗	const std::shared_ptr<ContactItem>& m_i = it_m->second;
388		✗	const std::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
389		✗	assert_pretty(it_m->first == it_d->first,
390			"it doesn't match the contact name between data and model");
391		✗	if (m_i->active) {
392		✗	switch (m_i->contact->get_type()) {
393		✗	case pinocchio::ReferenceFrame::LOCAL:
394		✗	break;
395		✗	case pinocchio::ReferenceFrame::WORLD:
396			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
397		✗	pinocchio.dtau_dq += d_i->dtau_dq;
398		✗	break;
399			}
400			}
401			}
402		✗	}
403
404			template <typename Scalar>
405			std::shared_ptr<ContactDataMultipleTpl<Scalar> >
406		✗	ContactModelMultipleTpl<Scalar>::createData(
407			pinocchio::DataTpl<Scalar>* const data) {
408			return std::allocate_shared<ContactDataMultiple>(
409		✗	Eigen::aligned_allocator<ContactDataMultiple>(), this, data);
410			}
411
412			template <typename Scalar>
413			template <typename NewScalar>
414		✗	ContactModelMultipleTpl<NewScalar> ContactModelMultipleTpl<Scalar>::cast()
415			const {
416			typedef ContactModelMultipleTpl<NewScalar> ReturnType;
417			typedef StateMultibodyTpl<NewScalar> StateType;
418			typedef ContactItemTpl<NewScalar> ContactType;
419		✗	ReturnType ret(
420		✗	std::make_shared<StateType>(state_->template cast<NewScalar>()), nu_);
421		✗	typename ContactModelContainer::const_iterator it_m, end_m;
422		✗	for (it_m = contacts_.begin(), end_m = contacts_.end(); it_m != end_m;
423		✗	++it_m) {
424		✗	const std::string name = it_m->first;
425		✗	const ContactType& m_i = it_m->second->template cast<NewScalar>();
426		✗	ret.addContact(name, m_i.contact, m_i.active);
427			}
428		✗	return ret;
429		✗	}
430
431			template <typename Scalar>
432			const std::shared_ptr<StateMultibodyTpl<Scalar> >&
433		✗	ContactModelMultipleTpl<Scalar>::get_state() const {
434		✗	return state_;
435			}
436
437			template <typename Scalar>
438			const typename ContactModelMultipleTpl<Scalar>::ContactModelContainer&
439		✗	ContactModelMultipleTpl<Scalar>::get_contacts() const {
440		✗	return contacts_;
441			}
442
443			template <typename Scalar>
444		✗	std::size_t ContactModelMultipleTpl<Scalar>::get_nc() const {
445		✗	return nc_;
446			}
447
448			template <typename Scalar>
449		✗	std::size_t ContactModelMultipleTpl<Scalar>::get_nc_total() const {
450		✗	return nc_total_;
451			}
452
453			template <typename Scalar>
454		✗	std::size_t ContactModelMultipleTpl<Scalar>::get_nu() const {
455		✗	return nu_;
456			}
457
458			template <typename Scalar>
459		✗	const std::set<std::string>& ContactModelMultipleTpl<Scalar>::get_active_set()
460			const {
461		✗	return active_set_;
462			}
463
464			template <typename Scalar>
465		✗	const std::set<std::string>& ContactModelMultipleTpl<Scalar>::get_inactive_set()
466			const {
467		✗	return inactive_set_;
468			}
469
470			template <typename Scalar>
471		✗	bool ContactModelMultipleTpl<Scalar>::getContactStatus(
472			const std::string& name) const {
473		✗	typename ContactModelContainer::const_iterator it = contacts_.find(name);
474		✗	if (it != contacts_.end()) {
475		✗	return it->second->active;
476			} else {
477			std::cerr << "Warning: we couldn't get the status of the " << name
478		✗	<< " contact item, it doesn't exist." << std::endl;
479		✗	return false;
480			}
481			}
482
483			template <typename Scalar>
484		✗	bool ContactModelMultipleTpl<Scalar>::getComputeAllContacts() const {
485		✗	return compute_all_contacts_;
486			}
487
488			template <typename Scalar>
489		✗	void ContactModelMultipleTpl<Scalar>::setComputeAllContacts(const bool status) {
490		✗	compute_all_contacts_ = status;
491		✗	}
492
493			template <class Scalar>
494		✗	std::ostream& operator<<(std::ostream& os,
495			const ContactModelMultipleTpl<Scalar>& model) {
496		✗	const auto& active = model.get_active_set();
497		✗	const auto& inactive = model.get_inactive_set();
498		✗	os << "ContactModelMultiple:" << std::endl;
499		✗	os << " Active:" << std::endl;
500		✗	for (std::set<std::string>::const_iterator it = active.begin();
501		✗	it != active.end(); ++it) {
502			const std::shared_ptr<
503		✗	typename ContactModelMultipleTpl<Scalar>::ContactItem>& contact_item =
504		✗	model.get_contacts().find(*it)->second;
505		✗	if (it != --active.end()) {
506		✗	os << " " << *it << ": " << *contact_item << std::endl;
507			} else {
508		✗	os << " " << *it << ": " << *contact_item << std::endl;
509			}
510			}
511		✗	os << " Inactive:" << std::endl;
512		✗	for (std::set<std::string>::const_iterator it = inactive.begin();
513		✗	it != inactive.end(); ++it) {
514			const std::shared_ptr<
515		✗	typename ContactModelMultipleTpl<Scalar>::ContactItem>& contact_item =
516		✗	model.get_contacts().find(*it)->second;
517		✗	if (it != --inactive.end()) {
518		✗	os << " " << *it << ": " << *contact_item << std::endl;
519			} else {
520		✗	os << " " << *it << ": " << *contact_item;
521			}
522			}
523		✗	return os;
524			}
525
526			} // namespace crocoddyl
527