GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraints/residual.hxx
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 74 91 81.3%
Branches: 71 148 48.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 #include "crocoddyl/core/utils/exception.hpp"
10
11 namespace crocoddyl {
12
13 using std::isfinite;
14
15 template <typename Scalar>
16 238 ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
17 std::shared_ptr<typename Base::StateAbstract> state,
18 std::shared_ptr<ResidualModelAbstract> residual, const VectorXs& lower,
19 const VectorXs& upper, const bool T_act)
20
1/2
✓ Branch 5 taken 238 times.
✗ Branch 6 not taken.
 238 : Base(state, residual, residual->get_nr(), 0) {
21
1/2
✓ Branch 1 taken 238 times.
✗ Branch 2 not taken.
 238 lb_ = lower;
22
1/2
✓ Branch 1 taken 238 times.
✗ Branch 2 not taken.
 238 ub_ = upper;
23
2/2
✓ Branch 2 taken 2646 times.
✓ Branch 3 taken 238 times.
 2884 for (std::size_t i = 0; i < residual_->get_nr(); ++i) {
24
8/10
✓ Branch 1 taken 2646 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2214 times.
✓ Branch 5 taken 432 times.
✓ Branch 7 taken 2214 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2041 times.
✓ Branch 11 taken 173 times.
✓ Branch 12 taken 2041 times.
✓ Branch 13 taken 605 times.
 2646 if (isfinite(lb_(i)) && isfinite(ub_(i))) {
25
3/6
✓ Branch 1 taken 2041 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2041 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2041 times.
 2041 if (lb_(i) - ub_(i) > 0) {
26 ✗ throw_pretty(
27 "Invalid argument: the upper bound is not equal to / higher than "
28 "the lower bound.")
29 }
30 }
31 }
32
4/8
✓ Branch 2 taken 238 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 238 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 238 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 238 times.
✗ Branch 11 not taken.
 476 if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() ||
33
5/10
✓ Branch 2 taken 238 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 238 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 238 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 238 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 238 times.
 476 (lb_.array() == std::numeric_limits<Scalar>::max()).any()) {
34 ✗ throw_pretty(
35 "Invalid argument: the lower bound cannot contain a positive "
36 "infinity/max value");
37 }
38
4/8
✓ Branch 2 taken 238 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 238 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 238 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 238 times.
✗ Branch 11 not taken.
 476 if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() ||
39
5/10
✓ Branch 2 taken 238 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 238 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 238 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 238 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 238 times.
 476 (ub_.array() == -std::numeric_limits<Scalar>::infinity()).any()) {
40 ✗ throw_pretty(
41 "Invalid argument: the lower bound cannot contain a negative "
42 "infinity/max value");
43 }
44
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 238 times.
 238 if (!T_act) {
45 ✗ T_constraint_ = false;
46 }
47 238 }
48
49 template <typename Scalar>
50 2423 ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
51 std::shared_ptr<typename Base::StateAbstract> state,
52 std::shared_ptr<ResidualModelAbstract> residual, const bool T_act)
53
1/2
✓ Branch 5 taken 2423 times.
✗ Branch 6 not taken.
 2423 : Base(state, residual, 0, residual->get_nr()) {
54
2/2
✓ Branch 0 taken 1702 times.
✓ Branch 1 taken 721 times.
 2423 if (!T_act) {
55 1702 T_constraint_ = false;
56 }
57 2423 }
58
59 template <typename Scalar>
60 5326 ConstraintModelResidualTpl<Scalar>::~ConstraintModelResidualTpl() {}
61
62 template <typename Scalar>
63 121688 void ConstraintModelResidualTpl<Scalar>::calc(
64 const std::shared_ptr<ConstraintDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
66 // Compute the constraint residual
67 121688 residual_->calc(data->residual, x, u);
68
69 // Fill the residual values for its corresponding type of constraint
70 121688 updateCalc(data);
71 121688 }
72
73 template <typename Scalar>
74 6594 void ConstraintModelResidualTpl<Scalar>::calc(
75 const std::shared_ptr<ConstraintDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x) {
77 // Compute the constraint residual
78 6594 residual_->calc(data->residual, x);
79
80 // Fill the residual values for its corresponding type of constraint
81 6594 updateCalc(data);
82 6594 }
83
84 template <typename Scalar>
85 19103 void ConstraintModelResidualTpl<Scalar>::calcDiff(
86 const std::shared_ptr<ConstraintDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
88 // Compute the derivatives of the residual function
89 19103 residual_->calcDiff(data->residual, x, u);
90
91 // Fill the residual values for its corresponding type of constraint
92 19103 updateCalcDiff(data);
93 19103 }
94
95 template <typename Scalar>
96 347 void ConstraintModelResidualTpl<Scalar>::calcDiff(
97 const std::shared_ptr<ConstraintDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x) {
99 // Compute the derivatives of the residual function
100 347 residual_->calcDiff(data->residual, x);
101
102 // Fill the residual values for its corresponding type of constraint
103 347 updateCalcDiff(data);
104 347 }
105
106 template <typename Scalar>
107 std::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
108 229668 ConstraintModelResidualTpl<Scalar>::createData(
109 DataCollectorAbstract* const data) {
110 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
111
1/2
✓ Branch 2 taken 229668 times.
✗ Branch 3 not taken.
 229668 data);
