GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2022-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_CORE_RESIDUALS_JOINT_TORQUE_HPP_
10			#define CROCODDYL_CORE_RESIDUALS_JOINT_TORQUE_HPP_
11
12			#include "crocoddyl/core/actuation-base.hpp"
13			#include "crocoddyl/core/data/joint.hpp"
14			#include "crocoddyl/core/fwd.hpp"
15			#include "crocoddyl/core/residual-base.hpp"
16
17			namespace crocoddyl {
18
19			/**
20			* @brief Define a joint-effort residual
21			*
22			* This residual function is defined as
23			* \f$\mathbf{r}=\mathbf{u}-\mathbf{u}^*\f$, where
24			* \f$\mathbf{u},\mathbf{u}^*\in~\mathbb{R}^{nu}\f$ are the current and
25			* reference joint effort inputs, respectively. Note that the dimension of the
26			* residual vector is obtained from `ActuationModelAbstract::nu`.
27			*
28			* Both residual and residual Jacobians are computed analytically.
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 ResidualModelJointEffortTpl : public ResidualModelAbstractTpl<_Scalar> {
37			public:
38			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39		✗	CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelJointEffortTpl)
40
41			typedef _Scalar Scalar;
42			typedef MathBaseTpl<Scalar> MathBase;
43			typedef ResidualModelAbstractTpl<Scalar> Base;
44			typedef ResidualDataJointEffortTpl<Scalar> Data;
45			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
46			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
47			typedef StateAbstractTpl<Scalar> StateAbstract;
48			typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
49			typedef typename MathBase::VectorXs VectorXs;
50			typedef typename MathBase::MatrixXs MatrixXs;
51
52			/**
53			* @brief Initialize the joint-effort residual model
54			*
55			* @param[in] state State description
56			* @param[in] actuation Actuation model
57			* @param[in] uref Reference joint effort
58			* @param[in] nu Dimension of the control vector
59			* @param[in] fwddyn Indicates that we have a forward dynamics problem
60			* (true) or inverse dynamics (false) (default false)
61			*/
62			ResidualModelJointEffortTpl(std::shared_ptr<StateAbstract> state,
63			std::shared_ptr<ActuationModelAbstract> actuation,
64			const VectorXs& uref, const std::size_t nu,
65			const bool fwddyn = false);
66
67			/**
68			* @brief Initialize the joint-effort residual model
69			*
70			* The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
71			*
72			* @param[in] state State description
73			* @param[in] actuation Actuation model
74			* @param[in] uref Reference joint effort
75			*/
76			ResidualModelJointEffortTpl(std::shared_ptr<StateAbstract> state,
77			std::shared_ptr<ActuationModelAbstract> actuation,
78			const VectorXs& uref);
79
80			/**
81			* @brief Initialize the joint-effort residual model
82			*
83			* The default reference joint effort is obtained from
84			* `MathBaseTpl<>::VectorXs::Zero(actuation->get_nu())`.
85			*
86			* @param[in] state State description
87			* @param[in] actuation Actuation model
88			* @param[in] nu Dimension of the control vector
89			*/
90			ResidualModelJointEffortTpl(std::shared_ptr<StateAbstract> state,
91			std::shared_ptr<ActuationModelAbstract> actuation,
92			const std::size_t nu);
93
94			/**
95			* @brief Initialize the joint-effort residual model
96			*
97			* The default reference joint effort is obtained from
98			* `MathBaseTpl<>::VectorXs::Zero(actuation->get_nu())`. The default `nu`
99			* value is obtained from `StateAbstractTpl::get_nv()`.
100			*
101			* @param[in] state State description
102			* @param[in] actuation Actuation model
103			*/
104			ResidualModelJointEffortTpl(
105			std::shared_ptr<StateAbstract> state,
106			std::shared_ptr<ActuationModelAbstract> actuation);
107
108		✗	virtual ~ResidualModelJointEffortTpl() = default;
109
110			/**
111			* @brief Compute the joint-effort residual
112			*
113			* @param[in] data Joint-effort residual data
114			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
115			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
116			*/
117			virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
118			const Eigen::Ref<const VectorXs>& x,
119			const Eigen::Ref<const VectorXs>& u) override;
120
121			/**
122			* @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
123			* data, const Eigen::Ref<const VectorXs>& x)
124			*/
125			virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
126			const Eigen::Ref<const VectorXs>& x) override;
127
128			/**
129			* @brief Compute the derivatives of the joint-effort residual
130			*
131			* @param[in] data Joint-effort residual data
132			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
133			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
134			*/
135			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
136			const Eigen::Ref<const VectorXs>& x,
137			const Eigen::Ref<const VectorXs>& u) override;
138
139			/**
140			* @brief @copydoc Base::calcDiff(const
141			* std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
142			* VectorXs>& x)
143			*/
144			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
145			const Eigen::Ref<const VectorXs>& x) override;
146
147			/**
148			* @brief Create the joint-effort residual data
149			*/
150			virtual std::shared_ptr<ResidualDataAbstract> createData(
151			DataCollectorAbstract* const data) override;
152
153			/**
154			* @brief Cast the joint-effort residual model to a different scalar type.
155			*
156			* It is useful for operations requiring different precision or scalar types.
157			*
158			* @tparam NewScalar The new scalar type to cast to.
159			* @return ResidualModelJointEffortTpl<NewScalar> A residual model with the
160			* new scalar type.
161			*/
162			template <typename NewScalar>
163			ResidualModelJointEffortTpl<NewScalar> cast() const;
164
165			/**
166			* @brief Return the reference joint-effort vector
167			*/
168			const VectorXs& get_reference() const;
169
170			/**
171			* @brief Modify the reference joint-effort vector
172			*/
173			void set_reference(const VectorXs& reference);
174
175			/**
176			* @brief Print relevant information of the joint-effort residual
177			*
178			* @param[out] os Output stream object
179			*/
180			virtual void print(std::ostream& os) const override;
181
182			protected:
183			using Base::nr_;
184			using Base::nu_;
185			using Base::q_dependent_;
186			using Base::state_;
187			using Base::v_dependent_;
188
189			private:
190			std::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model
191			VectorXs uref_; //!< Reference joint-effort input
192			bool fwddyn_; //!< True for forward dynamics, False for inverse dynamics
193			};
194
195			template <typename _Scalar>
196			struct ResidualDataJointEffortTpl : public ResidualDataAbstractTpl<_Scalar> {
197			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
198
199			typedef _Scalar Scalar;
200			typedef MathBaseTpl<Scalar> MathBase;
201			typedef ResidualDataAbstractTpl<Scalar> Base;
202			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
203
204			template <template <typename Scalar> class Model>
205		✗	ResidualDataJointEffortTpl(Model<Scalar>* const model,
206			DataCollectorAbstract* const data)
207		✗	: Base(model, data) {
208			// Check that proper shared data has been passed
209		✗	DataCollectorJointTpl<Scalar>* d =
210		✗	dynamic_cast<DataCollectorJointTpl<Scalar>*>(shared);
211		✗	if (d == nullptr) {
212		✗	throw_pretty(
213			"Invalid argument: the shared data should be derived from "
214			"DataCollectorJoint");
215			}
216		✗	joint = d->joint;
217		✗	}
218		✗	virtual ~ResidualDataJointEffortTpl() = default;
219
220			std::shared_ptr<JointDataAbstractTpl<Scalar> > joint; //!< Joint data
221			using Base::r;
222			using Base::Ru;
223			using Base::Rx;
224			using Base::shared;
225			};
226
227			} // namespace crocoddyl
228
229			/* --- Details -------------------------------------------------------------- */
230			/* --- Details -------------------------------------------------------------- */
231			/* --- Details -------------------------------------------------------------- */
232			#include "crocoddyl/core/residuals/joint-effort.hxx"
233
234			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelJointEffortTpl)
235			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataJointEffortTpl)
236
237			#endif // CROCODDYL_CORE_RESIDUALS_JOINT_TORQUE_HPP_
238