GCC Code Coverage Report

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