GCC Code Coverage Report

Directory: ./
File: src/utils.cpp
Date: 2025-04-18 04:07:36
Exec Total Coverage
Lines: 63 92 68.5%
Branches: 58 232 25.0%
Line Branch Exec Source
1 /*
2 * Copyright 2017, LAAS-CNRS
3 * Author: Steve Tonneau
4 */
5
6 #include <hpp/bezier-com-traj/utils.hh>
7 namespace bezier_com_traj {
8
9 1055 waypoint_t initwp(const size_t rows, const size_t cols) {
10 1055 waypoint_t w;
11
2/4
✓ Branch 1 taken 1055 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1055 times.
✗ Branch 5 not taken.
 1055 w.first = MatrixXX::Zero(rows, cols);
12
2/4
✓ Branch 1 taken 1055 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1055 times.
✗ Branch 5 not taken.
 1055 w.second = VectorX::Zero(rows);
13 1055 return w;
14 }
15
16 2596637 waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2) {
17
1/2
✓ Branch 2 taken 2596637 times.
✗ Branch 3 not taken.
 5193274 if (w1.second.rows() != w2.second.rows() ||
18
3/6
✓ Branch 0 taken 2596637 times.
✗ Branch 1 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2596637 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2596637 times.
 5193274 w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
19 ✗ throw std::runtime_error("You cannot add waypoint_t of different size.");
20
4/8
✓ Branch 2 taken 2596637 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2596637 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2596637 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2596637 times.
✗ Branch 12 not taken.
 2596637 return waypoint_t(w1.first + w2.first, w1.second + w2.second);
21 }
22
23 296 waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2) {
24
1/2
✓ Branch 2 taken 296 times.
✗ Branch 3 not taken.
 592 if (w1.second.rows() != w2.second.rows() ||
25
3/6
✓ Branch 0 taken 296 times.
✗ Branch 1 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 296 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 296 times.
 592 w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
26 ✗ throw std::runtime_error("You cannot add waypoint_t of different size.");
27
4/8
✓ Branch 2 taken 296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 296 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 296 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 296 times.
✗ Branch 12 not taken.
 296 return waypoint_t(w1.first - w2.first, w1.second - w2.second);
28 }
29
30 5107358 waypoint_t operator*(const double k, const waypoint_t& w) {
31
4/8
✓ Branch 2 taken 5107358 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5107358 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5107358 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5107358 times.
✗ Branch 12 not taken.
 5107358 return waypoint_t(k * w.first, k * w.second);
32 }
33
34 105726 waypoint_t operator*(const waypoint_t& w, const double k) {
35
4/8
✓ Branch 2 taken 105726 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 105726 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 105726 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 105726 times.
✗ Branch 12 not taken.
 105726 return waypoint_t(k * w.first, k * w.second);
36 }
37
38 template <>
39 ✗ waypoint9_t initwp<waypoint9_t>() {
40 ✗ waypoint9_t w;
41 ✗ w.first = Matrix39::Zero();
42 ✗ w.second = point3_t::Zero();
43 ✗ return w;
44 }
45
46 template <>
47 5 waypoint6_t initwp<waypoint6_t>() {
48 5 waypoint6_t w;
49
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 w.first = matrix6_t::Zero();
50
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 w.second = point6_t::Zero();
51 5 return w;
52 }
53
54 template <>
55 ✗ waypoint3_t initwp<waypoint3_t>() {
56 ✗ waypoint3_t w;
57 ✗ w.first = matrix3_t::Zero();
58 ✗ w.second = point3_t::Zero();
59 ✗ return w;
60 }
61
62 6959 Matrix3 skew(point_t_tC x) {
63
1/2
✓ Branch 2 taken 6959 times.
✗ Branch 3 not taken.
 6959 Matrix3 res = Matrix3::Zero();
64 6959 res(0, 1) = -x(2);
65 6959 res(0, 2) = x(1);
66 6959 res(1, 0) = x(2);
67 6959 res(1, 2) = -x(0);
68 6959 res(2, 0) = -x(1);
69 6959 res(2, 1) = x(0);
70 6959 return res;
71 }
72
73 25 std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(
74 const unsigned int degree) {
75 25 std::vector<ndcurves::Bern<double> > res;
76
2/2
✓ Branch 0 taken 165 times.
✓ Branch 1 taken 25 times.
 190 for (unsigned int i = 0; i <= (unsigned int)degree; ++i)
77
2/4
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
 165 res.push_back(ndcurves::Bern<double>(degree, i));
78 25 return res;
79 }
80
81 59 T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
82 const unsigned int pointsPerPhase) {
83 59 T_time timeArray;
84 59 double t = 0;
85
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 double t_total = phaseTimings.sum();
86
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 59 timeArray.push_back(std::make_pair(0., 0));
87
2/2
✓ Branch 1 taken 171 times.
✓ Branch 2 taken 59 times.
 230 for (int i = 0; i < phaseTimings.size(); ++i) {
88
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 171 double step = (double)phaseTimings[i] / pointsPerPhase;
89
2/2
✓ Branch 0 taken 1063 times.
✓ Branch 1 taken 171 times.
 1234 for (size_t j = 0; j < pointsPerPhase; ++j) {
90 1063 t += step;
91
2/4
✓ Branch 1 taken 1063 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1063 times.
✗ Branch 5 not taken.
 1063 timeArray.push_back(std::make_pair(t, i));
92 }
93 }
94 59 timeArray.pop_back();
95
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 timeArray.push_back(std::make_pair(
96
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 t_total, phaseTimings.size() - 1)); // avoid numerical errors
97 118 return timeArray;
98 }
99
100 2 T_time computeDiscretizedTime(const VectorX& phaseTimings,
101 const double timeStep) {
102 2 T_time timeArray;
103 2 double t = 0;
104 2 double currentTiming = 0.;
105
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
 8 for (int i = 0; i < phaseTimings.size(); ++i) {
106
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 assert(timeStep * 2 <= phaseTimings[i] &&
107 "Time step too high: should allow to contain at least 2 points per "
108 "phase");
109 6 t = currentTiming;
110
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 currentTiming += phaseTimings[i];
111
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 6 times.
 56 while (t < currentTiming) {
112
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 timeArray.push_back(std::make_pair(t, i));
113 50 t += timeStep;
114 }
115
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 timeArray.push_back(std::make_pair(currentTiming, i));
116 }
117 4 return timeArray;
118 }
119
120 ✗ void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
121 const std::string& fileName, bool clipZ) {
122 ✗ std::ofstream file;
123 using std::endl;
124 ✗ std::string path("/local/fernbac/bench_iros18/constraints_obj/");
125 ✗ path.append(fileName);
126 ✗ file.open(path.c_str(), std::ios::out | std::ios::trunc);
127 ✗ file << "3 1" << endl;
128 ✗ file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2]
129 ✗ << endl;
130 ✗ file << "4" << endl;
131 ✗ clipZ ? file << Ab.first.rows() + 2 << endl : file << Ab.first.rows() << endl;
132 ✗ for (int i = 0; i < Ab.first.rows(); ++i) {
133 ✗ file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t"
134 ✗ << Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001 << endl;
135 }
136 ✗ if (clipZ) {
137 ✗ file << "\t" << 0 << "\t" << 0 << "\t" << 1. << "\t" << -3. << endl;
138 ✗ file << "\t" << 0 << "\t" << 0 << "\t" << -1. << "\t" << -1. << endl;
139 }
140 ✗ file.close();
141 }
142
143 } // namespace bezier_com_traj
144