GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/control-gravity.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 12 0.0%
Branches: 0 24 0.0%
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 ✗ 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 ✗ ResidualDataControlGravTpl(Model<Scalar> *const model,
152 DataCollectorAbstract *const data)
153 ✗ : Base(model, data) {
154 // Check that proper shared data has been passed
155 ✗ DataCollectorActMultibodyTpl<Scalar> *d =
156 ✗ dynamic_cast<DataCollectorActMultibodyTpl<Scalar> *>(shared);
157 ✗ 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 ✗ static_cast<StateMultibody *>(model->get_state().get());
165 ✗ pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
166 ✗ actuation = d->actuation;
167 }
168 ✗ 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