Directory: | ./ |
---|---|
File: | src/utils.cpp |
Date: | 2025-03-18 04:20:50 |
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 | 2601539 | waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2) { | |
17 |
1/2✓ Branch 2 taken 2601539 times.
✗ Branch 3 not taken.
|
5203078 | if (w1.second.rows() != w2.second.rows() || |
18 |
3/6✓ Branch 0 taken 2601539 times.
✗ Branch 1 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2601539 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2601539 times.
|
5203078 | 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 2601539 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2601539 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2601539 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2601539 times.
✗ Branch 12 not taken.
|
2601539 | 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 | 5112260 | waypoint_t operator*(const double k, const waypoint_t& w) { | |
31 |
4/8✓ Branch 2 taken 5112260 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5112260 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5112260 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5112260 times.
✗ Branch 12 not taken.
|
5112260 | return waypoint_t(k * w.first, k * w.second); |
32 | } | ||
33 | |||
34 | 111752 | waypoint_t operator*(const waypoint_t& w, const double k) { | |
35 |
4/8✓ Branch 2 taken 111752 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 111752 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 111752 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 111752 times.
✗ Branch 12 not taken.
|
111752 | 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 |