GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraints/residual.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 4 5 80.0%
Branches: 0 2 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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_CONSTRAINTS_RESIDUAL_CONSTRAINT_HPP_
10 #define CROCODDYL_CORE_CONSTRAINTS_RESIDUAL_CONSTRAINT_HPP_
11
12 #include "crocoddyl/core/fwd.hpp"
13 //
14 #include "crocoddyl/core/constraint-base.hpp"
15 #include "crocoddyl/core/residual-base.hpp"
16
17 namespace crocoddyl {
18
19 /**
20 * @brief Residual-based constraint
21 *
22 * This constraint function uses a residual model to define equality /
23 * inequality constraint as \f[ \mathbf{\underline{r}} \leq
24 * \mathbf{r}(\mathbf{x}, \mathbf{u}) \leq \mathbf{\bar{r}} \f] where
25 * \f$\mathbf{r}(\cdot)\f$ describes the residual function, and
26 * \f$\mathbf{\underline{r}}\f$, \f$\mathbf{\bar{r}}\f$ are the lower and upper
27 * bounds, respectively. We can define element-wise equality constraints by
28 * defining the same value for both: lower and upper values. Additionally, if we
29 * do not define the bounds, then it is assumed that
30 * \f$\mathbf{\underline{r}}=\mathbf{\bar{r}}=\mathbf{0}\f$.
31 *
32 * The main computations are carring out in `calc` and `calcDiff` routines.
33 * `calc` computes the constraint residual and `calcDiff` computes the Jacobians
34 * of the constraint function. Concretely speaking, `calcDiff` builds a linear
35 * approximation of the constraint function with the form:
36 * \f$\mathbf{g_x}\in\mathbb{R}^{ng\times ndx}\f$,
37 * \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$,
38 * \f$\mathbf{h_x}\in\mathbb{R}^{nh\times ndx}\f$
39 * \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$.
40 * Additionally, it is important to note that `calcDiff()` computes the
41 * derivatives using the latest stored values by `calc()`. Thus, we need to run
42 * first `calc()`.
43 *
44 * \sa `ConstraintModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
45 */
46 template <typename _Scalar>
47 class ConstraintModelResidualTpl : public ConstraintModelAbstractTpl<_Scalar> {
48 public:
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 ✗ CROCODDYL_DERIVED_CAST(ConstraintModelBase, ConstraintModelResidualTpl)
51
52 typedef _Scalar Scalar;
53 typedef MathBaseTpl<Scalar> MathBase;
54 typedef ConstraintModelAbstractTpl<Scalar> Base;
55 typedef ConstraintDataResidualTpl<Scalar> Data;
56 typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
57 typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
58 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
59 typedef typename MathBase::VectorXs VectorXs;
60
61 /**
62 * @brief Initialize the residual constraint model as an inequality constraint
63 *
64 * @param[in] state State of the multibody system
65 * @param[in] residual Residual model
66 * @param[in] lower Lower bound (dimension of the residual vector)
67 * @param[in] upper Upper bound (dimension of the residual vector)
68 * @param[in] T_act False if we want to deactivate the residual at the
69 * terminal node (default true)
70 */
71 ConstraintModelResidualTpl(
72 std::shared_ptr<typename Base::StateAbstract> state,
73 std::shared_ptr<ResidualModelAbstract> residual, const VectorXs& lower,
74 const VectorXs& upper, const bool T_act = true);
75
76 /**
77 * @brief Initialize the residual constraint model as an equality constraint
78 *
79 * @param[in] state State of the multibody system
80 * @param[in] residual Residual model
81 * @param[in] T_act False if we want to deactivate the residual at the
82 * terminal node (default true)
83 */
84 ConstraintModelResidualTpl(
85 std::shared_ptr<typename Base::StateAbstract> state,
86 std::shared_ptr<ResidualModelAbstract> residual, const bool T_act = true);
87 5326 virtual ~ConstraintModelResidualTpl() = default;
88
89 /**
90 * @brief Compute the residual constraint
91 *
92 * @param[in] data Residual constraint data
93 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
94 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
95 */
96 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
97 const Eigen::Ref<const VectorXs>& x,
98 const Eigen::Ref<const VectorXs>& u) override;
99
100 /**
101 * @brief Compute the residual constraint based on state only
102 *
103 * It updates the constraint based on the state only. This function is
104 * commonly used in the terminal nodes of an optimal control problem.
105 *
106 * @param[in] data Residual constraint data
107 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
108 */
109 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>& x) override;
111
112 /**
113 * @brief Compute the derivatives of the residual constraint
114 *
115 * @param[in] data Residual constraint data
116 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
117 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
118 */
119 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
120 const Eigen::Ref<const VectorXs>& x,
121 const Eigen::Ref<const VectorXs>& u) override;
122
123 /**
124 * @brief Compute the derivatives of the residual constraint with respect to
125 * the state only
126 *
127 * It updates the Jacobian of the constraint function based on the state only.
128 * This function is commonly used in the terminal nodes of an optimal control
129 * problem.
130 *
131 * @param[in] data Residual constraint data
132 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
133 */
134 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
135 const Eigen::Ref<const VectorXs>& x) override;
136
137 /**
138 * @brief Create the residual constraint data
139 */
140 virtual std::shared_ptr<ConstraintDataAbstract> createData(
141 DataCollectorAbstract* const data) override;
142
143 /**
144 * @brief Cast the residual constraint model to a different scalar type.
145 *
146 * It is useful for operations requiring different precision or scalar types.
147 *
148 * @tparam NewScalar The new scalar type to cast to.
149 * @return ConstraintModelResidualTpl<NewScalar> A constraint model with the
150 * new scalar type.
151 */
152 template <typename NewScalar>
153 ConstraintModelResidualTpl<NewScalar> cast() const;
154
155 /**
156 * @brief Print relevant information of the cost-residual model
157 *
158 * @param[out] os Output stream object
159 */
160 virtual void print(std::ostream& os) const override;
161
162 private:
163 void updateCalc(const std::shared_ptr<ConstraintDataAbstract>& data);
164 void updateCalcDiff(const std::shared_ptr<ConstraintDataAbstract>& data);
165
166 protected:
167 using Base::lb_;
168 using Base::ng_;
169 using Base::nh_;
170 using Base::nu_;
171 using Base::residual_;
172 using Base::state_;
173 using Base::T_constraint_;
174 using Base::type_;
175 using Base::ub_;
176 using Base::unone_;
177 };
178
179 template <typename _Scalar>
180 struct ConstraintDataResidualTpl : public ConstraintDataAbstractTpl<_Scalar> {
181 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
182
183 typedef _Scalar Scalar;
184 typedef MathBaseTpl<Scalar> MathBase;
185 typedef ConstraintDataAbstractTpl<Scalar> Base;
186 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
187
188 template <template <typename Scalar> class Model>
189 229674 ConstraintDataResidualTpl(Model<Scalar>* const model,
190 DataCollectorAbstract* const data)
191 229674 : Base(model, data) {}
192 459332 virtual ~ConstraintDataResidualTpl() = default;
193
194 using Base::g;
195 using Base::Gu;
196 using Base::Gx;
197 using Base::h;
198 using Base::Hu;
199 using Base::Hx;
200 using Base::residual;
201 using Base::shared;
202 };
203
204 } // namespace crocoddyl
205
206 /* --- Details -------------------------------------------------------------- */
207 /* --- Details -------------------------------------------------------------- */
208 /* --- Details -------------------------------------------------------------- */
209 #include "crocoddyl/core/constraints/residual.hxx"
210
211 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ConstraintModelResidualTpl)
212 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintDataResidualTpl)
213
214 #endif // CROCODDYL_CORE_CONSTRAINTS_RESIDUAL_CONSTRAINT_HPP_
215