GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residuals/joint-effort.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 10 0.0%
Branches: 0 14 0.0%
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