112 }
113
114 template <typename Scalar>
115 128282 void ConstraintModelResidualTpl<Scalar>::updateCalc(
116 const std::shared_ptr<ConstraintDataAbstract>& data) {
117
2/4
✓ Branch 0 taken 7801 times.
✓ Branch 1 taken 120481 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 128282 switch (type_) {
118 7801 case ConstraintType::Inequality:
119 7801 data->g = data->residual->r;
120 7801 break;
121 120481 case ConstraintType::Equality:
122 120481 data->h = data->residual->r;
123 120481 break;
124 ✗ case ConstraintType::Both: // this condition is not supported and possible
125 ✗ break;
126 }
127 128282 }
128
129 template <typename Scalar>
130 19450 void ConstraintModelResidualTpl<Scalar>::updateCalcDiff(
131 const std::shared_ptr<ConstraintDataAbstract>& data) {
132 19450 const bool is_rq = residual_->get_q_dependent();
133 19450 const bool is_rv = residual_->get_v_dependent();
134
4/4
✓ Branch 2 taken 6792 times.
✓ Branch 3 taken 12658 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 6790 times.
 19450 const bool is_ru = residual_->get_u_dependent() || nu_ == 0;
135
2/4
✓ Branch 0 taken 218 times.
✓ Branch 1 taken 19232 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 19450 switch (type_) {
136 218 case ConstraintType::Inequality:
137
4/4
✓ Branch 0 taken 198 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 96 times.
✓ Branch 3 taken 102 times.
 218 if (is_rq && is_rv) {
138 96 data->Gx = data->residual->Rx;
139
2/2
✓ Branch 0 taken 102 times.
✓ Branch 1 taken 20 times.
 122 } else if (is_rq) {
140 102 const std::size_t nv = state_->get_nv();
141
2/4
✓ Branch 5 taken 102 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 102 times.
✗ Branch 9 not taken.
 102 data->Gx.leftCols(nv) = data->residual->Rx.leftCols(nv);
142
1/2
✓ Branch 3 taken 102 times.
✗ Branch 4 not taken.
 102 data->Gx.rightCols(nv).setZero();
143
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 20 } else if (is_rv) {
144 ✗ const std::size_t nv = state_->get_nv();
145 ✗ data->Gx.leftCols(nv).setZero();
146 ✗ data->Gx.rightCols(nv) = data->residual->Rx.rightCols(nv);
147 }
148
2/2
✓ Branch 0 taken 74 times.
✓ Branch 1 taken 144 times.
 218 if (is_ru) {
149 74 data->Gu = data->residual->Ru;
150 }
151 218 break;
152 19232 case ConstraintType::Equality:
153
4/4
✓ Branch 0 taken 19212 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 15867 times.
✓ Branch 3 taken 3345 times.
 19232 if (is_rq && is_rv) {
154 15867 data->Hx = data->residual->Rx;
155
2/2
✓ Branch 0 taken 3345 times.
✓ Branch 1 taken 20 times.
 3365 } else if (is_rq) {
156 3345 const std::size_t nv = state_->get_nv();
157
2/4
✓ Branch 5 taken 3345 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3345 times.
✗ Branch 9 not taken.
 3345 data->Hx.leftCols(nv) = data->residual->Rx.leftCols(nv);
158
1/2
✓ Branch 3 taken 3345 times.
✗ Branch 4 not taken.
 3345 data->Hx.rightCols(nv).setZero();
159
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 20 } else if (is_rv) {
160 ✗ const std::size_t nv = state_->get_nv();
161 ✗ data->Hx.leftCols(nv).setZero();
162 ✗ data->Hx.rightCols(nv) = data->residual->Rx.rightCols(nv);
163 }
164
2/2
✓ Branch 0 taken 12586 times.
✓ Branch 1 taken 6646 times.
 19232 if (is_ru) {
165 12586 data->Hu = data->residual->Ru;
166 }
167 19232 break;
168 ✗ case ConstraintType::Both: // this condition is not supported and possible
169 ✗ break;
170 }
171 19450 }
172
173 template <typename Scalar>
174 ✗ void ConstraintModelResidualTpl<Scalar>::print(std::ostream& os) const {
175 ✗ os << "ConstraintModelResidual {" << *residual_ << "}";
176 }
177
178 } // namespace crocoddyl
179