GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraints/residual.hxx
Date: 2025-01-16 08:47:40
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 235 ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
17 boost::shared_ptr<typename Base::StateAbstract> state,
18 boost::shared_ptr<ResidualModelAbstract> residual, const VectorXs& lower,
19 const VectorXs& upper, const bool T_act)
20
1/2
✓ Branch 5 taken 235 times.
✗ Branch 6 not taken.
 235 : Base(state, residual, residual->get_nr(), 0) {
21
1/2
✓ Branch 1 taken 235 times.
✗ Branch 2 not taken.
 235 lb_ = lower;
22
1/2
✓ Branch 1 taken 235 times.
✗ Branch 2 not taken.
 235 ub_ = upper;
23
2/2
✓ Branch 2 taken 2715 times.
✓ Branch 3 taken 235 times.
 2950 for (std::size_t i = 0; i < residual_->get_nr(); ++i) {
24
8/10
✓ Branch 1 taken 2715 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2284 times.
✓ Branch 5 taken 431 times.
✓ Branch 7 taken 2284 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2108 times.
✓ Branch 11 taken 176 times.
✓ Branch 12 taken 2108 times.
✓ Branch 13 taken 607 times.
 2715 if (isfinite(lb_(i)) && isfinite(ub_(i))) {
25
3/6
✓ Branch 1 taken 2108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2108 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2108 times.
 2108 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 235 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 235 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 235 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 235 times.
✗ Branch 11 not taken.
 470 if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() ||
33
5/10
✓ Branch 2 taken 235 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 235 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 235 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 235 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 235 times.
 470 (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 235 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 235 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 235 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 235 times.
✗ Branch 11 not taken.
 470 if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() ||
39
5/10
✓ Branch 2 taken 235 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 235 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 235 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 235 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 235 times.
 470 (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 235 times.
 235 if (!T_act) {
45 ✗ T_constraint_ = false;
46 }
47 235 }
48
49 template <typename Scalar>
50 2426 ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
51 boost::shared_ptr<typename Base::StateAbstract> state,
52 boost::shared_ptr<ResidualModelAbstract> residual, const bool T_act)
53
1/2
✓ Branch 5 taken 2426 times.
✗ Branch 6 not taken.
 2426 : Base(state, residual, 0, residual->get_nr()) {
54
2/2
✓ Branch 0 taken 1702 times.
✓ Branch 1 taken 724 times.
 2426 if (!T_act) {
55 1702 T_constraint_ = false;
56 }
57 2426 }
58
59 template <typename Scalar>
60 5326 ConstraintModelResidualTpl<Scalar>::~ConstraintModelResidualTpl() {}
61
62 template <typename Scalar>
63 121688 void ConstraintModelResidualTpl<Scalar>::calc(
64 const boost::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 6588 void ConstraintModelResidualTpl<Scalar>::calc(
75 const boost::shared_ptr<ConstraintDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x) {
77 // Compute the constraint residual
78 6588 residual_->calc(data->residual, x);
79
80 // Fill the residual values for its corresponding type of constraint
81 6588 updateCalc(data);
82 6588 }
83
84 template <typename Scalar>
85 19103 void ConstraintModelResidualTpl<Scalar>::calcDiff(
86 const boost::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 341 void ConstraintModelResidualTpl<Scalar>::calcDiff(
97 const boost::shared_ptr<ConstraintDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x) {
99 // Compute the derivatives of the residual function
100 341 residual_->calcDiff(data->residual, x);
101
102 // Fill the residual values for its corresponding type of constraint
103 341 updateCalcDiff(data);
104 341 }
105
106 template <typename Scalar>
107 boost::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
108 229668 ConstraintModelResidualTpl<Scalar>::createData(
109 DataCollectorAbstract* const data) {
110 ✗ return boost::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 128276 void ConstraintModelResidualTpl<Scalar>::updateCalc(
116 const boost::shared_ptr<ConstraintDataAbstract>& data) {
117
2/4
✓ Branch 0 taken 7793 times.
✓ Branch 1 taken 120483 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 128276 switch (type_) {
118 7793 case ConstraintType::Inequality:
119 7793 data->g = data->residual->r;
120 7793 break;
121 120483 case ConstraintType::Equality:
122 120483 data->h = data->residual->r;
123 120483 break;
124 ✗ case ConstraintType::Both: // this condition is not supported and possible
125 ✗ break;
126 }
127 128276 }
128
129 template <typename Scalar>
130 19444 void ConstraintModelResidualTpl<Scalar>::updateCalcDiff(
131 const boost::shared_ptr<ConstraintDataAbstract>& data) {
132 19444 const bool is_rq = residual_->get_q_dependent();
133 19444 const bool is_rv = residual_->get_v_dependent();
134
4/4
✓ Branch 2 taken 6780 times.
✓ Branch 3 taken 12664 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 6778 times.
 19444 const bool is_ru = residual_->get_u_dependent() || nu_ == 0;
135
2/4
✓ Branch 0 taken 204 times.
✓ Branch 1 taken 19240 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 19444 switch (type_) {
136 204 case ConstraintType::Inequality:
137
4/4
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 22 times.
✓ Branch 2 taken 92 times.
✓ Branch 3 taken 90 times.
 204 if (is_rq && is_rv) {
138 92 data->Gx = data->residual->Rx;
139
2/2
✓ Branch 0 taken 90 times.
✓ Branch 1 taken 22 times.
 112 } else if (is_rq) {
140 90 const std::size_t nv = state_->get_nv();
141
2/4
✓ Branch 5 taken 90 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 90 times.
✗ Branch 9 not taken.
 90 data->Gx.leftCols(nv) = data->residual->Rx.leftCols(nv);
142
1/2
✓ Branch 3 taken 90 times.
✗ Branch 4 not taken.
 90 data->Gx.rightCols(nv).setZero();
143
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
 22 } 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 76 times.
✓ Branch 1 taken 128 times.
 204 if (is_ru) {
149 76 data->Gu = data->residual->Ru;
150 }
151 204 break;
152 19240 case ConstraintType::Equality:
153
4/4
✓ Branch 0 taken 19216 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 15859 times.
✓ Branch 3 taken 3357 times.
 19240 if (is_rq && is_rv) {
154 15859 data->Hx = data->residual->Rx;
155
2/2
✓ Branch 0 taken 3357 times.
✓ Branch 1 taken 24 times.
 3381 } else if (is_rq) {
156 3357 const std::size_t nv = state_->get_nv();
157
2/4
✓ Branch 5 taken 3357 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3357 times.
✗ Branch 9 not taken.
 3357 data->Hx.leftCols(nv) = data->residual->Rx.leftCols(nv);
158
1/2
✓ Branch 3 taken 3357 times.
✗ Branch 4 not taken.
 3357 data->Hx.rightCols(nv).setZero();
159
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
 24 } 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 12590 times.
✓ Branch 1 taken 6650 times.
 19240 if (is_ru) {
165 12590 data->Hu = data->residual->Ru;
166 }
167 19240 break;
168 ✗ case ConstraintType::Both: // this condition is not supported and possible
169 ✗ break;
170 }
171 19444 }
172
173 template <typename Scalar>
174 ✗ void ConstraintModelResidualTpl<Scalar>::print(std::ostream& os) const {
175 ✗ os << "ConstraintModelResidual {" << *residual_ << "}";
176 }
177
178 } // namespace crocoddyl
179