GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residuals/control.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 1 2 50.0%
Branches: 0 2 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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_CORE_RESIDUALS_CONTROL_HPP_
11 #define CROCODDYL_CORE_RESIDUALS_CONTROL_HPP_
12
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/residual-base.hpp"
15
16 namespace crocoddyl {
17
18 /**
19 * @brief Define a control residual
20 *
21 * This residual function is defined as
22 * \f$\mathbf{r}=\mathbf{u}-\mathbf{u}^*\f$, where
23 * \f$\mathbf{u},\mathbf{u}^*\in~\mathbb{R}^{nu}\f$ are the current and
24 * reference control inputs, respectively. Note that the dimension of the
25 * residual vector is obtained from `nu`.
26 *
27 * Both residual and residual Jacobians are computed analytically.
28 *
29 * As described in ResidualModelAbstractTpl(), the residual value and its
30 * Jacobians are calculated by `calc` and `calcDiff`, respectively.
31 *
32 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
33 */
34 template <typename _Scalar>
35 class ResidualModelControlTpl : public ResidualModelAbstractTpl<_Scalar> {
36 public:
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelControlTpl)
39
40 typedef _Scalar Scalar;
41 typedef MathBaseTpl<Scalar> MathBase;
42 typedef ResidualModelAbstractTpl<Scalar> Base;
43 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
44 typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
45 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
46 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
47 typedef typename MathBase::VectorXs VectorXs;
48 typedef typename MathBase::MatrixXs MatrixXs;
49
50 /**
51 * @brief Initialize the control residual model
52 *
53 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
54 *
55 * @param[in] state State of the multibody system
56 * @param[in] uref Reference control input
57 */
58 ResidualModelControlTpl(std::shared_ptr<typename Base::StateAbstract> state,
59 const VectorXs& uref);
60
61 /**
62 * @brief Initialize the control 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] nu Dimension of the control vector
68 */
69 ResidualModelControlTpl(std::shared_ptr<typename Base::StateAbstract> state,
70 const std::size_t nu);
71
72 /**
73 * @brief Initialize the control residual model
74 *
75 * The default reference control is obtained from
76 * `MathBaseTpl<>::VectorXs::Zero(nu)`.
77 *
78 * @param[in] state State of the multibody system
79 */
80 explicit ResidualModelControlTpl(
81 std::shared_ptr<typename Base::StateAbstract> state);
82 2846 virtual ~ResidualModelControlTpl() = default;
83
84 /**
85 * @brief Compute the control residual
86 *
87 * @param[in] data Control 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 calc(const std::shared_ptr<ResidualDataAbstract>& data,
92 const Eigen::Ref<const VectorXs>& x,
93 const Eigen::Ref<const VectorXs>& u) override;
94
95 /**
96 * @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
97 * data, const Eigen::Ref<const VectorXs>& x)
98 */
99 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>& x) override;
101
102 /**
103 * @brief Compute the derivatives of the control residual
104 *
105 * @param[in] data Control residual data
106 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
107 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
108 */
109 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>& x,
111 const Eigen::Ref<const VectorXs>& u) override;
112
113 /**
114 * @brief Create the control residual data
115 */
116 virtual std::shared_ptr<ResidualDataAbstract> createData(
117 DataCollectorAbstract* const data) override;
118
119 /**
120 * @brief Compute the derivative of the control-cost function
121 *
122 * This function assumes that the derivatives of the activation and residual
123 * are computed via calcDiff functions.
124 *
125 * @param cdata Cost data
126 * @param rdata Residual data
127 * @param adata Activation data
128 * @param update_u Update the derivative of the cost function w.r.t. to the
129 * control if True.
130 */
131 virtual void calcCostDiff(
132 const std::shared_ptr<CostDataAbstract>& cdata,
133 const std::shared_ptr<ResidualDataAbstract>& rdata,
134 const std::shared_ptr<ActivationDataAbstract>& adata,
135 const bool update_u = true) override;
136
137 /**
138 * @brief Cast the control residual model to a different scalar type.
139 *
140 * It is useful for operations requiring different precision or scalar types.
141 *
142 * @tparam NewScalar The new scalar type to cast to.
143 * @return ResidualModelControlTpl<NewScalar> A residual model with the
144 * new scalar type.
145 */
146 template <typename NewScalar>
147 ResidualModelControlTpl<NewScalar> cast() const;
148
149 /**
150 * @brief Return the reference control vector
151 */
152 const VectorXs& get_reference() const;
153
154 /**
155 * @brief Modify the reference control vector
156 */
157 void set_reference(const VectorXs& reference);
158
159 /**
160 * @brief Print relevant information of the control residual
161 *
162 * @param[out] os Output stream object
163 */
164 virtual void print(std::ostream& os) const override;
165
166 protected:
167 using Base::nu_;
168 using Base::state_;
169
170 private:
171 VectorXs uref_; //!< Reference control input
172 };
173
174 } // namespace crocoddyl
175
176 /* --- Details -------------------------------------------------------------- */
177 /* --- Details -------------------------------------------------------------- */
178 /* --- Details -------------------------------------------------------------- */
179 #include "crocoddyl/core/residuals/control.hxx"
180
181 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelControlTpl)
182
183 #endif // CROCODDYL_CORE_RESIDUALS_CONTROL_HPP_
184