GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residuals/joint-acceleration.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
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_CORE_RESIDUALS_JOINT_ACCELERATION_HPP_
10 #define CROCODDYL_CORE_RESIDUALS_JOINT_ACCELERATION_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-acceleration 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 acceleration, respectively. Note that the dimension of the
26 * residual vector is obtained from `StateAbstract::nv`, as it represents the
27 * generalized acceleration.
28 *
29 * Both residual and residual Jacobians are computed analytically.
30 *
31 * As described in ResidualModelAbstractTpl(), the residual value and its
32 * Jacobians are calculated by `calc` and `calcDiff`, respectively.
33 *
34 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
35 */
36 template <typename _Scalar>
37 class ResidualModelJointAccelerationTpl
38 : public ResidualModelAbstractTpl<_Scalar> {
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelJointAccelerationTpl)
42
43 typedef _Scalar Scalar;
44 typedef MathBaseTpl<Scalar> MathBase;
45 typedef ResidualModelAbstractTpl<Scalar> Base;
46 typedef ResidualDataJointAccelerationTpl<Scalar> Data;
47 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
48 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
49 typedef StateAbstractTpl<Scalar> StateAbstract;
50 typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::MatrixXs MatrixXs;
53
54 /**
55 * @brief Initialize the joint-acceleration residual model
56 *
57 * @param[in] state State description
58 * @param[in] aref Reference joint acceleration
59 * @param[in] nu Dimension of the control vector
60 */
61 ResidualModelJointAccelerationTpl(std::shared_ptr<StateAbstract> state,
62 const VectorXs& aref, const std::size_t nu);
63
64 /**
65 * @brief Initialize the joint-acceleration residual model
66 *
67 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
68 *
69 * @param[in] state State description
70 * @param[in] aref Reference joint acceleration
71 */
72 ResidualModelJointAccelerationTpl(std::shared_ptr<StateAbstract> state,
73 const VectorXs& aref);
74
75 /**
76 * @brief Initialize the joint-acceleration residual model
77 *
78 * The default reference joint acceleration is obtained from
79 * `MathBaseTpl<>::VectorXs::Zero(state->get_nv())`.
80 *
81 * @param[in] state State description
82 * @param[in] nu Dimension of the control vector
83 */
84 ResidualModelJointAccelerationTpl(std::shared_ptr<StateAbstract> state,
85 const std::size_t nu);
86
87 /**
88 * @brief Initialize the joint-acceleration residual model
89 *
90 * The default reference joint acceleration is obtained from
91 * `MathBaseTpl<>::VectorXs::Zero(state->get_nv())`. The default `nu` value is
92 * obtained from `StateAbstractTpl::get_nv()`.
93 *
94 * @param[in] state State description
95 */
96 ResidualModelJointAccelerationTpl(std::shared_ptr<StateAbstract> state);
97
98 ✗ virtual ~ResidualModelJointAccelerationTpl() = default;
99
100 /**
101 * @brief Compute the joint-acceleration residual
102 *
103 * @param[in] data Joint-acceleration residual data
104 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
105 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
106 */
107 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
108 const Eigen::Ref<const VectorXs>& x,
109 const Eigen::Ref<const VectorXs>& u) override;
110
111 /**
112 * @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
113 * data, const Eigen::Ref<const VectorXs>& x)
114 */
115 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
116 const Eigen::Ref<const VectorXs>& x) override;
117
118 /**
119 * @brief Compute the derivatives of the joint-acceleration residual
120 *
121 * @param[in] data Joint-acceleration residual data
122 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
123 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
124 */
125 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
126 const Eigen::Ref<const VectorXs>& x,
127 const Eigen::Ref<const VectorXs>& u) override;
128
129 /**
130 * @brief Create the joint-acceleration residual data
131 */
132 virtual std::shared_ptr<ResidualDataAbstract> createData(
133 DataCollectorAbstract* const data) override;
134
135 /**
136 * @brief Cast the joint-acceleration residual model to a different scalar
137 * type.
138 *
139 * It is useful for operations requiring different precision or scalar types.
140 *
141 * @tparam NewScalar The new scalar type to cast to.
142 * @return ResidualModelJointAccelerationTpl<NewScalar> A residual model with
143 * the new scalar type.
144 */
145 template <typename NewScalar>
146 ResidualModelJointAccelerationTpl<NewScalar> cast() const;
147
148 /**
149 * @brief Return the reference joint-acceleration vector
150 */
151 const VectorXs& get_reference() const;
152
153 /**
154 * @brief Modify the reference joint-acceleration vector
155 */
156 void set_reference(const VectorXs& reference);
157
158 /**
159 * @brief Print relevant information of the joint-acceleration residual
160 *
161 * @param[out] os Output stream object
162 */
163 virtual void print(std::ostream& os) const override;
164
165 protected:
166 using Base::nr_;
167 using Base::nu_;
168 using Base::state_;
169
170 private:
171 VectorXs aref_; //!< Reference joint-acceleration input
172 };
173
174 template <typename _Scalar>
175 struct ResidualDataJointAccelerationTpl
176 : public ResidualDataAbstractTpl<_Scalar> {
177 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
178
179 typedef _Scalar Scalar;
180 typedef MathBaseTpl<Scalar> MathBase;
181 typedef ResidualDataAbstractTpl<Scalar> Base;
182 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
183
184 template <template <typename Scalar> class Model>
185 ✗ ResidualDataJointAccelerationTpl(Model<Scalar>* const model,
186 DataCollectorAbstract* const data)
187 ✗ : Base(model, data) {
188 // Check that proper shared data has been passed
189 ✗ DataCollectorJointTpl<Scalar>* d =
190 ✗ dynamic_cast<DataCollectorJointTpl<Scalar>*>(shared);
191 ✗ if (d == nullptr) {
192 ✗ throw_pretty(
193 "Invalid argument: the shared data should be derived from "
194 "DataCollectorJoint");
195 }
196 ✗ joint = d->joint;
197 }
198 ✗ virtual ~ResidualDataJointAccelerationTpl() = default;
199
200 std::shared_ptr<JointDataAbstractTpl<Scalar> > joint; //!< Joint data
201 using Base::r;
202 using Base::Ru;
203 using Base::Rx;
204 using Base::shared;
205 };
206
207 } // namespace crocoddyl
208
209 /* --- Details -------------------------------------------------------------- */
210 /* --- Details -------------------------------------------------------------- */
211 /* --- Details -------------------------------------------------------------- */
212 #include "crocoddyl/core/residuals/joint-acceleration.hxx"
213
214 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
215 crocoddyl::ResidualModelJointAccelerationTpl)
216 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
217 crocoddyl::ResidualDataJointAccelerationTpl)
218
219 #endif // CROCODDYL_CORE_RESIDUALS_JOINT_ACCELERATION_HPP_
220