GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraints/residual.hpp
Date: 2025-01-16 08:47:40
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 boost::shared_ptr<typename Base::StateAbstract> state,
71 boost::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 boost::shared_ptr<typename Base::StateAbstract> state,
84 boost::shared_ptr<ResidualModelAbstract> residual,
85 const bool T_act = true);
86 virtual ~ConstraintModelResidualTpl();
87
88 /**
89 * @brief Compute the residual constraint
90 *
91 * @param[in] data Residual constraint 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 calc(const boost::shared_ptr<ConstraintDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>& x,
97 const Eigen::Ref<const VectorXs>& u);
98
99 /**
100 * @brief Compute the residual constraint based on state only
101 *
102 * It updates the constraint based on the state only. This function is
103 * commonly used in the terminal nodes of an optimal control problem.
104 *
105 * @param[in] data Residual constraint data
106 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
107 */
108 virtual void calc(const boost::shared_ptr<ConstraintDataAbstract>& data,
109 const Eigen::Ref<const VectorXs>& x);
110
111 /**
112 * @brief Compute the derivatives of the residual constraint
113 *
114 * @param[in] data Residual constraint data
115 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
116 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
117 */
118 virtual void calcDiff(const boost::shared_ptr<ConstraintDataAbstract>& data,
119 const Eigen::Ref<const VectorXs>& x,
120 const Eigen::Ref<const VectorXs>& u);
121
122 /**
123 * @brief Compute the derivatives of the residual constraint with respect to
124 * the state only
125 *
126 * It updates the Jacobian of the constraint function based on the state only.
127 * This function is commonly used in the terminal nodes of an optimal control
128 * problem.
129 *
130 * @param[in] data Residual constraint data
131 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
132 */
133 virtual void calcDiff(const boost::shared_ptr<ConstraintDataAbstract>& data,
134 const Eigen::Ref<const VectorXs>& x);
135
136 /**
137 * @brief Create the residual constraint data
138 */
139 virtual boost::shared_ptr<ConstraintDataAbstract> createData(
140 DataCollectorAbstract* const data);
141
142 /**
143 * @brief Print relevant information of the cost-residual model
144 *
145 * @param[out] os Output stream object
146 */
147 virtual void print(std::ostream& os) const;
148
149 private:
150 void updateCalc(const boost::shared_ptr<ConstraintDataAbstract>& data);
151 void updateCalcDiff(const boost::shared_ptr<ConstraintDataAbstract>& data);
152
153 protected:
154 using Base::lb_;
155 using Base::ng_;
156 using Base::nh_;
157 using Base::nu_;
158 using Base::residual_;
159 using Base::state_;
160 using Base::T_constraint_;
161 using Base::type_;
162 using Base::ub_;
163 using Base::unone_;
164 };
165
166 template <typename _Scalar>
167 struct ConstraintDataResidualTpl : public ConstraintDataAbstractTpl<_Scalar> {
168 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
169
170 typedef _Scalar Scalar;
171 typedef MathBaseTpl<Scalar> MathBase;
172 typedef ConstraintDataAbstractTpl<Scalar> Base;
173 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
174
175 template <template <typename Scalar> class Model>
176 229668 ConstraintDataResidualTpl(Model<Scalar>* const model,
177 DataCollectorAbstract* const data)
178 229668 : Base(model, data) {}
179
180 using Base::g;
181 using Base::Gu;
182 using Base::Gx;
183 using Base::h;
184 using Base::Hu;
185 using Base::Hx;
186 using Base::residual;
187 using Base::shared;
188 };
189
190 } // namespace crocoddyl
191
192 /* --- Details -------------------------------------------------------------- */
193 /* --- Details -------------------------------------------------------------- */
194 /* --- Details -------------------------------------------------------------- */
195 #include "crocoddyl/core/constraints/residual.hxx"
196
197 #endif // CROCODDYL_CORE_CONSTRAINTS_RESIDUAL_CONSTRAINT_HPP_
198