GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/states/euclidean.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 60 86 69.8%
Branches: 63 368 17.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
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 489 StateVectorTpl<Scalar>::StateVectorTpl(const std::size_t nx)
14 489 : StateAbstractTpl<Scalar>(nx, nx) {}
15
16 template <typename Scalar>
17 992 StateVectorTpl<Scalar>::~StateVectorTpl() {}
18
19 template <typename Scalar>
20 2282 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::zero() const {
21
1/2
✓ Branch 2 taken 2282 times.
✗ Branch 3 not taken.
 2282 return VectorXs::Zero(nx_);
22 }
23
24 template <typename Scalar>
25 1967 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::rand() const {
26
1/2
✓ Branch 2 taken 1967 times.
✗ Branch 3 not taken.
 1967 return VectorXs::Random(nx_);
27 }
28
29 template <typename Scalar>
30 9230 void StateVectorTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
31 const Eigen::Ref<const VectorXs>& x1,
32 Eigen::Ref<VectorXs> dxout) const {
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9230 times.
 9230 if (static_cast<std::size_t>(x0.size()) != nx_) {
34 ✗ throw_pretty(
35 "Invalid argument: " << "x0 has wrong dimension (it should be " +
36 std::to_string(nx_) + ")");
37 }
38
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9230 times.
 9230 if (static_cast<std::size_t>(x1.size()) != nx_) {
39 ✗ throw_pretty(
40 "Invalid argument: " << "x1 has wrong dimension (it should be " +
41 std::to_string(nx_) + ")");
42 }
43
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9230 times.
 9230 if (static_cast<std::size_t>(dxout.size()) != ndx_) {
44 ✗ throw_pretty(
45 "Invalid argument: " << "dxout has wrong dimension (it should be " +
46 std::to_string(ndx_) + ")");
47 }
48
1/2
✓ Branch 2 taken 9230 times.
✗ Branch 3 not taken.
 9230 dxout = x1 - x0;
49 9230 }
50
51 template <typename Scalar>
52 25240 void StateVectorTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
53 const Eigen::Ref<const VectorXs>& dx,
54 Eigen::Ref<VectorXs> xout) const {
55
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25240 times.
 25240 if (static_cast<std::size_t>(x.size()) != nx_) {
56 ✗ throw_pretty(
57 "Invalid argument: " << "x has wrong dimension (it should be " +
58 std::to_string(nx_) + ")");
59 }
60
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25240 times.
 25240 if (static_cast<std::size_t>(dx.size()) != ndx_) {
61 ✗ throw_pretty(
62 "Invalid argument: " << "dx has wrong dimension (it should be " +
63 std::to_string(ndx_) + ")");
64 }
65
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25240 times.
 25240 if (static_cast<std::size_t>(xout.size()) != nx_) {
66 ✗ throw_pretty(
67 "Invalid argument: " << "xout has wrong dimension (it should be " +
68 std::to_string(nx_) + ")");
69 }
70
1/2
✓ Branch 2 taken 25240 times.
✗ Branch 3 not taken.
 25240 xout = x + dx;
71 25240 }
72
73 template <typename Scalar>
74 196 void StateVectorTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>&,
75 const Eigen::Ref<const VectorXs>&,
76 Eigen::Ref<MatrixXs> Jfirst,
77 Eigen::Ref<MatrixXs> Jsecond,
78 const Jcomponent firstsecond) const {
79
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 196 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.
 196 assert_pretty(
80 is_a_Jcomponent(firstsecond),
81 ("firstsecond must be one of the Jcomponent {both, first, second}"));
82
4/4
✓ Branch 0 taken 177 times.
✓ Branch 1 taken 19 times.
✓ Branch 2 taken 175 times.
✓ Branch 3 taken 2 times.
 196 if (firstsecond == first || firstsecond == both) {
83
2/4
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 194 times.
 388 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
84
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 194 times.
 194 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
85 ✗ throw_pretty(
86 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
87 std::to_string(ndx_) + "," +
88 std::to_string(ndx_) + ")");
89 }
90 194 Jfirst.setZero();
91
3/6
✓ Branch 1 taken 194 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 194 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 194 times.
✗ Branch 8 not taken.
 194 Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, Scalar(-1.));
92 }
93
4/4
✓ Branch 0 taken 194 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 175 times.
✓ Branch 3 taken 19 times.
 196 if (firstsecond == second || firstsecond == both) {
94
2/4
✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 177 times.
 354 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
95
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 177 times.
 177 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
96 ✗ throw_pretty(
97 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
98 std::to_string(ndx_) + "," +
99 std::to_string(ndx_) + ")");
100 }
101 177 Jsecond.setZero();
102
3/6
✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 177 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 177 times.
✗ Branch 8 not taken.
 177 Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, Scalar(1.));
