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			#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
10			#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
11
12			#include "crocoddyl/core/actuation-base.hpp"
13			#include "crocoddyl/core/constraints/constraint-manager.hpp"
14			#include "crocoddyl/core/costs/cost-sum.hpp"
15			#include "crocoddyl/core/diff-action-base.hpp"
16			#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
17			#include "crocoddyl/multibody/data/contacts.hpp"
18			#include "crocoddyl/multibody/fwd.hpp"
19			#include "crocoddyl/multibody/states/multibody.hpp"
20
21			namespace crocoddyl {
22
23			/**
24			* @brief Differential action model for contact inverse dynamics in multibody
25			* systems.
26			*
27			* This class implements forward kinematic with holonomic contact constraints
28			* (defined at the acceleration level) and inverse-dynamics computation using
29			* the Recursive Newton Euler Algorithm (RNEA). The stack of cost and constraint
30			* functions are implemented in `CostModelSumTpl` and
31			* `ConstraintModelManagerTpl`, respectively. The acceleration and contact
32			* forces are decision variables defined as the control inputs, and the
33			* under-actuation and contact constraint are under the name `tau` and its frame
34			* name, thus the user is not allowed to use it.
35			*
36			* Additionally, it is important to note that `calcDiff()` computes the
37			* derivatives using the latest stored values by `calc()`. Thus, we need to
38			* first run `calc()`.
39			*
40			* \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
41			* `createData()`
42			*/
43			template <typename _Scalar>
44			class DifferentialActionModelContactInvDynamicsTpl
45			: public DifferentialActionModelAbstractTpl<_Scalar> {
46			public:
47			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48		✗	CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
49			DifferentialActionModelContactInvDynamicsTpl)
50
51			typedef _Scalar Scalar;
52			typedef DifferentialActionModelAbstractTpl<Scalar> Base;
53			typedef DifferentialActionDataContactInvDynamicsTpl<Scalar> Data;
54			typedef StateMultibodyTpl<Scalar> StateMultibody;
55			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
56			typedef CostModelSumTpl<Scalar> CostModelSum;
57			typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
58			typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
59			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
60			typedef DifferentialActionDataAbstractTpl<Scalar>
61			DifferentialActionDataAbstract;
62			typedef ContactItemTpl<Scalar> ContactItem;
63			typedef MathBaseTpl<Scalar> MathBase;
64			typedef typename MathBase::VectorXs VectorXs;
65			typedef typename MathBase::MatrixXs MatrixXs;
66
67			/**
68			* @brief Initialize the contact inverse-dynamics action model
69			*
70			* It describes the kinematic evolution of the multibody system with contacts,
71			* and computes the needed torques using inverse-dynamics.
72			*
73			* @param[in] state State of the multibody system
74			* @param[in] actuation Actuation model
75			* @param[in] contacts Multiple contacts
76			* @param[in] costs Cost model
77			*/
78			DifferentialActionModelContactInvDynamicsTpl(
79			std::shared_ptr<StateMultibody> state,
80			std::shared_ptr<ActuationModelAbstract> actuation,
81			std::shared_ptr<ContactModelMultiple> contacts,
82			std::shared_ptr<CostModelSum> costs);
83
84			/**
85			* @brief Initialize the contact inverse-dynamics action model
86			*
87			* @param[in] state State of the multibody system
88			* @param[in] actuation Actuation model
89			* @param[in] contacts Multiple contacts
90			* @param[in] costs Cost model
91			* @param[in] constraints Constraints model
92			*/
93			DifferentialActionModelContactInvDynamicsTpl(
94			std::shared_ptr<StateMultibody> state,
95			std::shared_ptr<ActuationModelAbstract> actuation,
96			std::shared_ptr<ContactModelMultiple> contacts,
97			std::shared_ptr<CostModelSum> costs,
98			std::shared_ptr<ConstraintModelManager> constraints);
99		✗	virtual ~DifferentialActionModelContactInvDynamicsTpl() = default;
100
101			/**
102			* @brief Compute the system acceleration, cost value and constraint residuals
103			*
104			* It extracts the acceleration value from control vector and also computes
105			* the cost and constraints.
106			*
107			* @param[in] data Contact inverse-dynamics data
108			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
109			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
110			*/
111			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
112			const Eigen::Ref<const VectorXs>& x,
113			const Eigen::Ref<const VectorXs>& u) override;
114
115			/**
116			* @brief @copydoc Base::calc(const
117			* std::shared_ptr<DifferentialActionDataAbstract>& data, const
118			* Eigen::Ref<const VectorXs>& x)
119			*/
120			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
121			const Eigen::Ref<const VectorXs>& x) override;
122
123			/**
124			* @brief Compute the derivatives of the dynamics, cost and constraint
125			* functions
126			*
127			* It computes the partial derivatives of the dynamical system and the cost
128			* and contraint functions. It assumes that `calc()` has been run first. This
129			* function builds a quadratic approximation of the time-continuous action
130			* model (i.e., dynamical system, cost and constraint functions).
131			*
132			* @param[in] data Contact inverse-dynamics data
133			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
134			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
135			*/
136			virtual void calcDiff(
137			const std::shared_ptr<DifferentialActionDataAbstract>& data,
138			const Eigen::Ref<const VectorXs>& x,
139			const Eigen::Ref<const VectorXs>& u) override;
140
141			/**
142			* @brief @copydoc Base::calcDiff(const
143			* std::shared_ptr<DifferentialActionDataAbstract>& data, const
144			* Eigen::Ref<const VectorXs>& x)
145			*/
146			virtual void calcDiff(
147			const std::shared_ptr<DifferentialActionDataAbstract>& data,
148			const Eigen::Ref<const VectorXs>& x) override;
149
150			/**
151			* @brief Create the contact inverse-dynamics data
152			*
153			* @return contact inverse-dynamics data
154			*/
155			virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
156
157			/**
158			* @brief Cast the contact-invdyn model to a different scalar type.
159			*
160			* It is useful for operations requiring different precision or scalar types.
161			*
162			* @tparam NewScalar The new scalar type to cast to.
163			* @return DifferentialActionModelContactInvDynamicsTpl<NewScalar> A
164			* differential-action model with the new scalar type.
165			*/
166			template <typename NewScalar>
167			DifferentialActionModelContactInvDynamicsTpl<NewScalar> cast() const;
168
169			/**
170			* @brief Checks that a specific data belongs to the contact inverse-dynamics
171			* model
172			*/
173			virtual bool checkData(
174			const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
175
176			/**
177			* @brief Computes the quasic static commands
178			*
179			* The quasic static commands are the ones produced for a reference posture as
180			* an equilibrium point with zero acceleration, i.e., for
181			* \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
182			*
183			* @param[in] data Contact inverse-dynamics data
184			* @param[out] u Quasic-static commands
185			* @param[in] x State point (velocity has to be zero)
186			* @param[in] maxiter Maximum allowed number of iterations (default 100)
187			* @param[in] tol Tolerance (default 1e-9)
188			*/
189			virtual void quasiStatic(
190			const std::shared_ptr<DifferentialActionDataAbstract>& data,
191			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
192			const std::size_t maxiter = 100,
193			const Scalar tol = Scalar(1e-9)) override;
194
195			/**
196			* @brief Return the number of inequality constraints
197			*/
198			virtual std::size_t get_ng() const override;
199
200			/**
201			* @brief Return the number of equality constraints
202			*/
203			virtual std::size_t get_nh() const override;
204
205			/**
206			* @brief Return the number of equality terminal constraints
207			*/
208			virtual std::size_t get_ng_T() const override;
209
210			/**
211			* @brief Return the number of equality terminal constraints
212			*/
213			virtual std::size_t get_nh_T() const override;
214
215			/**
216			* @brief Return the lower bound of the inequality constraints
217			*/
218			virtual const VectorXs& get_g_lb() const override;
219
220			/**
221			* @brief Return the upper bound of the inequality constraints
222			*/
223			virtual const VectorXs& get_g_ub() const override;
224
225			/**
226			* @brief Return the actuation model
227			*/
228			const std::shared_ptr<ActuationModelAbstract>& get_actuation() const;
229
230			/**
231			* @brief Return the contact model
232			*/
233			const std::shared_ptr<ContactModelMultiple>& get_contacts() const;
234
235			/**
236			* @brief Return the cost model
237			*/
238			const std::shared_ptr<CostModelSum>& get_costs() const;
239
240			/**
241			* @brief Return the constraint model manager
242			*/
243			const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
244
245			/**
246			* @brief Return the Pinocchio model
247			*/
248			pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
249
250			/**
251			* @brief Print relevant information of the contact inverse-dynamics model
252			* @param[out] os Output stream object
253			*/
254			virtual void print(std::ostream& os) const override;
255
256			protected:
257			using Base::g_lb_; //!< Lower bound of the inequality constraints
258			using Base::g_ub_; //!< Upper bound of the inequality constraints
259			using Base::ng_; //!< Number of inequality constraints
260			using Base::nh_; //!< Number of equality constraints
261			using Base::nu_; //!< Control dimension
262			using Base::state_; //!< Model of the state
263
264			private:
265			void init(const std::shared_ptr<StateMultibody>& state);
266			std::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
267			std::shared_ptr<ContactModelMultiple> contacts_; //!< Contact model
268			std::shared_ptr<CostModelSum> costs_; //!< Cost model
269			std::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model
270			pinocchio::ModelTpl<Scalar>* pinocchio_; //!< Pinocchio model
271
272			public:
273			/**
274			* @brief Actuation residual
275			*
276			* This residual function enforces the torques of under-actuated joints (e.g.,
277			* floating-base joints) to be zero. We compute these torques and their
278			* derivatives using RNEA inside
279			* `DifferentialActionModelContactInvDynamicsTpl`.
280			*
281			* As described in `ResidualModelAbstractTpl`, the residual value and its
282			* Jacobians are calculated by `calc` and `calcDiff`, respectively.
283			*
284			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
285			*/
286			class ResidualModelActuation : public ResidualModelAbstractTpl<_Scalar> {
287			public:
288			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
289		✗	CROCODDYL_INNER_DERIVED_CAST(ResidualModelBase,
290			DifferentialActionModelContactInvDynamicsTpl,
291			ResidualModelActuation)
292
293			typedef _Scalar Scalar;
294			typedef MathBaseTpl<Scalar> MathBase;
295			typedef ResidualModelAbstractTpl<Scalar> Base;
296			typedef StateMultibodyTpl<Scalar> StateMultibody;
297			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
298			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
299			typedef typename MathBase::VectorXs VectorXs;
300
301			/**
302			* @brief Initialize the actuation residual model
303			*
304			* @param[in] state State of the multibody system
305			* @param[in] nu Dimension of the joint torques
306			* @param[in] nc Dimension of all the contacts
307			*/
308		✗	ResidualModelActuation(std::shared_ptr<StateMultibody> state,
309			const std::size_t nu, const std::size_t nc)
310		✗	: Base(state, state->get_nv() - nu, state->get_nv() + nc, true, true,
311			true),
312		✗	na_(nu),
313		✗	nc_(nc) {}
314		✗	virtual ~ResidualModelActuation() = default;
315
316			/**
317			* @brief Compute the actuation residual
318			*
319			* @param[in] data Actuation residual data
320			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
321			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nv+nu}\f$
322			*/
323		✗	virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
324			const Eigen::Ref<const VectorXs>&,
325			const Eigen::Ref<const VectorXs>&) override {
326			typename Data::ResidualDataActuation* d =
327		✗	static_cast<typename Data::ResidualDataActuation*>(data.get());
328			// Update the under-actuation set and residual
329		✗	std::size_t nrow = 0;
330		✗	for (std::size_t k = 0;
331		✗	k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
332		✗	if (!d->actuation->tau_set[k]) {
333		✗	data->r(nrow) = d->pinocchio->tau(k);
334		✗	nrow += 1;
335			}
336			}
337		✗	}
338
339			/**
340			* @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
341			* data, const Eigen::Ref<const VectorXs>& x)
342			*/
343		✗	virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
344			const Eigen::Ref<const VectorXs>&) override {
345		✗	data->r.setZero();
346		✗	}
347
348			/**
349			* @brief Compute the derivatives of the actuation residual
350			*
351			* @param[in] data Actuation residual data
352			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
353			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
354			*/
355		✗	virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
356			const Eigen::Ref<const VectorXs>&,
357			const Eigen::Ref<const VectorXs>&) override {
358			typename Data::ResidualDataActuation* d =
359		✗	static_cast<typename Data::ResidualDataActuation*>(data.get());
360		✗	std::size_t nrow = 0;
361		✗	const std::size_t nv = state_->get_nv();
362		✗	d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq;
363		✗	d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv;
364		✗	d->dtau_dx -= d->actuation->dtau_dx;
365		✗	d->dtau_du.leftCols(nv) = d->pinocchio->M;
366		✗	d->dtau_du.rightCols(nc_) = -d->contact->Jc.transpose();
367		✗	for (std::size_t k = 0;
368		✗	k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
369		✗	if (!d->actuation->tau_set[k]) {
370		✗	d->Rx.row(nrow) = d->dtau_dx.row(k);
371		✗	d->Ru.row(nrow) = d->dtau_du.row(k);
372		✗	nrow += 1;
373			}
374			}
375		✗	}
376
377			/**
378			* @brief @copydoc Base::calcDiff(const
379			* std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
380			* VectorXs>& x)
381			*/
382		✗	virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
383			const Eigen::Ref<const VectorXs>&) override {
384		✗	data->Rx.setZero();
385		✗	data->Ru.setZero();
386		✗	}
387
388			/**
389			* @brief Create the actuation residual data
390			*
391			* @return Actuation residual data
392			*/
393		✗	virtual std::shared_ptr<ResidualDataAbstract> createData(
394			DataCollectorAbstract* const data) override {
395			return std::allocate_shared<typename Data::ResidualDataActuation>(
396		✗	Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
397		✗	this, data);
398			}
399
400			/**
401			* @brief Cast the actuation-residual model to a different scalar type.
402			*
403			* It is useful for operations requiring different precision or scalar
404			* types.
405			*
406			* @tparam NewScalar The new scalar type to cast to.
407			* @return
408			* DifferentialActionModelContactInvDynamicsTpl<NewScalar>::ResidualModelActuation
409			* A residual model with the new scalar type.
410			*/
411			template <typename NewScalar>
412			typename DifferentialActionModelContactInvDynamicsTpl<
413			NewScalar>::ResidualModelActuation
414		✗	cast() const {
415			typedef typename DifferentialActionModelContactInvDynamicsTpl<
416			NewScalar>::ResidualModelActuation ReturnType;
417			typedef StateMultibodyTpl<NewScalar> StateType;
418		✗	ReturnType ret(std::static_pointer_cast<StateType>(
419		✗	state_->template cast<NewScalar>()),
420		✗	na_, nc_);
421		✗	return ret;
422			}
423
424			/**
425			* @brief Print relevant information of the actuation residual model
426			*
427			* @param[out] os Output stream object
428			*/
429		✗	virtual void print(std::ostream& os) const override {
430		✗	os << "ResidualModelActuation {nx=" << state_->get_nx()
431		✗	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_
432		✗	<< "}";
433		✗	}
434
435			protected:
436			using Base::nu_;
437			using Base::state_;
438
439			private:
440			std::size_t na_; //!< Number of actuated joints
441			std::size_t nc_; //!< Number of contacts
442			};
443
444			public:
445			/**
446			* @brief Contact-acceleration residual
447			*
448			* This residual function is defined as \f$\mathbf{r} = \mathbf{a_0}\f$, where
449			* \f$\mathbf{a_0}\f$ defines the desired contact acceleration, which might
450			* also include the Baumgarte stabilization gains. Furthermore, the Jacobians
451			* of the residual function are computed analytically. This is used by
452			* `ConstraintModelManagerTpl` inside parent
453			* `DifferentialActionModelContactInvDynamicsTpl` class.
454			*
455			* As described in `ResidualModelAbstractTpl`, the residual value and its
456			* Jacobians are calculated by `calc` and `calcDiff`, respectively.
457			*
458			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
459			*/
460			class ResidualModelContact : public ResidualModelAbstractTpl<_Scalar> {
461			public:
462			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
463		✗	CROCODDYL_INNER_DERIVED_CAST(ResidualModelBase,
464			DifferentialActionModelContactInvDynamicsTpl,
465			ResidualModelContact)
466
467			typedef _Scalar Scalar;
468			typedef MathBaseTpl<Scalar> MathBase;
469			typedef ResidualModelAbstractTpl<Scalar> Base;
470			typedef StateMultibodyTpl<Scalar> StateMultibody;
471			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
472			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
473			typedef typename MathBase::VectorXs VectorXs;
474			typedef typename MathBase::MatrixXs MatrixXs;
475
476			/**
477			* @brief Initialize the contact-acceleration residual model
478			*
479			* @param[in] state State of the multibody system
480			* @param[in] id Contact frame id
481			* @param[in] nr Dimension of the contact-acceleration residual
482			* @param[in] nc Dimension of all contacts
483			*/
484		✗	ResidualModelContact(std::shared_ptr<StateMultibody> state,
485			const pinocchio::FrameIndex id, const std::size_t nr,
486			const std::size_t nc)
487		✗	: Base(state, nr, state->get_nv() + nc, true, true, true),
488		✗	id_(id),
489		✗	frame_name_(state->get_pinocchio()->frames[id].name),
490		✗	nc_(nc) {}
491		✗	virtual ~ResidualModelContact() = default;
492
493			/**
494			* @brief Compute the contact-acceleration residual
495			*
496			* @param[in] data Contact-acceleration residual data
497			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
498			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nv+nu}\f$
499			*/
500		✗	void calc(const std::shared_ptr<ResidualDataAbstract>& data,
501			const Eigen::Ref<const VectorXs>&,
502			const Eigen::Ref<const VectorXs>&) override {
503			typename Data::ResidualDataContact* d =
504		✗	static_cast<typename Data::ResidualDataContact*>(data.get());
505		✗	d->r = d->contact->a0;
506		✗	}
507
508			/**
509			* @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
510			* data, const Eigen::Ref<const VectorXs>& x)
511			*/
512		✗	virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
513			const Eigen::Ref<const VectorXs>&) override {
514		✗	data->r.setZero();
515		✗	}
516
517			/**
518			* @brief Compute the derivatives of the contact-acceleration residual
519			*
520			* @param[in] data Contact-acceleration residual data
521			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
522			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
523			*/
524		✗	void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
525			const Eigen::Ref<const VectorXs>&,
526			const Eigen::Ref<const VectorXs>&) override {
527			typename Data::ResidualDataContact* d =
528		✗	static_cast<typename Data::ResidualDataContact*>(data.get());
529		✗	d->Rx = d->contact->da0_dx;
530		✗	d->Ru.leftCols(state_->get_nv()) = d->contact->Jc;
531		✗	}
532
533			/**
534			* @brief @copydoc Base::calcDiff(const
535			* std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
536			* VectorXs>& x)
537			*/
538		✗	virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
539			const Eigen::Ref<const VectorXs>&) override {
540		✗	data->Rx.setZero();
541		✗	data->Ru.setZero();
542		✗	}
543
544			/**
545			* @brief Create the contact-acceleration residual data
546			*
547			* @return contact-acceleration residual data
548			*/
549		✗	virtual std::shared_ptr<ResidualDataAbstract> createData(
550			DataCollectorAbstract* const data) override {
551			return std::allocate_shared<typename Data::ResidualDataContact>(
552		✗	Eigen::aligned_allocator<typename Data::ResidualDataContact>(), this,
553		✗	data, id_);
554			}
555
556			/**
557			* @brief Cast the contact-residual model to a different scalar type.
558			*
559			* It is useful for operations requiring different precision or scalar
560			* types.
561			*
562			* @tparam NewScalar The new scalar type to cast to.
563			* @return typename
564			* DifferentialActionModelContactInvDynamicsTpl<NewScalar>::ResidualModelContact
565			* A residual model with the new scalar type.
566			*/
567			template <typename NewScalar>
568			typename DifferentialActionModelContactInvDynamicsTpl<
569			NewScalar>::ResidualModelContact
570		✗	cast() const {
571			typedef typename DifferentialActionModelContactInvDynamicsTpl<
572			NewScalar>::ResidualModelContact ReturnType;
573			typedef StateMultibodyTpl<NewScalar> StateType;
574		✗	ReturnType ret(std::static_pointer_cast<StateType>(
575		✗	state_->template cast<NewScalar>()),
576		✗	id_, nr_, nc_);
577		✗	return ret;
578			}
579
580			/**
581			* @brief Print relevant information of the contact-acceleration residual
582			* model
583			*
584			* @param[out] os Output stream object
585			*/
586		✗	virtual void print(std::ostream& os) const override {
587		✗	os << "ResidualModelContact {frame=" << frame_name_ << ", nr=" << nr_
588		✗	<< "}";
589		✗	}
590
591			protected:
592			using Base::nr_;
593			using Base::nu_;
594			using Base::state_;
595
596			private:
597			pinocchio::FrameIndex id_; //!< Reference frame id
598			std::string frame_name_; //!< Reference frame name
599			std::size_t nc_; //!< Dimension of all contacts
600			};
601			};
602			template <typename _Scalar>
603			struct DifferentialActionDataContactInvDynamicsTpl
604			: public DifferentialActionDataAbstractTpl<_Scalar> {
605			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
606			typedef _Scalar Scalar;
607			typedef MathBaseTpl<Scalar> MathBase;
608			typedef DifferentialActionDataAbstractTpl<Scalar> Base;
609			typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
610			typedef DataCollectorJointActMultibodyInContactTpl<Scalar>
611			DataCollectorJointActMultibodyInContact;
612			typedef CostDataSumTpl<Scalar> CostDataSum;
613			typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
614			typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
615			typedef ContactItemTpl<Scalar> ContactItem;
616			typedef typename MathBase::VectorXs VectorXs;
617			typedef typename MathBase::MatrixXs MatrixXs;
618
619			template <template <typename Scalar> class Model>
620		✗	explicit DifferentialActionDataContactInvDynamicsTpl(
621			Model<Scalar>* const model)
622			: Base(model),
623		✗	pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
624		✗	multibody(
625		✗	&pinocchio, model->get_actuation()->createData(),
626			std::make_shared<JointDataAbstract>(
627		✗	model->get_state(), model->get_actuation(), model->get_nu()),
628		✗	model->get_contacts()->createData(&pinocchio)),
629		✗	tmp_xstatic(model->get_state()->get_nx()),
630		✗	tmp_rstatic(model->get_actuation()->get_nu() +
631		✗	model->get_contacts()->get_nc()),
632		✗	tmp_Jstatic(model->get_state()->get_nv(),
633		✗	model->get_actuation()->get_nu() +
634		✗	model->get_contacts()->get_nc()) {
635			// Set constant values for Fu, df_dx, and df_du
636		✗	const std::size_t nv = model->get_state()->get_nv();
637		✗	const std::size_t nc = model->get_contacts()->get_nc_total();
638		✗	Fu.leftCols(nv).diagonal().setOnes();
639		✗	multibody.joint->da_du.leftCols(nv).diagonal().setOnes();
640		✗	MatrixXs df_dx = MatrixXs::Zero(nc, model->get_state()->get_ndx());
641		✗	MatrixXs df_du = MatrixXs::Zero(nc, model->get_nu());
642		✗	std::size_t fid = 0;
643		✗	for (typename ContactModelMultiple::ContactModelContainer::const_iterator
644		✗	it = model->get_contacts()->get_contacts().begin();
645		✗	it != model->get_contacts()->get_contacts().end(); ++it) {
646		✗	const std::size_t nc = it->second->contact->get_nc();
647		✗	df_du.block(fid, nv + fid, nc, nc).diagonal().setOnes();
648		✗	fid += nc;
649			}
650		✗	std::vector<bool> contact_status;
651		✗	for (typename ContactModelMultiple::ContactModelContainer::const_iterator
652		✗	it = model->get_contacts()->get_contacts().begin();
653		✗	it != model->get_contacts()->get_contacts().end(); ++it) {
654		✗	const std::shared_ptr<ContactItem>& m_i = it->second;
655		✗	contact_status.push_back(m_i->active);
656		✗	m_i->active = true;
657			}
658		✗	model->get_contacts()->updateForceDiff(multibody.contacts, df_dx, df_du);
659		✗	std::size_t cid = 0;
660		✗	for (typename ContactModelMultiple::ContactModelContainer::const_iterator
661		✗	it = model->get_contacts()->get_contacts().begin();
662		✗	it != model->get_contacts()->get_contacts().end(); ++it) {
663		✗	const std::shared_ptr<ContactItem>& m_i = it->second;
664		✗	m_i->active = contact_status[cid];
665		✗	cid++;
666			}
667		✗	costs = model->get_costs()->createData(&multibody);
668		✗	constraints = model->get_constraints()->createData(&multibody);
669		✗	costs->shareMemory(this);
670		✗	constraints->shareMemory(this);
671
672		✗	tmp_xstatic.setZero();
673		✗	tmp_rstatic.setZero();
674		✗	tmp_Jstatic.setZero();
675		✗	}
676		✗	virtual ~DifferentialActionDataContactInvDynamicsTpl() = default;
677
678			pinocchio::DataTpl<Scalar> pinocchio; //!< Pinocchio data
679			DataCollectorJointActMultibodyInContact multibody; //!< Multibody data
680			std::shared_ptr<CostDataSum> costs; //!< Costs data
681			std::shared_ptr<ConstraintDataManager> constraints; //!< Constraints data
682			VectorXs
683			tmp_xstatic; //!< State point used for computing the quasi-static input
684			VectorXs
685			tmp_rstatic; //!< Factorization used for computing the quasi-static input
686			MatrixXs tmp_Jstatic; //!< Jacobian used for computing the quasi-static input
687
688			using Base::cost;
689			using Base::Fu;
690			using Base::Fx;
691			using Base::Lu;
692			using Base::Luu;
693			using Base::Lx;
694			using Base::Lxu;
695			using Base::Lxx;
696			using Base::r;
697			using Base::xout;
698
699			struct ResidualDataActuation : public ResidualDataAbstractTpl<_Scalar> {
700			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
701
702			typedef _Scalar Scalar;
703			typedef MathBaseTpl<Scalar> MathBase;
704			typedef ResidualDataAbstractTpl<Scalar> Base;
705			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
706			typedef DataCollectorActMultibodyInContactTpl<Scalar>
707			DataCollectorActMultibodyInContact;
708			typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
709			typedef ContactDataMultipleTpl<Scalar> ContactDataMultiple;
710			typedef typename MathBase::MatrixXs MatrixXs;
711
712			template <template <typename Scalar> class Model>
713		✗	ResidualDataActuation(Model<Scalar>* const model,
714			DataCollectorAbstract* const data)
715			: Base(model, data),
716		✗	dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
717		✗	dtau_du(model->get_state()->get_nv(), model->get_nu()) {
718			// Check that proper shared data has been passed
719		✗	DataCollectorActMultibodyInContact* d =
720		✗	dynamic_cast<DataCollectorActMultibodyInContact*>(shared);
721		✗	if (d == NULL) {
722		✗	throw_pretty(
723			"Invalid argument: the shared data should be derived from "
724			"DataCollectorActMultibodyInContact");
725			}
726			// Avoids data casting at runtime
727		✗	pinocchio = d->pinocchio;
728		✗	actuation = d->actuation;
729		✗	contact = d->contacts;
730		✗	dtau_dx.setZero();
731		✗	dtau_du.setZero();
732		✗	}
733		✗	virtual ~ResidualDataActuation() = default;
734
735			pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
736			std::shared_ptr<ActuationDataAbstract> actuation; //!< Actuation data
737			std::shared_ptr<ContactDataMultiple> contact; //!< Contact data
738			MatrixXs dtau_dx;
739			MatrixXs dtau_du;
740			using Base::r;
741			using Base::Ru;
742			using Base::Rx;
743			using Base::shared;
744			};
745
746			struct ResidualDataContact : public ResidualDataAbstractTpl<_Scalar> {
747			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
748
749			typedef _Scalar Scalar;
750			typedef MathBaseTpl<Scalar> MathBase;
751			typedef ResidualDataAbstractTpl<Scalar> Base;
752			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
753			typedef DataCollectorMultibodyInContactTpl<Scalar>
754			DataCollectorMultibodyInContact;
755			typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
756
757			template <template <typename Scalar> class Model>
758		✗	ResidualDataContact(Model<Scalar>* const model,
759			DataCollectorAbstract* const data, const std::size_t id)
760		✗	: Base(model, data) {
761		✗	DataCollectorMultibodyInContact* d =
762		✗	dynamic_cast<DataCollectorMultibodyInContact*>(shared);
763		✗	if (d == NULL) {
764		✗	throw_pretty(
765			"Invalid argument: the shared data should be derived from "
766			"DataCollectorMultibodyInContact");
767			}
768		✗	typename ContactModelMultiple::ContactDataContainer::iterator it, end;
769		✗	for (it = d->contacts->contacts.begin(),
770		✗	end = d->contacts->contacts.end();
771		✗	it != end; ++it) {
772		✗	if (id == it->second->frame) { // TODO(cmastalli): use model->get_id()
773			// and avoid to pass id in constructor
774		✗	contact = it->second.get();
775		✗	break;
776			}
777			}
778		✗	}
779		✗	virtual ~ResidualDataContact() = default;
780
781			ContactDataAbstractTpl<Scalar>* contact; //!< Contact force data
782			using Base::r;
783			using Base::Ru;
784			using Base::Rx;
785			using Base::shared;
786			};
787			};
788			} // namespace crocoddyl
789
790			/* --- Details -------------------------------------------------------------- */
791			/* --- Details -------------------------------------------------------------- */
792			/* --- Details -------------------------------------------------------------- */
793			#include <crocoddyl/multibody/actions/contact-invdyn.hxx>
794
795			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
796			crocoddyl::DifferentialActionModelContactInvDynamicsTpl)
797			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
798			crocoddyl::DifferentialActionDataContactInvDynamicsTpl)
799
800			#endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
801