GCC Code Coverage Report

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