GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/controls/poly-two-rk.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 76 114 66.7%
Branches: 75 400 18.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, University of Edinburgh, University of Trento
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 168 ControlParametrizationModelPolyTwoRKTpl<
13 Scalar>::ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw,
14 const RKType rktype)
15 168 : Base(nw, 3 * nw), rktype_(rktype) {
16
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 168 times.
 168 if (rktype_ == RKType::two) {
17 ✗ std::cerr << "Invalid argument: RK2 parametrization is not supported"
18 ✗ << std::endl;
19 }
20 168 }
21
22 template <typename Scalar>
23 340 ControlParametrizationModelPolyTwoRKTpl<
24 340 Scalar>::~ControlParametrizationModelPolyTwoRKTpl() {}
25
26 template <typename Scalar>
27 16196 void ControlParametrizationModelPolyTwoRKTpl<Scalar>::calc(
28 const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
29 const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
30
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16196 times.
 16196 if (static_cast<std::size_t>(u.size()) != nu_) {
31 ✗ throw_pretty(
32 "Invalid argument: " << "u has wrong dimension (it should be " +
33 std::to_string(nu_) + ")");
34 }
35 16196 Data* d = static_cast<Data*>(data.get());
36
1/2
✓ Branch 1 taken 16196 times.
✗ Branch 2 not taken.
 16196 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p0 = u.head(nw_);
37 16196 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p1 =
38
1/2
✓ Branch 1 taken 16196 times.
✗ Branch 2 not taken.
 16196 u.segment(nw_, nw_);
39
1/2
✓ Branch 1 taken 16196 times.
✗ Branch 2 not taken.
 16196 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p2 = u.tail(nw_);
40 16196 d->tmp_t2 = t * t;
41
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 6945 times.
✓ Branch 2 taken 9251 times.
✗ Branch 3 not taken.
 16196 switch (rktype_) {
42 ✗ case two:
43 ✗ std::cerr << "Invalid argument: RK2 parametrization is not supported"
44 ✗ << std::endl;
45 ✗ break;
46 6945 case three:
47
1/2
✓ Branch 1 taken 6945 times.
✗ Branch 2 not taken.
 6945 d->c[2] = Scalar(4.5) * d->tmp_t2 - Scalar(1.5) * t;
48
1/2
✓ Branch 1 taken 6945 times.
✗ Branch 2 not taken.
 6945 d->c[1] = -Scalar(9.) * d->tmp_t2 + Scalar(6.) * t;
49
1/2
✓ Branch 1 taken 6945 times.
✗ Branch 2 not taken.
 6945 d->c[0] = Scalar(4.5) * (d->tmp_t2 - t) + Scalar(1.);
50 6945 break;
51 9251 case four:
52
1/2
✓ Branch 1 taken 9251 times.
✗ Branch 2 not taken.
 9251 d->c[2] = Scalar(2.) * d->tmp_t2 - t;
53
2/4
✓ Branch 1 taken 9251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9251 times.
✗ Branch 5 not taken.
 9251 d->c[1] = -Scalar(2.) * d->c[2] + Scalar(2.) * t;
54
2/4
✓ Branch 1 taken 9251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9251 times.
✗ Branch 5 not taken.
 9251 d->c[0] = d->c[2] - Scalar(2.) * t + Scalar(1.);
55 9251 break;
56 }
57
9/18
✓ Branch 1 taken 16196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16196 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16196 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16196 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 16196 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 16196 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 16196 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 16196 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 16196 times.
✗ Branch 26 not taken.
 16196 d->w = d->c[2] * p2 + d->c[1] * p1 + d->c[0] * p0;
58 16196 }
59
60 template <typename Scalar>
61 6 void ControlParametrizationModelPolyTwoRKTpl<Scalar>::calcDiff(
62 const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
63 const Scalar, const Eigen::Ref<const VectorXs>&) const {
64 6 Data* d = static_cast<Data*>(data.get());
65
3/6
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
 6 d->dw_du.leftCols(nw_).diagonal().array() = d->c[0];
66
3/6
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
 6 d->dw_du.middleCols(nw_, nw_).diagonal().array() = d->c[1];
67
3/6
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
 6 d->dw_du.rightCols(nw_).diagonal().array() = d->c[2];
68 6 }
69
70 template <typename Scalar>
71 boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
72 16359 ControlParametrizationModelPolyTwoRKTpl<Scalar>::createData() {
73
1/2
✓ Branch 2 taken 16359 times.
✗ Branch 3 not taken.
 16359 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
74 }
75
76 template <typename Scalar>
77 ✗ void ControlParametrizationModelPolyTwoRKTpl<Scalar>::params(
78 const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
79 const Scalar, const Eigen::Ref<const VectorXs>& w) const {
80 ✗ if (static_cast<std::size_t>(w.size()) != nw_) {
81 ✗ throw_pretty(
82 "Invalid argument: " << "w has wrong dimension (it should be " +
83 std::to_string(nw_) + ")");
84 }
85 ✗ data->u.head(nw_) = w;
86 ✗ data->u.segment(nw_, nw_) = w;
87 ✗ data->u.tail(nw_) = w;
88 }
89
90 template <typename Scalar>
91 161 void ControlParametrizationModelPolyTwoRKTpl<Scalar>::convertBounds(
92 const Eigen::Ref<const VectorXs>& w_lb,
93 const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
94 Eigen::Ref<VectorXs> u_ub) const {
95
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 161 times.
 161 if (static_cast<std::size_t>(u_lb.size()) != nu_) {
96 ✗ throw_pretty(
97 "Invalid argument: " << "u_lb has wrong dimension (it should be " +
98 std::to_string(nu_) + ")");
99 }
100
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 161 times.
 161 if (static_cast<std::size_t>(u_ub.size()) != nu_) {
101 ✗ throw_pretty(
102 "Invalid argument: " << "u_ub has wrong dimension (it should be " +
103 std::to_string(nu_) + ")");
104 }
105
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 161 times.
 161 if (static_cast<std::size_t>(w_lb.size()) != nw_) {
106 ✗ throw_pretty(
107 "Invalid argument: " << "w_lb has wrong dimension (it should be " +
108 std::to_string(nw_) + ")");
109 }
110
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 161 times.
 161 if (static_cast<std::size_t>(w_ub.size()) != nw_) {
111 ✗ throw_pretty(
112 "Invalid argument: " << "w_ub has wrong dimension (it should be " +
113 std::to_string(nw_) + ")");
114 }
115
1/2
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
 161 u_lb.head(nw_) = w_lb;
116
1/2
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
 161 u_lb.segment(nw_, nw_) = w_lb;
117
1/2
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
 161 u_lb.tail(nw_) = w_lb;
118
1/2
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
 161 u_ub.head(nw_) = w_ub;
119
1/2
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
 161 u_ub.segment(nw_, nw_) = w_ub;
120
1/2
✓ Branch 2 taken 161 times.
✗ Branch 3 not taken.
 161 u_ub.tail(nw_) = w_ub;
121 161 }
122
123 template <typename Scalar>
124 692 void ControlParametrizationModelPolyTwoRKTpl<Scalar>::multiplyByJacobian(
125 const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
126 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
127 const AssignmentOp op) const {
128
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 692 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 692 assert_pretty(is_a_AssignmentOp(op),
129 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
130
3/6
✓ Branch 2 taken 692 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 692 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 692 times.
 1384 if (A.rows() != out.rows() || static_cast<std::size_t>(A.cols()) != nw_ ||
131
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 692 times.
 692 static_cast<std::size_t>(out.cols()) != nu_) {
132 ✗ throw_pretty("Invalid argument: " << "A and out have wrong dimensions (" +
133 std::to_string(A.rows()) + "," +
134 std::to_string(A.cols()) +
135 " and " +
136 std::to_string(out.rows()) + "," +
137 std::to_string(out.cols()) + +")");
138 }
139 692 Data* d = static_cast<Data*>(data.get());
140
2/4
✓ Branch 0 taken 577 times.
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 692 switch (op) {
141 577 case setto:
142
2/4
✓ Branch 3 taken 577 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 577 times.
✗ Branch 7 not taken.
 577 out.leftCols(nw_) = d->c[0] * A;
143
2/4
✓ Branch 3 taken 577 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 577 times.
✗ Branch 7 not taken.
 577 out.middleCols(nw_, nw_) = d->c[1] * A;
144
2/4
✓ Branch 3 taken 577 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 577 times.
✗ Branch 7 not taken.
 577 out.rightCols(nw_) = d->c[2] * A;
145 577 break;
146 115 case addto:
147
2/4
✓ Branch 3 taken 115 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
 115 out.leftCols(nw_) += d->c[0] * A;
148
2/4
✓ Branch 3 taken 115 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
 115 out.middleCols(nw_, nw_) += d->c[1] * A;
149
2/4
✓ Branch 3 taken 115 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
 115 out.rightCols(nw_) += d->c[2] * A;
150 115 break;
151 ✗ case rmfrom:
152 ✗ out.leftCols(nw_) -= d->c[0] * A;
153 ✗ out.middleCols(nw_, nw_) -= d->c[1] * A;
154 ✗ out.rightCols(nw_) -= d->c[2] * A;
155 ✗ break;
156 ✗ default:
157 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
158 break;
159 }
160 692 }
161
162 template <typename Scalar>
163 324 void ControlParametrizationModelPolyTwoRKTpl<Scalar>::
164 multiplyJacobianTransposeBy(
165 const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
166 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
167 const AssignmentOp op) const {
168
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 324 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 324 assert_pretty(is_a_AssignmentOp(op),
169 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
170
3/6
✓ Branch 2 taken 324 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 324 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 324 times.
 648 if (A.cols() != out.cols() || static_cast<std::size_t>(A.rows()) != nw_ ||
171
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 324 times.
 324 static_cast<std::size_t>(out.rows()) != nu_) {
172 ✗ throw_pretty("Invalid argument: " << "A and out have wrong dimensions (" +
173 std::to_string(A.rows()) + "," +
174 std::to_string(A.cols()) +
175 " and " +
176 std::to_string(out.rows()) + "," +
177 std::to_string(out.cols()) + ")");
178 }
179 324 Data* d = static_cast<Data*>(data.get());
180
1/4
✓ Branch 0 taken 324 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 324 switch (op) {
181 324 case setto:
182
2/4
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 out.topRows(nw_) = d->c[0] * A;
183
2/4
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 out.middleRows(nw_, nw_) = d->c[1] * A;
184
2/4
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 324 times.
✗ Branch 7 not taken.
 324 out.bottomRows(nw_) = d->c[2] * A;
185 324 break;
186 ✗ case addto:
187 ✗ out.topRows(nw_) += d->c[0] * A;
188 ✗ out.middleRows(nw_, nw_) += d->c[1] * A;
189 ✗ out.bottomRows(nw_) += d->c[2] * A;
190 ✗ break;
191 ✗ case rmfrom:
192 ✗ out.topRows(nw_) -= d->c[0] * A;
193 ✗ out.middleRows(nw_, nw_) -= d->c[1] * A;
194 ✗ out.bottomRows(nw_) -= d->c[2] * A;
195 ✗ break;
196 ✗ default:
197 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
198 break;
199 }
200 324 }
201
202 } // namespace crocoddyl
203