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_FREE_INVDYN_HPP_
10			#define CROCODDYL_MULTIBODY_ACTIONS_FREE_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/core/residual-base.hpp"
17			#include "crocoddyl/multibody/data/multibody.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 free inverse dynamics in multibody
25			* systems.
26			*
27			* This class implements forward kinematic with an inverse-dynamics computed
28			* using the Recursive Newton Euler Algorithm (RNEA). The stack of cost and
29			* constraint functions are implemented in `CostModelSumTpl` and
30			* `ConstraintModelManagerTpl`, respectively. The accelerations are the decision
31			* variables defined as the control inputs, and the under-actuation constraint
32			* is under the name `tau`, thus the user is not allowed to use it.
33			*
34			*
35			* \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`,
36			* `createData()`
37			*/
38			template <typename _Scalar>
39			class DifferentialActionModelFreeInvDynamicsTpl
40			: public DifferentialActionModelAbstractTpl<_Scalar> {
41			public:
42			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43		✗	CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
44			DifferentialActionModelFreeInvDynamicsTpl)
45
46			typedef _Scalar Scalar;
47			typedef MathBaseTpl<Scalar> MathBase;
48			typedef DifferentialActionModelAbstractTpl<Scalar> Base;
49			typedef DifferentialActionDataFreeInvDynamicsTpl<Scalar> Data;
50			typedef DifferentialActionDataAbstractTpl<Scalar>
51			DifferentialActionDataAbstract;
52			typedef CostModelSumTpl<Scalar> CostModelSum;
53			typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager;
54			typedef StateMultibodyTpl<Scalar> StateMultibody;
55			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
56			typedef ConstraintModelResidualTpl<Scalar> ConstraintModelResidual;
57			typedef typename MathBase::VectorXs VectorXs;
58
59			/**
60			* @brief Initialize the free inverse-dynamics action model
61			*
62			* It describes the kinematic evolution of the multibody system and computes
63			* the needed torques using inverse dynamics.
64			*
65			* @param[in] state State of the multibody system
66			* @param[in] actuation Actuation model
67			* @param[in] costs Cost model
68			*/
69			DifferentialActionModelFreeInvDynamicsTpl(
70			std::shared_ptr<StateMultibody> state,
71			std::shared_ptr<ActuationModelAbstract> actuation,
72			std::shared_ptr<CostModelSum> costs);
73
74			/**
75			* @brief Initialize the free inverse-dynamics action model
76			*
77			* @param[in] state State of the multibody system
78			* @param[in] actuation Actuation model
79			* @param[in] costs Cost model
80			* @param[in] constraints Constraints model
81			*/
82			DifferentialActionModelFreeInvDynamicsTpl(
83			std::shared_ptr<StateMultibody> state,
84			std::shared_ptr<ActuationModelAbstract> actuation,
85			std::shared_ptr<CostModelSum> costs,
86			std::shared_ptr<ConstraintModelManager> constraints);
87		✗	virtual ~DifferentialActionModelFreeInvDynamicsTpl() = default;
88
89			/**
90			* @brief Compute the system acceleration, cost value and constraint residuals
91			*
92			* It extracts the acceleration value from control vector and also computes
93			* the cost and constraints.
94			*
95			* @param[in] data Free inverse-dynamics data
96			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
97			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
98			*/
99			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
100			const Eigen::Ref<const VectorXs>& x,
101			const Eigen::Ref<const VectorXs>& u) override;
102
103			/**
104			* @brief @copydoc Base::calc(const
105			* std::shared_ptr<DifferentialActionDataAbstract>& data, const
106			* Eigen::Ref<const VectorXs>& x)
107			*/
108			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
109			const Eigen::Ref<const VectorXs>& x) override;
110
111			/**
112			* @brief Compute the derivatives of the dynamics, cost and constraint
113			* functions
114			*
115			* It computes the partial derivatives of the dynamical system and the cost
116			* and contraint functions. It assumes that `calc()` has been run first. This
117			* function builds a quadratic approximation of the time-continuous action
118			* model (i.e., dynamical system, cost and constraint functions).
119			*
120			* @param[in] data Free inverse-dynamics data
121			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
122			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
123			*/
124			virtual void calcDiff(
125			const std::shared_ptr<DifferentialActionDataAbstract>& data,
126			const Eigen::Ref<const VectorXs>& x,
127			const Eigen::Ref<const VectorXs>& u) override;
128
129			/**
130			* @brief @copydoc Base::calcDiff(const
131			* std::shared_ptr<DifferentialActionDataAbstract>& data, const
132			* Eigen::Ref<const VectorXs>& x)
133			*/
134			virtual void calcDiff(
135			const std::shared_ptr<DifferentialActionDataAbstract>& data,
136			const Eigen::Ref<const VectorXs>& x) override;
137
138			/**
139			* @brief Create the free inverse-dynamics data
140			*
141			* @return free inverse-dynamics data
142			*/
143			virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
144
145			/**
146			* @brief Cast the free-invdyn model to a different scalar type.
147			*
148			* It is useful for operations requiring different precision or scalar types.
149			*
150			* @tparam NewScalar The new scalar type to cast to.
151			* @return DifferentialActionModelFreeInvDynamicsTpl<NewScalar> A
152			* differential-action model with the new scalar type.
153			*/
154			template <typename NewScalar>
155			DifferentialActionModelFreeInvDynamicsTpl<NewScalar> cast() const;
156
157			/**
158			* @brief Checks that a specific data belongs to the free inverse-dynamics
159			* model
160			*/
161			virtual bool checkData(
162			const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
163
164			/**
165			* @brief Computes the quasic static commands
166			*
167			* The quasic static commands are the ones produced for a reference posture as
168			* an equilibrium point with zero acceleration, i.e., for
169			* \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
170			*
171			* @param[in] data Free inverse-dynamics data
172			* @param[out] u Quasic-static commands
173			* @param[in] x State point (velocity has to be zero)
174			* @param[in] maxiter Maximum allowed number of iterations (default 100)
175			* @param[in] tol Tolerance (default 1e-9)
176			*/
177			virtual void quasiStatic(
178			const std::shared_ptr<DifferentialActionDataAbstract>& data,
179			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
180			const std::size_t maxiter = 100,
181			const Scalar tol = Scalar(1e-9)) override;
182
183			/**
184			* @brief Return the number of inequality constraints
185			*/
186			virtual std::size_t get_ng() const override;
187
188			/**
189			* @brief Return the number of equality constraints
190			*/
191			virtual std::size_t get_nh() const override;
192
193			/**
194			* @brief Return the number of equality terminal constraints
195			*/
196			virtual std::size_t get_ng_T() const override;
197
198			/**
199			* @brief Return the number of equality terminal constraints
200			*/
201			virtual std::size_t get_nh_T() const override;
202
203			/**
204			* @brief Return the lower bound of the inequality constraints
205			*/
206			virtual const VectorXs& get_g_lb() const override;
207
208			/**
209			* @brief Return the upper bound of the inequality constraints
210			*/
211			virtual const VectorXs& get_g_ub() const override;
212
213			/**
214			* @brief Return the actuation model
215			*/
216			const std::shared_ptr<ActuationModelAbstract>& get_actuation() const;
217
218			/**
219			* @brief Return the cost model
220			*/
221			const std::shared_ptr<CostModelSum>& get_costs() const;
222
223			/**
224			* @brief Return the constraint model manager
225			*/
226			const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
227
228			/**
229			* @brief Return the Pinocchio model
230			*/
231			pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
232
233			/**
234			* @brief Print relevant information of the free inverse-dynamics model
235			*
236			* @param[out] os Output stream object
237			*/
238			virtual void print(std::ostream& os) const override;
239
240			protected:
241			using Base::g_lb_; //!< Lower bound of the inequality constraints
242			using Base::g_ub_; //!< Upper bound of the inequality constraints
243			using Base::ng_; //!< Number of inequality constraints
244			using Base::nh_; //!< Number of equality constraints
245			using Base::nu_; //!< Control dimension
246			using Base::state_; //!< Model of the state
247
248			private:
249			void init(const std::shared_ptr<StateMultibody>& state);
250			std::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
251			std::shared_ptr<CostModelSum> costs_; //!< Cost model
252			std::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model
253			pinocchio::ModelTpl<Scalar>* pinocchio_; //!< Pinocchio model
254
255			public:
256			/**
257			* @brief Actuation residual
258			*
259			* This residual function enforces the torques of under-actuated joints (e.g.,
260			* floating-base joints) to be zero. We compute these torques and their
261			* derivatives using RNEA inside `DifferentialActionModelFreeInvDynamicsTpl`.
262			*
263			* As described in `ResidualModelAbstractTpl`, the residual value and its
264			* Jacobians are calculated by `calc` and `calcDiff`, respectively.
265			*
266			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
267			*/
268			class ResidualModelActuation : public ResidualModelAbstractTpl<_Scalar> {
269			public:
270			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
271		✗	CROCODDYL_INNER_DERIVED_CAST(ResidualModelBase,
272			DifferentialActionModelFreeInvDynamicsTpl,
273			ResidualModelActuation)
274
275			typedef _Scalar Scalar;
276			typedef MathBaseTpl<Scalar> MathBase;
277			typedef ResidualModelAbstractTpl<Scalar> Base;
278			typedef StateMultibodyTpl<Scalar> StateMultibody;
279			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
280			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
281			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
282			typedef typename MathBase::VectorXs VectorXs;
283			typedef typename MathBase::MatrixXs MatrixXs;
284
285			/**
286			* @brief Initialize the actuation residual model
287			*
288			* @param[in] state State of the multibody system
289			* @param[in] nu Dimension of the joint torques
290			*/
291		✗	ResidualModelActuation(std::shared_ptr<StateMultibody> state,
292			const std::size_t nu)
293		✗	: Base(state, state->get_nv() - nu, state->get_nv(), true, true, true),
294		✗	na_(nu) {}
295		✗	virtual ~ResidualModelActuation() = default;
296
297			/**
298			* @brief Compute the actuation residual
299			*
300			* @param[in] data Actuation residual data
301			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
302			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nv+nu}\f$
303			*/
304		✗	virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
305			const Eigen::Ref<const VectorXs>&,
306			const Eigen::Ref<const VectorXs>&) override {
307			typename Data::ResidualDataActuation* d =
308		✗	static_cast<typename Data::ResidualDataActuation*>(data.get());
309			// Update the under-actuation set and residual
310		✗	std::size_t nrow = 0;
311		✗	for (std::size_t k = 0;
312		✗	k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
313		✗	if (!d->actuation->tau_set[k]) {
314		✗	data->r(nrow) = d->pinocchio->tau(k);
315		✗	nrow += 1;
316			}
317			}
318		✗	}
319
320			/**
321			* @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
322			* data, const Eigen::Ref<const VectorXs>& x)
323			*/
324		✗	virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
325			const Eigen::Ref<const VectorXs>&) override {
326		✗	data->r.setZero();
327		✗	}
328
329			/**
330			* @brief Compute the derivatives of the actuation residual
331			*
332			* @param[in] data Actuation residual data
333			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
334			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
335			*/
336		✗	virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
337			const Eigen::Ref<const VectorXs>&,
338			const Eigen::Ref<const VectorXs>&) override {
339			typename Data::ResidualDataActuation* d =
340		✗	static_cast<typename Data::ResidualDataActuation*>(data.get());
341		✗	std::size_t nrow = 0;
342		✗	const std::size_t nv = state_->get_nv();
343		✗	d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq;
344		✗	d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv;
345		✗	d->dtau_dx -= d->actuation->dtau_dx;
346		✗	for (std::size_t k = 0;
347		✗	k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
348		✗	if (!d->actuation->tau_set[k]) {
349		✗	d->Rx.row(nrow) = d->dtau_dx.row(k);
350		✗	d->Ru.row(nrow) = d->pinocchio->M.row(k);
351		✗	nrow += 1;
352			}
353			}
354		✗	}
355
356			/**
357			* @brief @copydoc Base::calcDiff(const
358			* std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
359			* VectorXs>& x)
360			*/
361		✗	virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
362			const Eigen::Ref<const VectorXs>&) override {
363		✗	data->Rx.setZero();
364		✗	data->Ru.setZero();
365		✗	}
366
367			/**
368			* @brief Create the actuation residual data
369			*
370			* @return Actuation residual data
371			*/
372		✗	virtual std::shared_ptr<ResidualDataAbstract> createData(
373			DataCollectorAbstract* const data) override {
374			return std::allocate_shared<typename Data::ResidualDataActuation>(
375		✗	Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
376		✗	this, data);
377			}
378
379			/**
380			* @brief Cast the actuation-residual model to a different scalar type.
381			*
382			* It is useful for operations requiring different precision or scalar
383			* types.
384			*
385			* @tparam NewScalar The new scalar type to cast to.
386			* @return typename
387			* DifferentialActionModelFreeInvDynamicsTpl<NewScalar>::ResidualModelActuation
388			* A residual model with the new scalar type.
389			*/
390			template <typename NewScalar>
391			typename DifferentialActionModelFreeInvDynamicsTpl<
392			NewScalar>::ResidualModelActuation
393		✗	cast() const {
394			typedef typename DifferentialActionModelFreeInvDynamicsTpl<
395			NewScalar>::ResidualModelActuation ReturnType;
396			typedef StateMultibodyTpl<NewScalar> StateType;
397		✗	ReturnType ret(std::static_pointer_cast<StateType>(
398		✗	state_->template cast<NewScalar>()),
399		✗	na_);
400		✗	return ret;
401			}
402
403			/**
404			* @brief Print relevant information of the actuation residual model
405			*
406			* @param[out] os Output stream object
407			*/
408		✗	virtual void print(std::ostream& os) const override {
409		✗	os << "ResidualModelActuation {nx=" << state_->get_nx()
410		✗	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_
411		✗	<< "}";
412		✗	}
413
414			protected:
415			std::size_t na_; //!< Dimension of the joint torques
416			using Base::nu_;
417			using Base::state_;
418			};
419			};
420
421			template <typename _Scalar>
422			struct DifferentialActionDataFreeInvDynamicsTpl
423			: public DifferentialActionDataAbstractTpl<_Scalar> {
424			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
425			typedef _Scalar Scalar;
426			typedef MathBaseTpl<Scalar> MathBase;
427			typedef DifferentialActionDataAbstractTpl<Scalar> Base;
428			typedef JointDataAbstractTpl<Scalar> JointDataAbstract;
429			typedef DataCollectorJointActMultibodyTpl<Scalar>
430			DataCollectorJointActMultibody;
431			typedef CostDataSumTpl<Scalar> CostDataSum;
432			typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
433			typedef typename MathBase::VectorXs VectorXs;
434
435			template <template <typename Scalar> class Model>
436		✗	explicit DifferentialActionDataFreeInvDynamicsTpl(Model<Scalar>* const model)
437			: Base(model),
438		✗	pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
439		✗	multibody(
440		✗	&pinocchio, model->get_actuation()->createData(),
441			std::make_shared<JointDataAbstract>(
442		✗	model->get_state(), model->get_actuation(), model->get_nu())),
443		✗	costs(model->get_costs()->createData(&multibody)),
444		✗	constraints(model->get_constraints()->createData(&multibody)),
445		✗	tmp_xstatic(model->get_state()->get_nx()) {
446		✗	const std::size_t nv = model->get_state()->get_nv();
447		✗	Fu.leftCols(nv).diagonal().setOnes();
448		✗	multibody.joint->da_du.leftCols(nv).diagonal().setOnes();
449		✗	costs->shareMemory(this);
450		✗	constraints->shareMemory(this);
451		✗	tmp_xstatic.setZero();
452		✗	}
453		✗	virtual ~DifferentialActionDataFreeInvDynamicsTpl() = default;
454
455			pinocchio::DataTpl<Scalar> pinocchio; //!< Pinocchio data
456			DataCollectorJointActMultibody multibody; //!< Multibody data
457			std::shared_ptr<CostDataSum> costs; //!< Costs data
458			std::shared_ptr<ConstraintDataManager> constraints; //!< Constraints data
459			VectorXs
460			tmp_xstatic; //!< State point used for computing the quasi-static input
461			using Base::cost;
462			using Base::Fu;
463			using Base::Fx;
464			using Base::Lu;
465			using Base::Luu;
466			using Base::Lx;
467			using Base::Lxu;
468			using Base::Lxx;
469			using Base::r;
470			using Base::xout;
471
472			struct ResidualDataActuation : public ResidualDataAbstractTpl<_Scalar> {
473			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
474
475			typedef _Scalar Scalar;
476			typedef MathBaseTpl<Scalar> MathBase;
477			typedef ResidualDataAbstractTpl<Scalar> Base;
478			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
479			typedef DataCollectorActMultibodyTpl<Scalar> DataCollectorActMultibody;
480			typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
481			typedef typename MathBase::MatrixXs MatrixXs;
482
483			template <template <typename Scalar> class Model>
484		✗	ResidualDataActuation(Model<Scalar>* const model,
485			DataCollectorAbstract* const data)
486			: Base(model, data),
487		✗	dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()) {
488		✗	dtau_dx.setZero();
489			// Check that proper shared data has been passed
490		✗	DataCollectorActMultibody* d =
491		✗	dynamic_cast<DataCollectorActMultibody*>(shared);
492		✗	if (d == NULL) {
493		✗	throw_pretty(
494			"Invalid argument: the shared data should be derived from "
495			"DataCollectorActMultibody");
496			}
497
498			// Avoids data casting at runtime
499		✗	pinocchio = d->pinocchio;
500		✗	actuation = d->actuation;
501		✗	}
502
503			pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
504			std::shared_ptr<ActuationDataAbstract> actuation; //!< Actuation data
505			MatrixXs dtau_dx;
506			using Base::r;
507			using Base::Ru;
508			using Base::Rx;
509			using Base::shared;
510			};
511			};
512
513			} // namespace crocoddyl
514
515			/* --- Details -------------------------------------------------------------- */
516			/* --- Details -------------------------------------------------------------- */
517			/* --- Details -------------------------------------------------------------- */
518			#include <crocoddyl/multibody/actions/free-invdyn.hxx>
519
520			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
521			crocoddyl::DifferentialActionModelFreeInvDynamicsTpl)
522			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
523			crocoddyl::DifferentialActionDataFreeInvDynamicsTpl)
524
525			#endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
526