103 }
104 196 }
105
106 template <typename Scalar>
107 2519 void StateVectorTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>&,
108 const Eigen::Ref<const VectorXs>&,
109 Eigen::Ref<MatrixXs> Jfirst,
110 Eigen::Ref<MatrixXs> Jsecond,
111 const Jcomponent firstsecond,
112 const AssignmentOp op) const {
113
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 2519 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.
 2519 assert_pretty(
114 is_a_Jcomponent(firstsecond),
115 ("firstsecond must be one of the Jcomponent {both, first, second}"));
116
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 2519 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.
 2519 assert_pretty(is_a_AssignmentOp(op),
117 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
118
4/4
✓ Branch 0 taken 640 times.
✓ Branch 1 taken 1879 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 622 times.
 2519 if (firstsecond == first || firstsecond == both) {
119
2/4
✓ Branch 1 taken 1897 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1897 times.
 3794 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
120
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1897 times.
 1897 static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
121 ✗ throw_pretty(
122 "Invalid argument: " << "Jfirst has wrong dimension (it should be " +
123 std::to_string(ndx_) + "," +
124 std::to_string(ndx_) + ")");
125 }
126
2/4
✓ Branch 0 taken 19 times.
✓ Branch 1 taken 1878 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1897 switch (op) {
127 19 case setto:
128
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 19 Jfirst.diagonal().array() = Scalar(1.);
129 19 break;
130 1878 case addto:
131
3/6
✓ Branch 1 taken 1878 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1878 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1878 times.
✗ Branch 8 not taken.
 1878 Jfirst.diagonal().array() += Scalar(1.);
132 1878 break;
133 ✗ case rmfrom:
134 ✗ Jfirst.diagonal().array() -= Scalar(1.);
135 ✗ break;
136 ✗ default:
137 ✗ throw_pretty(
138 "Invalid argument: allowed operators: setto, addto, rmfrom");
139 break;
140 }
141 }
142
4/4
✓ Branch 0 taken 1897 times.
✓ Branch 1 taken 622 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 1879 times.
 2519 if (firstsecond == second || firstsecond == both) {
143
2/4
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 640 times.
 1280 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
144
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 640 times.
 640 static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
145 ✗ throw_pretty(
146 "Invalid argument: " << "Jsecond has wrong dimension (it should be " +
147 std::to_string(ndx_) + "," +
148 std::to_string(ndx_) + ")");
149 }
150
1/4
✓ Branch 0 taken 640 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 640 switch (op) {
151 640 case setto:
152
3/6
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 640 times.
✗ Branch 8 not taken.
 640 Jsecond.diagonal().array() = Scalar(1.);
153 640 break;
154 ✗ case addto:
155 ✗ Jsecond.diagonal().array() += Scalar(1.);
156 ✗ break;
157 ✗ case rmfrom:
158 ✗ Jsecond.diagonal().array() -= Scalar(1.);
159 ✗ break;
160 ✗ default:
161 ✗ throw_pretty(
162 "Invalid argument: allowed operators: setto, addto, rmfrom");
163 break;
164 }
165 }
166 2519 }
167
168 template <typename Scalar>
169 3758 void StateVectorTpl<Scalar>::JintegrateTransport(
170 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&,
171 Eigen::Ref<MatrixXs>, const Jcomponent firstsecond) const {
172
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 3758 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.
 3758 assert_pretty(is_a_Jcomponent(firstsecond), (""));
173
3/4
✓ Branch 0 taken 3757 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3757 times.
 3758 if (firstsecond != first && firstsecond != second) {
174 ✗ throw_pretty(
175 "Invalid argument: firstsecond must be either first or second. both "
176 "not supported for this operation.");
177 }
178 3758 }
179
180 template <typename Scalar>
181 template <typename NewScalar>
182 5 StateVectorTpl<NewScalar> StateVectorTpl<Scalar>::cast() const {
183 typedef StateVectorTpl<NewScalar> ReturnType;
184 5 ReturnType res(nx_);
185 5 return res;
186 }
187
188 template <typename Scalar>
189 ✗ void StateVectorTpl<Scalar>::print(std::ostream& os) const {
190 ✗ os << "StateVector {nx=" << nx_ << "}";
191 }
192
193 } // namespace crocoddyl
194