GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2020-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			#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
11			#define CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
12
13			#include "crocoddyl/core/residual-base.hpp"
14			#include "crocoddyl/multibody/data/multibody.hpp"
15			#include "crocoddyl/multibody/states/multibody.hpp"
16
17			namespace crocoddyl {
18
19			/**
20			* @brief Control gravity residual
21			*
22			* This residual function is defined as
23			* \f$\mathbf{r}=\mathbf{u}-\mathbf{g}(\mathbf{q})\f$, where
24			* \f$\mathbf{u}\in~\mathbb{R}^{nu}\f$ is the current control input,
25			* \f$\mathbf{g}(\mathbf{q})\f$ is the gravity torque corresponding to the
26			* current configuration, \f$\mathbf{q}\in~\mathbb{R}^{nq}\f$ the current
27			* position joints input. Note that the dimension of the residual vector is
28			* obtained from `StateAbstractTpl::get_nv()`.
29			*
30			* As described in `ResidualModelAbstractTpl()`, the residual value and its
31			* Jacobians are calculated by `calc()` and `calcDiff()`, respectively.
32			*
33			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
34			*/
35			template <typename _Scalar>
36			class ResidualModelControlGravTpl : public ResidualModelAbstractTpl<_Scalar> {
37			public:
38			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39		✗	CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelControlGravTpl)
40
41			typedef _Scalar Scalar;
42			typedef MathBaseTpl<Scalar> MathBase;
43			typedef ResidualModelAbstractTpl<Scalar> Base;
44			typedef ResidualDataControlGravTpl<Scalar> Data;
45			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
46			typedef StateMultibodyTpl<Scalar> StateMultibody;
47			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
48			typedef typename MathBase::VectorXs VectorXs;
49			typedef typename MathBase::MatrixXs MatrixXs;
50
51			/**
52			* @brief Initialize the control gravity residual model
53			*
54			* @param[in] state State of the multibody system
55			* @param[in] nu Dimension of control vector
56			*/
57			ResidualModelControlGravTpl(std::shared_ptr<StateMultibody> state,
58			const std::size_t nu);
59
60			/**
61			* @brief Initialize the control gravity residual model
62			*
63			* The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
64			*
65			* @param[in] state State of the multibody system
66			*/
67			ResidualModelControlGravTpl(std::shared_ptr<StateMultibody> state);
68		204	virtual ~ResidualModelControlGravTpl() = default;
69
70			/**
71			* @brief Compute the control gravity residual
72			*
73			* @param[in] data Control residual data
74			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
75			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
76			*/
77			virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
78			const Eigen::Ref<const VectorXs> &x,
79			const Eigen::Ref<const VectorXs> &u) override;
80
81			/**
82			* @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
83			* data, const Eigen::Ref<const VectorXs>& x)
84			*/
85			virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
86			const Eigen::Ref<const VectorXs> &x) override;
87
88			/**
89			* @brief Compute the Jacobians of the control gravity residual
90			*
91			* @param[in] data Control residual data
92			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
93			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
94			*/
95			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data,
96			const Eigen::Ref<const VectorXs> &x,
97			const Eigen::Ref<const VectorXs> &u) override;
98
99			/**
100			* @brief @copydoc Base::calcDiff(const
101			* std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
102			* VectorXs>& x)
103			*/
104			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data,
105			const Eigen::Ref<const VectorXs> &x) override;
106
107			virtual std::shared_ptr<ResidualDataAbstract> createData(
108			DataCollectorAbstract *const data) override;
109
110			/**
111			* @brief Cast the control-gravity residual model to a different scalar type.
112			*
113			* It is useful for operations requiring different precision or scalar types.
114			*
115			* @tparam NewScalar The new scalar type to cast to.
116			* @return ResidualModelControlGravTpl<NewScalar> A residual model with the
117			* new scalar type.
118			*/
119			template <typename NewScalar>
120			ResidualModelControlGravTpl<NewScalar> cast() const;
121
122			/**
123			* @brief Print relevant information of the control-grav residual
124			*
125			* @param[out] os Output stream object
126			*/
127			virtual void print(std::ostream &os) const override;
128
129			protected:
130			using Base::nu_;
131			using Base::state_;
132			using Base::v_dependent_;
133
134			private:
135			typename StateMultibody::PinocchioModel
136			pin_model_; //!< Pinocchio model used for internal computations
137			};
138
139			template <typename _Scalar>
140			struct ResidualDataControlGravTpl : public ResidualDataAbstractTpl<_Scalar> {
141			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142
143			typedef _Scalar Scalar;
144			typedef MathBaseTpl<Scalar> MathBase;
145			typedef ResidualDataAbstractTpl<Scalar> Base;
146			typedef StateMultibodyTpl<Scalar> StateMultibody;
147			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
148			typedef pinocchio::DataTpl<Scalar> PinocchioData;
149
150			template <template <typename Scalar> class Model>
151		2847	ResidualDataControlGravTpl(Model<Scalar> *const model,
152			DataCollectorAbstract *const data)
153	1/2 ✓ Branch 2 taken 2845 times. ✗ Branch 3 not taken.	2847	: Base(model, data) {
154			// Check that proper shared data has been passed
155		2847	DataCollectorActMultibodyTpl<Scalar> *d =
156	1/2 ✓ Branch 0 taken 2845 times. ✗ Branch 1 not taken.	2847	dynamic_cast<DataCollectorActMultibodyTpl<Scalar> *>(shared);
157	1/2 ✗ Branch 0 not taken. ✓ Branch 1 taken 2845 times.	2847	if (d == NULL) {
158		✗	throw_pretty(
159			"Invalid argument: the shared data should be derived from "
160			"DataCollectorActMultibodyTpl");
161			}
162			// Avoids data casting at runtime
163			StateMultibody *sm =
164	1/2 ✓ Branch 1 taken 2845 times. ✗ Branch 2 not taken.	2847	static_cast<StateMultibody *>(model->get_state().get());
165	3/6 ✓ Branch 1 taken 2845 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 2845 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 2845 times. ✗ Branch 9 not taken.	2847	pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
166		2847	actuation = d->actuation;
167		2847	}
168		5690	virtual ~ResidualDataControlGravTpl() = default;
169
170			PinocchioData pinocchio; //!< Pinocchio data
171			std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
172			actuation; //!< Actuation data
173			using Base::r;
174			using Base::Ru;
175			using Base::Rx;
176			using Base::shared;
177			};
178
179			} // namespace crocoddyl
180
181			/* --- Details -------------------------------------------------------------- */
182			/* --- Details -------------------------------------------------------------- */
183			/* --- Details -------------------------------------------------------------- */
184			#include "crocoddyl/multibody/residuals/control-gravity.hxx"
185
186			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelControlGravTpl)
187			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataControlGravTpl)
188
189			#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
190