GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/controls/poly-one.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 55 85 64.7%
Branches: 48 324 14.8%
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 260 ControlParametrizationModelPolyOneTpl<
14 Scalar>::ControlParametrizationModelPolyOneTpl(const std::size_t nw)
15 260 : Base(nw, 2 * nw) {}
16
17 template <typename Scalar>
18 16576 void ControlParametrizationModelPolyOneTpl<Scalar>::calc(
19 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
20 const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
21
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16576 times.
 16576 if (static_cast<std::size_t>(u.size()) != nu_) {
22 ✗ throw_pretty(
23 "Invalid argument: " << "u has wrong dimension (it should be " +
24 std::to_string(nu_) + ")");
25 }
26 16576 Data* d = static_cast<Data*>(data.get());
27 16576 d->c[1] = Scalar(2.) * t;
28 16576 d->c[0] = Scalar(1.) - d->c[1];
29
7/14
✓ Branch 2 taken 16576 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16576 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16576 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16576 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 16576 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 16576 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 16576 times.
✗ Branch 22 not taken.
 16576 data->w = d->c[0] * u.head(nw_) + d->c[1] * u.tail(nw_);
30 16576 }
31
32 template <typename Scalar>
33 6 void ControlParametrizationModelPolyOneTpl<Scalar>::calcDiff(
34 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
35 const Scalar, const Eigen::Ref<const VectorXs>&) const {
36 6 Data* d = static_cast<Data*>(data.get());
37
3/6
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 data->dw_du.leftCols(nw_).diagonal().array() = d->c[0];
38
3/6
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 data->dw_du.rightCols(nw_).diagonal().array() = d->c[1];
39 6 }
40
41 template <typename Scalar>
42 std::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
43 16785 ControlParametrizationModelPolyOneTpl<Scalar>::createData() {
44
1/2
✓ Branch 2 taken 16785 times.
✗ Branch 3 not taken.
 16785 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
45 }
46
47 template <typename Scalar>
48 ✗ void ControlParametrizationModelPolyOneTpl<Scalar>::params(
49 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
50 const Scalar, const Eigen::Ref<const VectorXs>& w) const {
51 ✗ if (static_cast<std::size_t>(w.size()) != nw_) {
52 ✗ throw_pretty(
53 "Invalid argument: " << "w has wrong dimension (it should be " +
54 std::to_string(nw_) + ")");
55 }
56 ✗ data->u.head(nw_) = w;
57 ✗ data->u.tail(nw_) = w;
58 }
59
60 template <typename Scalar>
61 253 void ControlParametrizationModelPolyOneTpl<Scalar>::convertBounds(
62 const Eigen::Ref<const VectorXs>& w_lb,
63 const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
64 Eigen::Ref<VectorXs> u_ub) const {
65
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 253 times.
 253 if (static_cast<std::size_t>(u_lb.size()) != nu_) {
66 ✗ throw_pretty(
67 "Invalid argument: " << "u_lb has wrong dimension (it should be " +
68 std::to_string(nu_) + ")");
69 }
70
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 253 times.
 253 if (static_cast<std::size_t>(u_ub.size()) != nu_) {
71 ✗ throw_pretty(
72 "Invalid argument: " << "u_ub has wrong dimension (it should be " +
73 std::to_string(nu_) + ")");
74 }
75
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 253 times.
 253 if (static_cast<std::size_t>(w_lb.size()) != nw_) {
76 ✗ throw_pretty(
77 "Invalid argument: " << "w_lb has wrong dimension (it should be " +
78 std::to_string(nw_) + ")");
79 }
80
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 253 times.
 253 if (static_cast<std::size_t>(w_ub.size()) != nw_) {
81 ✗ throw_pretty(
82 "Invalid argument: " << "w_ub has wrong dimension (it should be " +
83 std::to_string(nw_) + ")");
84 }
85
1/2
✓ Branch 2 taken 253 times.
✗ Branch 3 not taken.
 253 u_lb.head(nw_) = w_lb;
86
1/2
✓ Branch 2 taken 253 times.
✗ Branch 3 not taken.
 253 u_lb.tail(nw_) = w_lb;
87
1/2
✓ Branch 2 taken 253 times.
✗ Branch 3 not taken.
 253 u_ub.head(nw_) = w_ub;
88
1/2
✓ Branch 2 taken 253 times.
✗ Branch 3 not taken.
 253 u_ub.tail(nw_) = w_ub;
89 253 }
90
91 template <typename Scalar>
92 899 void ControlParametrizationModelPolyOneTpl<Scalar>::multiplyByJacobian(
93 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
94 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
95 const AssignmentOp op) const {
96
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 899 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.
 899 assert_pretty(is_a_AssignmentOp(op),
97 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
98
3/6
✓ Branch 2 taken 899 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 899 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 899 times.
 1798 if (A.rows() != out.rows() || static_cast<std::size_t>(A.cols()) != nw_ ||
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 899 times.
 899 static_cast<std::size_t>(out.cols()) != nu_) {
100 ✗ throw_pretty("Invalid argument: " << "A and out have wrong dimensions (" +
101 std::to_string(A.rows()) + "," +
102 std::to_string(A.cols()) +
103 " and " +
104 std::to_string(out.rows()) + "," +
105 std::to_string(out.cols()) + +")");
106 }
107 899 Data* d = static_cast<Data*>(data.get());
108
2/4
✓ Branch 0 taken 761 times.
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 899 switch (op) {
109 761 case setto:
110
2/4
✓ Branch 3 taken 761 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 761 times.
✗ Branch 7 not taken.
 761 out.leftCols(nw_) = d->c[0] * A;
111
2/4
✓ Branch 3 taken 761 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 761 times.
✗ Branch 7 not taken.
 761 out.rightCols(nw_) = d->c[1] * A;
112 761 break;
113 138 case addto:
114
2/4
✓ Branch 3 taken 138 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 138 times.
✗ Branch 7 not taken.
 138 out.leftCols(nw_) += d->c[0] * A;
115
2/4
✓ Branch 3 taken 138 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 138 times.
✗ Branch 7 not taken.
 138 out.rightCols(nw_) += d->c[1] * A;
116 138 break;
117 ✗ case rmfrom:
118 ✗ out.leftCols(nw_) -= d->c[0] * A;
119 ✗ out.rightCols(nw_) -= d->c[1] * A;
120 ✗ break;
121 ✗ default:
122 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
123 break;
124 }
125 899 }
126
127 template <typename Scalar>
128 416 void ControlParametrizationModelPolyOneTpl<Scalar>::multiplyJacobianTransposeBy(
129 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
130 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
131 const AssignmentOp op) const {
132
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 416 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.
 416 assert_pretty(is_a_AssignmentOp(op),
133 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
134
3/6
✓ Branch 2 taken 416 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 416 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 416 times.
 832 if (A.cols() != out.cols() || static_cast<std::size_t>(A.rows()) != nw_ ||
135
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 416 times.
 416 static_cast<std::size_t>(out.rows()) != nu_) {
136 ✗ throw_pretty("Invalid argument: " << "A and out have wrong dimensions (" +
137 std::to_string(A.rows()) + "," +
138 std::to_string(A.cols()) +
139 " and " +
140 std::to_string(out.rows()) + "," +
141 std::to_string(out.cols()) + ")");
142 }
143 416 Data* d = static_cast<Data*>(data.get());
144
1/4
✓ Branch 0 taken 416 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 416 switch (op) {
145 416 case setto:
146
2/4
✓ Branch 3 taken 416 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 416 times.
✗ Branch 7 not taken.
 416 out.topRows(nw_) = d->c[0] * A;
147
2/4
✓ Branch 3 taken 416 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 416 times.
✗ Branch 7 not taken.
 416 out.bottomRows(nw_) = d->c[1] * A;
148 416 break;
149 ✗ case addto:
150 ✗ out.topRows(nw_) += d->c[0] * A;
151 ✗ out.bottomRows(nw_) += d->c[1] * A;
152 ✗ break;
153 ✗ case rmfrom:
154 ✗ out.topRows(nw_) -= d->c[0] * A;
155 ✗ out.bottomRows(nw_) -= d->c[1] * A;
156 ✗ break;
157 ✗ default:
158 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
159 break;
160 }
161 416 }
162
163 template <typename Scalar>
164 template <typename NewScalar>
165 ControlParametrizationModelPolyOneTpl<NewScalar>
166 3 ControlParametrizationModelPolyOneTpl<Scalar>::cast() const {
167 typedef ControlParametrizationModelPolyOneTpl<NewScalar> ReturnType;
168 3 ReturnType ret(nw_);
169 3 return ret;
170 }
171
172 template <typename Scalar>
173 ✗ void ControlParametrizationModelPolyOneTpl<Scalar>::print(
174 std::ostream& os) const {
175 ✗ os << "ControlParametrizationModelPolyZero {nw=" << nw_ << "}";
176 }
177
178 } // namespace crocoddyl
179