GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraints/residual.hpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 2 2 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2024, 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
51 typedef _Scalar Scalar;
52 typedef MathBaseTpl<Scalar> MathBase;
53 typedef ConstraintModelAbstractTpl<Scalar> Base;
54 typedef ConstraintDataResidualTpl<Scalar> Data;
55 typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
56 typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
57 typedef typename MathBase::VectorXs VectorXs;
58
59 /**
60 * @brief Initialize the residual constraint model as an inequality constraint
61 *
62 * @param[in] state State of the multibody system
63 * @param[in] residual Residual model
64 * @param[in] lower Lower bound (dimension of the residual vector)
65 * @param[in] upper Upper bound (dimension of the residual vector)
66 * @param[in] T_act False if we want to deactivate the residual at the
67 * terminal node (default true)
68 */
69 ConstraintModelResidualTpl(
70 std::shared_ptr<typename Base::StateAbstract> state,
71 std::shared_ptr<ResidualModelAbstract> residual, const VectorXs& lower,
72 const VectorXs& upper, const bool T_act = true);
73
74 /**
75 * @brief Initialize the residual constraint model as an equality constraint
76 *
77 * @param[in] state State of the multibody system
78 * @param[in] residual Residual model
79 * @param[in] T_act False if we want to deactivate the residual at the
80 * terminal node (default true)
81 */
82 ConstraintModelResidualTpl(
83 std::shared_ptr<typename Base::StateAbstract> state,
84 std::shared_ptr<ResidualModelAbstract> residual, const bool T_act = true);
85 virtual ~ConstraintModelResidualTpl();
86
87 /**
88 * @brief Compute the residual constraint
89 *
90 * @param[in] data Residual constraint data
91 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
92 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
93 */
94 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>& x,
96 const Eigen::Ref<const VectorXs>& u);
97
98 /**
99 * @brief Compute the residual constraint based on state only
100 *
101 * It updates the constraint based on the state only. This function is
102 * commonly used in the terminal nodes of an optimal control problem.
103 *
104 * @param[in] data Residual constraint data
105 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
106 */
107 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
108 const Eigen::Ref<const VectorXs>& x);
109
110 /**
111 * @brief Compute the derivatives of the residual constraint
112 *
113 * @param[in] data Residual constraint data
114 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
115 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
116 */
117 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
118 const Eigen::Ref<const VectorXs>& x,
119 const Eigen::Ref<const VectorXs>& u);
120
121 /**
122 * @brief Compute the derivatives of the residual constraint with respect to
123 * the state only
124 *
125 * It updates the Jacobian of the constraint function based on the state only.
126 * This function is commonly used in the terminal nodes of an optimal control
127 * problem.
128 *
129 * @param[in] data Residual constraint data
130 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
131 */
132 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
133 const Eigen::Ref<const VectorXs>& x);
134
135 /**
136 * @brief Create the residual constraint data
137 */
138 virtual std::shared_ptr<ConstraintDataAbstract> createData(
139 DataCollectorAbstract* const data);
140
141 /**
142 * @brief Print relevant information of the cost-residual model
143 *
144 * @param[out] os Output stream object
145 */
146 virtual void print(std::ostream& os) const;
147
148 private:
149 void updateCalc(const std::shared_ptr<ConstraintDataAbstract>& data);
150 void updateCalcDiff(const std::shared_ptr<ConstraintDataAbstract>& data);
151
152 protected:
153 using Base::lb_;
154 using Base::ng_;
155 using Base::nh_;
156 using Base::nu_;
157 using Base::residual_;
158 using Base::state_;
159 using Base::T_constraint_;
160 using Base::type_;
161 using Base::ub_;
162 using Base::unone_;
163 };
164
165 template <typename _Scalar>
166 struct ConstraintDataResidualTpl : public ConstraintDataAbstractTpl<_Scalar> {
167 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
168
169 typedef _Scalar Scalar;
170 typedef MathBaseTpl<Scalar> MathBase;
171 typedef ConstraintDataAbstractTpl<Scalar> Base;
172 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
173
174 template <template <typename Scalar> class Model>
175 229668 ConstraintDataResidualTpl(Model<Scalar>* const model,
176 DataCollectorAbstract* const data)
177 229668 : Base(model, data) {}
178
179 using Base::g;
180 using Base::Gu;
181 using Base::Gx;
182 using Base::h;
183 using Base::Hu;
184 using Base::Hx;
185 using Base::residual;
186 using Base::shared;
187 };
188
189 } // namespace crocoddyl
190
191 /* --- Details -------------------------------------------------------------- */
192 /* --- Details -------------------------------------------------------------- */
193 /* --- Details -------------------------------------------------------------- */
194 #include "crocoddyl/core/constraints/residual.hxx"
195
196 #endif // CROCODDYL_CORE_CONSTRAINTS_RESIDUAL_CONSTRAINT_HPP_
197