GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/com-position.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) 2021-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_COM_POSITION_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_COM_POSITION_HPP_
12
13 #include "crocoddyl/core/residual-base.hpp"
14 #include "crocoddyl/multibody/data/multibody.hpp"
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/states/multibody.hpp"
17
18 namespace crocoddyl {
19
20 /**
21 * @brief CoM position residual
22 *
23 * This residual function defines the CoM tracking as
24 * \f$\mathbf{r}=\mathbf{c}-\mathbf{c}^*\f$, where
25 * \f$\mathbf{c},\mathbf{c}^*\in~\mathbb{R}^3\f$ are the current and reference
26 * CoM position, respectively. Note that the dimension of the residual vector is
27 * obtained from 3. Furthermore, the Jacobians of the residual function are
28 * 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 ResidualModelCoMPositionTpl : public ResidualModelAbstractTpl<_Scalar> {
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelCoMPositionTpl)
40
41 typedef _Scalar Scalar;
42 typedef MathBaseTpl<Scalar> MathBase;
43 typedef ResidualModelAbstractTpl<Scalar> Base;
44 typedef ResidualDataCoMPositionTpl<Scalar> Data;
45 typedef StateMultibodyTpl<Scalar> StateMultibody;
46 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
47 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
48 typedef typename MathBase::Vector3s Vector3s;
49 typedef typename MathBase::VectorXs VectorXs;
50
51 /**
52 * @brief Initialize the CoM position residual model
53 *
54 * @param[in] state State of the multibody system
55 * @param[in] cref Reference CoM position
56 * @param[in] nu Dimension of the control vector
57 */
58 ResidualModelCoMPositionTpl(std::shared_ptr<StateMultibody> state,
59 const Vector3s& cref, const std::size_t nu);
60
61 /**
62 * @brief Initialize the CoM position residual model
63 *
64 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
65 *
66 * @param[in] state State of the multibody system
67 * @param[in] cref Reference CoM position
68 */
69 ResidualModelCoMPositionTpl(std::shared_ptr<StateMultibody> state,
70 const Vector3s& cref);
71 ✗ virtual ~ResidualModelCoMPositionTpl() = default;
72
73 /**
74 * @brief Compute the CoM position residual
75 *
76 * @param[in] data CoM position residual data
77 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
78 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
79 */
80 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
81 const Eigen::Ref<const VectorXs>& x,
82 const Eigen::Ref<const VectorXs>& u) override;
83
84 /**
85 * @brief Compute the derivatives of the CoM position residual
86 *
87 * @param[in] data CoM position residual data
88 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
89 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
90 */
91 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
92 const Eigen::Ref<const VectorXs>& x,
93 const Eigen::Ref<const VectorXs>& u) override;
94 virtual std::shared_ptr<ResidualDataAbstract> createData(
95 DataCollectorAbstract* const data) override;
96
97 /**
98 * @brief Cast the com-position residual model to a different scalar type.
99 *
100 * It is useful for operations requiring different precision or scalar types.
101 *
102 * @tparam NewScalar The new scalar type to cast to.
103 * @return ResidualModelCoMPositionTpl<NewScalar> A residual model with the
104 * new scalar type.
105 */
106 template <typename NewScalar>
107 ResidualModelCoMPositionTpl<NewScalar> cast() const;
108
109 /**
110 * @brief Return the CoM position reference
111 */
112 const Vector3s& get_reference() const;
113
114 /**
115 * @brief Modify the CoM position reference
116 */
117 void set_reference(const Vector3s& cref);
118
119 /**
120 * @brief Print relevant information of the com-position residual
121 *
122 * @param[out] os Output stream object
123 */
124 virtual void print(std::ostream& os) const override;
125
126 protected:
127 using Base::nu_;
128 using Base::state_;
129 using Base::u_dependent_;
130 using Base::v_dependent_;
131
132 private:
133 Vector3s cref_; //!< Reference CoM position
134 };
135
136 template <typename _Scalar>
137 struct ResidualDataCoMPositionTpl : public ResidualDataAbstractTpl<_Scalar> {
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139
140 typedef _Scalar Scalar;
141 typedef MathBaseTpl<Scalar> MathBase;
142 typedef ResidualDataAbstractTpl<Scalar> Base;
143 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
144 typedef typename MathBase::Matrix3xs Matrix3xs;
145
146 template <template <typename Scalar> class Model>
147 ✗ ResidualDataCoMPositionTpl(Model<Scalar>* const model,
148 DataCollectorAbstract* const data)
149 ✗ : Base(model, data) {
150 // Check that proper shared data has been passed
151 ✗ DataCollectorMultibodyTpl<Scalar>* d =
152 ✗ dynamic_cast<DataCollectorMultibodyTpl<Scalar>*>(shared);
153 ✗ if (d == NULL) {
154 ✗ throw_pretty(
155 "Invalid argument: the shared data should be derived from "
156 "DataCollectorMultibody");
157 }
158
159 // Avoids data casting at runtime
160 ✗ pinocchio = d->pinocchio;
161 }
162 ✗ virtual ~ResidualDataCoMPositionTpl() = default;
163
164 pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
165 using Base::r;
166 using Base::Ru;
167 using Base::Rx;
168 using Base::shared;
169 };
170
171 } // namespace crocoddyl
172
173 /* --- Details -------------------------------------------------------------- */
174 /* --- Details -------------------------------------------------------------- */
175 /* --- Details -------------------------------------------------------------- */
176 #include "crocoddyl/multibody/residuals/com-position.hxx"
177
178 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelCoMPositionTpl)
179 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataCoMPositionTpl)
180
181 #endif // CROCODDYL_MULTIBODY_RESIDUALS_COM_POSITION_HPP_
182