GCC Code Coverage Report

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