GCC Code Coverage Report

Directory: ./
File: src/waypoints_definition.cpp
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 37 103 35.9%
Branches: 29 194 14.9%
Line Branch Exec Source
1 /*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Pierre Fernbach
4 */
5
6 #ifndef BEZIER_COM_TRAJ_WP_DEF_H
7 #define BEZIER_COM_TRAJ_WP_DEF_H
8
9 #include <hpp/bezier-com-traj/data.hh>
10 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
11 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh>
12 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
13 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0.hh>
14 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh>
15 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh>
16 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh>
17 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh>
18 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh>
19 #include <hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh>
20 #include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
21
22 #include "boost/assign.hpp"
23
24 namespace bezier_com_traj {
25 /**
26 * This file is used to choose the correct expressions of the curves waypoints,
27 * depending on the options set in ProblemData.constraints
28 */
29
30 67 int dimVar(const ProblemData& pData) {
31
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 67 times.
 67 if (pData.constraints_.flag_ & FIVE_FREE_VAR)
32 ✗ return 15;
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 67 times.
 67 else if (pData.constraints_.flag_ & FOUR_FREE_VAR)
34 ✗ return 12;
35
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 67 times.
 67 else if (pData.constraints_.flag_ & THREE_FREE_VAR)
36 ✗ return 9;
37
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 67 times.
 67 else if (pData.constraints_.flag_ & TWO_FREE_VAR)
38 ✗ return 6;
39 else
40 67 return 3;
41 }
42
43 typedef std::pair<double, point3_t> coefs_t;
44 typedef coefs_t (*evalCurveAtTime)(const std::vector<point_t>& pi, double t);
45 typedef std::map<ConstraintFlag, evalCurveAtTime> T_evalCurveAtTime;
46 typedef T_evalCurveAtTime::const_iterator CIT_evalCurveAtTime;
47 static const T_evalCurveAtTime evalCurveAtTimes =
48 boost::assign::map_list_of(c0_dc0_c1::flag, c0_dc0_c1::evaluateCurveAtTime)(
49 c0_dc0_dc1::flag, c0_dc0_dc1::evaluateCurveAtTime)(
50 c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateCurveAtTime)(
51 c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateCurveAtTime)(
52 c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateCurveAtTime)(
53 c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)(
54 c0_dc0_ddc0_ddc1_dc1_c1::flag,
55 c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime)(
56 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
57 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime);
58
59 /** @brief evaluateCurveAtTime compute the expression of the point on the curve
60 * c at t, defined by the waypoint pi and one free waypoint (x)
61 * @param pi constant waypoints of the curve
62 * @param t param (normalized !)
63 * @return the expression of the waypoint such that wp.first . x + wp.second =
64 * point on curve
65 */
66 // TODOin C++ 10, all these methods could be just one function :)
67 414 coefs_t evaluateCurveAtTime(const ProblemData& pData,
68 const std::vector<point_t>& pi, double t) {
69
1/2
✓ Branch 1 taken 414 times.
✗ Branch 2 not taken.
 414 CIT_evalCurveAtTime cit = evalCurveAtTimes.find(pData.constraints_.flag_);
70
1/2
✓ Branch 2 taken 414 times.
✗ Branch 3 not taken.
 414 if (cit != evalCurveAtTimes.end())
71
1/2
✓ Branch 2 taken 414 times.
✗ Branch 3 not taken.
 828 return cit->second(pi, t);
72 else {
73 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
74 ✗ throw std::runtime_error("Current constraints set are not implemented");
75 }
76 }
77
78 typedef coefs_t (*evalAccCurveAtTime)(const std::vector<point_t>& pi, double T,
79 double t);
80 typedef std::map<ConstraintFlag, evalAccCurveAtTime> T_evalAccCurveAtTime;
81 typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
82 static const T_evalAccCurveAtTime evalAccCurveAtTimes =
83 boost::assign::map_list_of(c0_dc0_c1::flag,
84 c0_dc0_c1::evaluateAccelerationCurveAtTime)(
85 c0_dc0_dc1::flag, c0_dc0_dc1::evaluateAccelerationCurveAtTime)(
86 c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)(
87 c0_dc0_ddc0::flag, c0_dc0_ddc0::evaluateAccelerationCurveAtTime)(
88 c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime)(
89 c0_dc0_ddc0_dc1_c1::flag,
90 c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)(
91 c0_dc0_ddc0_ddc1_dc1_c1::flag,
92 c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)(
93 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
94 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
95
96 /** @brief evaluateAccelerationCurveAtTime compute the expression of the point
97 * on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
98 * @param pi constant waypoints of the curve
99 * @param t param (normalized !)
100 * @return the expression of the waypoint such that wp.first . x + wp.second =
101 * point on curve
102 */
103 1242 coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData,
104 const std::vector<point_t>& pi,
105 double T, double t) {
106 CIT_evalAccCurveAtTime cit =
107
1/2
✓ Branch 1 taken 1242 times.
✗ Branch 2 not taken.
 1242 evalAccCurveAtTimes.find(pData.constraints_.flag_);
108
1/2
✓ Branch 2 taken 1242 times.
✗ Branch 3 not taken.
 1242 if (cit != evalAccCurveAtTimes.end())
109
1/2
✓ Branch 2 taken 1242 times.
✗ Branch 3 not taken.
 2484 return cit->second(pi, T, t);
110 else {
111 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
112 ✗ throw std::runtime_error("Current constraints set are not implemented");
113 }
114 }
115
116 typedef waypoint_t (*evalCurveWaypointAtTime)(const std::vector<point_t>& pi,
117 double t);
118 typedef std::map<ConstraintFlag, evalCurveWaypointAtTime>
119 T_evalCurveWaypointAtTime;
120 typedef T_evalCurveWaypointAtTime::const_iterator CIT_evalCurveWaypointAtTime;
121 static const T_evalCurveWaypointAtTime evalCurveWaypointAtTimes =
122 boost::assign::map_list_of(
123 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
124 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
125 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
126 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime)(
127 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
128 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateCurveWaypointAtTime);
129
130 /** @brief evaluateCurveAtTime compute the expression of the point on the curve
131 * c at t, defined by the waypoint pi and one free waypoint (x)
132 * @param pi constant waypoints of the curve
133 * @param t param (normalized !)
134 * @return the expression of the waypoint such that wp.first . x + wp.second =
135 * point on curve
136 */
137 // TODOin C++ 10, all these methods could be just one function :)
138 ✗ waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData,
139 const std::vector<point_t>& pi,
140 double t) {
141 CIT_evalCurveWaypointAtTime cit =
142 ✗ evalCurveWaypointAtTimes.find(pData.constraints_.flag_);
143 ✗ if (cit != evalCurveWaypointAtTimes.end())
144 ✗ return cit->second(pi, t);
145 else {
146 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
147 ✗ throw std::runtime_error("Current constraints set are not implemented");
148 }
149 }
150 typedef waypoint_t (*evalVelCurveWaypointAtTime)(const std::vector<point_t>& pi,
151 const double T, double t);
152 typedef std::map<ConstraintFlag, evalVelCurveWaypointAtTime>
153 T_evalVelCurveWaypointAtTime;
154 typedef T_evalVelCurveWaypointAtTime::const_iterator
155 CIT_evalVelCurveWaypointAtTime;
156 static const T_evalVelCurveWaypointAtTime evalVelCurveWaypointAtTimes =
157 boost::assign::map_list_of(
158 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
159 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
160 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
161 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime)(
162 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
163 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateVelocityCurveWaypointAtTime);
164
165 /** @brief evaluateCurveAtTime compute the expression of the point on the curve
166 * c at t, defined by the waypoint pi and one free waypoint (x)
167 * @param pi constant waypoints of the curve
168 * @param t param (normalized !)
169 * @return the expression of the waypoint such that wp.first . x + wp.second =
170 * point on curve
171 */
172 // TODOin C++ 10, all these methods could be just one function :)
173 ✗ waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData,
174 const double T,
175 const std::vector<point_t>& pi,
176 double t) {
177 CIT_evalVelCurveWaypointAtTime cit =
178 ✗ evalVelCurveWaypointAtTimes.find(pData.constraints_.flag_);
179 ✗ if (cit != evalVelCurveWaypointAtTimes.end())
180 ✗ return cit->second(pi, T, t);
181 else {
182 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
183 ✗ throw std::runtime_error("Current constraints set are not implemented");
184 }
185 }
186 typedef waypoint_t (*evalAccCurveWaypointAtTime)(const std::vector<point_t>& pi,
187 const double T, double t);
188 typedef std::map<ConstraintFlag, evalAccCurveWaypointAtTime>
189 T_evalAccCurveWaypointAtTime;
190 typedef T_evalAccCurveWaypointAtTime::const_iterator
191 CIT_evalAccCurveWaypointAtTime;
192 static const T_evalAccCurveWaypointAtTime evalAccCurveWaypointAtTimes =
193 boost::assign::map_list_of(
194 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
195 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveWaypointAtTime)(
196 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
197 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::
198 evaluateAccelerationCurveWaypointAtTime)(
199 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
200 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::
201 evaluateAccelerationCurveWaypointAtTime);
202
203 /** @brief evaluateCurveAtTime compute the expression of the point on the curve
204 * c at t, defined by the waypoint pi and one free waypoint (x)
205 * @param pi constant waypoints of the curve
206 * @param t param (normalized !)
207 * @return the expression of the waypoint such that wp.first . x + wp.second =
208 * point on curve
209 */
210 // TODOin C++ 10, all these methods could be just one function :)
211 ✗ waypoint_t evaluateAccelerationCurveWaypointAtTime(
212 const ProblemData& pData, const double T, const std::vector<point_t>& pi,
213 double t) {
214 CIT_evalAccCurveWaypointAtTime cit =
215 ✗ evalAccCurveWaypointAtTimes.find(pData.constraints_.flag_);
216 ✗ if (cit != evalAccCurveWaypointAtTimes.end())
217 ✗ return cit->second(pi, T, t);
218 else {
219 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
220 ✗ throw std::runtime_error("Current constraints set are not implemented");
221 }
222 }
223 typedef waypoint_t (*evalJerkCurveWaypointAtTime)(
224 const std::vector<point_t>& pi, const double T, double t);
225 typedef std::map<ConstraintFlag, evalJerkCurveWaypointAtTime>
226 T_evalJerkCurveWaypointAtTime;
227 typedef T_evalJerkCurveWaypointAtTime::const_iterator
228 CIT_evalJerkCurveWaypointAtTime;
229 static const T_evalJerkCurveWaypointAtTime evalJerkCurveWaypointAtTimes =
230 boost::assign::map_list_of(
231 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
232 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
233 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
234 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime)(
235 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
236 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::evaluateJerkCurveWaypointAtTime);
237
238 /** @brief evaluateCurveAtTime compute the expression of the point on the curve
239 * c at t, defined by the waypoint pi and one free waypoint (x)
240 * @param pi constant waypoints of the curve
241 * @param t param (normalized !)
242 * @return the expression of the waypoint such that wp.first . x + wp.second =
243 * point on curve
244 */
245 // TODOin C++ 10, all these methods could be just one function :)
246 ✗ waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData,
247 const double T,
248 const std::vector<point_t>& pi,
249 double t) {
250 CIT_evalJerkCurveWaypointAtTime cit =
251 ✗ evalJerkCurveWaypointAtTimes.find(pData.constraints_.flag_);
252 ✗ if (cit != evalJerkCurveWaypointAtTimes.end())
253 ✗ return cit->second(pi, T, t);
254 else {
255 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
256 ✗ throw std::runtime_error("Current constraints set are not implemented");
257 }
258 }
259
260 typedef std::vector<point_t> (*compConsWp)(const ProblemData& pData, double T);
261 typedef std::map<ConstraintFlag, compConsWp> T_compConsWp;
262 typedef T_compConsWp::const_iterator CIT_compConsWp;
263 static const T_compConsWp compConsWps = boost::assign::map_list_of(
264 c0_dc0_c1::flag, c0_dc0_c1::computeConstantWaypoints)(
265 c0_dc0_dc1::flag, c0_dc0_dc1::computeConstantWaypoints)(
266 c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeConstantWaypoints)(
267 c0_dc0_ddc0::flag, c0_dc0_ddc0::computeConstantWaypoints)(
268 c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeConstantWaypoints)(
269 c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)(
270 c0_dc0_ddc0_ddc1_dc1_c1::flag,
271 c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints)(
272 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
273 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints)(
274 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
275 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeConstantWaypoints)(
276 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
277 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeConstantWaypoints);
278
279 /**
280 * @brief computeConstantWaypoints compute the constant waypoints of c(t)
281 * defined by the constraints on initial and final states
282 * @param pData
283 * @param T
284 * @return
285 */
286 187 std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
287 double T) {
288
1/2
✓ Branch 1 taken 187 times.
✗ Branch 2 not taken.
 187 CIT_compConsWp cit = compConsWps.find(pData.constraints_.flag_);
289
1/2
✓ Branch 2 taken 187 times.
✗ Branch 3 not taken.
 187 if (cit != compConsWps.end())
290
1/2
✓ Branch 2 taken 187 times.
✗ Branch 3 not taken.
 374 return cit->second(pData, T);
291 else {
292 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
293 ✗ throw std::runtime_error("Current constraints set are not implemented");
294 }
295 }
296
297 42 bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(
298 const ProblemData& pData, double T) {
299 42 const int DIM_POINT = 3; // FIXME : always true ??
300
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 const int DIM_VAR = dimVar(pData);
301
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 std::vector<point_t> pts = computeConstantWaypoints(pData, T);
302
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 bezier_wp_t::t_point_t wps;
303
2/2
✓ Branch 4 taken 222 times.
✓ Branch 5 taken 42 times.
 264 for (std::vector<point_t>::const_iterator pit = pts.begin(); pit != pts.end();
304 222 ++pit) {
305
1/2
✓ Branch 1 taken 222 times.
✗ Branch 2 not taken.
 222 waypoint_t w = initwp(DIM_POINT, DIM_VAR);
306
3/4
✓ Branch 3 taken 222 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 44 times.
✓ Branch 6 taken 178 times.
 222 if (pit->isZero()) {
307
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44 times.
✗ Branch 5 not taken.
 44 w.first = MatrixXX::Identity(DIM_POINT, DIM_VAR);
308 } else {
309
1/2
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
 178 w.second = *pit;
310 }
311
1/2
✓ Branch 1 taken 222 times.
✗ Branch 2 not taken.
 222 wps.push_back(w);
312 222 }
313 84 return wps;
314 42 }
315
316 typedef std::vector<waypoint_t> (*compVelWp)(const ProblemData& pData, double T,
317 std::vector<bezier_t::point_t> pi);
318 typedef std::map<ConstraintFlag, compVelWp> T_compVelWp;
319 typedef T_compVelWp::const_iterator CIT_compVelWp;
320 static const T_compVelWp compVelWps = boost::assign::map_list_of(
321 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
322 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
323 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
324 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityWaypoints)(
325 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
326 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityWaypoints);
327
328 /**
329 * @brief computeConstantWaypoints compute the constant waypoints of c(t)
330 * defined by the constraints on initial and final states
331 * @param pData
332 * @param T
333 * @return
334 */
335 ✗ std::vector<waypoint_t> computeVelocityWaypoints(
336 const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
337 ✗ CIT_compVelWp cit = compVelWps.find(pData.constraints_.flag_);
338 ✗ if (cit != compVelWps.end())
339 ✗ return cit->second(pData, T, pi);
340 else {
341 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
342 ✗ throw std::runtime_error("Current constraints set are not implemented");
343 }
344 }
345
346 typedef std::vector<waypoint_t> (*compAccWp)(const ProblemData& pData, double T,
347 std::vector<bezier_t::point_t> pi);
348 typedef std::map<ConstraintFlag, compAccWp> T_compAccWp;
349 typedef T_compAccWp::const_iterator CIT_compAccWp;
350 static const T_compAccWp compAccWps = boost::assign::map_list_of(
351 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
352 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
353 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
354 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeAccelerationWaypoints)(
355 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
356 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
357
358 /**
359 * @brief computeConstantWaypoints compute the constant waypoints of c(t)
360 * defined by the constraints on initial and final states
361 * @param pData
362 * @param T
363 * @return
364 */
365 ✗ std::vector<waypoint_t> computeAccelerationWaypoints(
366 const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
367 ✗ CIT_compAccWp cit = compAccWps.find(pData.constraints_.flag_);
368 ✗ if (cit != compAccWps.end())
369 ✗ return cit->second(pData, T, pi);
370 else {
371 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
372 ✗ throw std::runtime_error("Current constraints set are not implemented");
373 }
374 }
375
376 typedef std::vector<waypoint_t> (*compJerkWp)(
377 const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
378 typedef std::map<ConstraintFlag, compJerkWp> T_compJerkWp;
379 typedef T_compJerkWp::const_iterator CIT_compJerkWp;
380 static const T_compJerkWp compJerkWps = boost::assign::map_list_of(
381 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
382 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints)(
383 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
384 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeJerkWaypoints)(
385 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
386 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeJerkWaypoints);
387
388 /**
389 * @brief computeConstantWaypoints compute the constant waypoints of c(t)
390 * defined by the constraints on initial and final states
391 * @param pData
392 * @param T
393 * @return
394 */
395 ✗ std::vector<waypoint_t> computeJerkWaypoints(
396 const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
397 ✗ CIT_compJerkWp cit = compJerkWps.find(pData.constraints_.flag_);
398 ✗ if (cit != compJerkWps.end())
399 ✗ return cit->second(pData, T, pi);
400 else {
401 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
402 ✗ throw std::runtime_error("Current constraints set are not implemented");
403 }
404 }
405
406 typedef bezier_wp_t::t_point_t (*compWp)(const ProblemData& pData, double T);
407 typedef std::map<ConstraintFlag, compWp> T_compWp;
408 typedef T_compWp::const_iterator CIT_compWp;
409 static const T_compWp compWps = boost::assign::map_list_of(
410 c0_dc0_c1::flag, c0_dc0_c1::computeWwaypoints)(
411 c0_dc0_dc1::flag, c0_dc0_dc1::computeWwaypoints)(
412 c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeWwaypoints)(
413 c0_dc0_ddc0::flag, c0_dc0_ddc0::computeWwaypoints)(
414 c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeWwaypoints)(
415 c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeWwaypoints)(
416 c0_dc0_ddc0_ddc1_dc1_c1::flag, c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints)(
417 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
418 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints);
419
420 /**
421 * @brief computeWwaypoints compute the constant waypoints of w(t) defined by
422 * the constraints on initial and final states
423 * @param pData
424 * @param T
425 * @return
426 */
427 63 bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
428
1/2
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
 63 CIT_compWp cit = compWps.find(pData.constraints_.flag_);
429
1/2
✓ Branch 2 taken 63 times.
✗ Branch 3 not taken.
 63 if (cit != compWps.end())
430
1/2
✓ Branch 2 taken 63 times.
✗ Branch 3 not taken.
 126 return cit->second(pData, T);
431 else {
432 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
433 ✗ throw std::runtime_error("Current constraints set are not implemented");
434 }
435 }
436
437 typedef coefs_t (*compFinalVelP)(const ProblemData& pData, double T);
438 typedef std::map<ConstraintFlag, compFinalVelP> T_compFinalVelP;
439 typedef T_compFinalVelP::const_iterator CIT_compFinalVelP;
440 static const T_compFinalVelP compFinalVelPs = boost::assign::map_list_of(
441 c0_dc0_c1::flag, c0_dc0_c1::computeFinalVelocityPoint)(
442 c0_dc0_dc1::flag, c0_dc0_dc1::computeFinalVelocityPoint)(
443 c0_dc0_dc1_c1::flag, c0_dc0_dc1_c1::computeFinalVelocityPoint)(
444 c0_dc0_ddc0::flag, c0_dc0_ddc0::computeFinalVelocityPoint)(
445 c0_dc0_ddc0_c1::flag, c0_dc0_ddc0_c1::computeFinalVelocityPoint)(
446 c0_dc0_ddc0_dc1_c1::flag, c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)(
447 c0_dc0_ddc0_ddc1_dc1_c1::flag,
448 c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
449
450 ✗ coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
451 ✗ CIT_compFinalVelP cit = compFinalVelPs.find(pData.constraints_.flag_);
452 ✗ if (cit != compFinalVelPs.end())
453 ✗ return cit->second(pData, T);
454 else {
455 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
456 ✗ throw std::runtime_error("Current constraints set are not implemented");
457 }
458 }
459
460 typedef std::pair<MatrixXX, VectorX> (*compVelCost)(
461 const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi);
462 typedef std::map<ConstraintFlag, compVelCost> T_compVelCost;
463 typedef T_compVelCost::const_iterator CIT_compVelCost;
464 static const T_compVelCost compVelCosts = boost::assign::map_list_of(
465 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag,
466 c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityCost)(
467 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag,
468 c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityCost)(
469 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::flag,
470 c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1::computeVelocityCost);
471
472 /**
473 * @brief computeVelocityCost the matrices H and g defining a cost that minimise
474 * the integral of the squared velocity
475 * @param pData
476 * @param T
477 * @return
478 */
479 ✗ std::pair<MatrixXX, VectorX> computeVelocityCost(
480 const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi) {
481 ✗ CIT_compVelCost cit = compVelCosts.find(pData.constraints_.flag_);
482 ✗ if (cit != compVelCosts.end())
483 ✗ return cit->second(pData, T, pi);
484 else {
485 ✗ std::cout << "Current constraints set are not implemented" << std::endl;
486 ✗ throw std::runtime_error("Current constraints set are not implemented");
487 }
488 }
489
490 } // namespace bezier_com_traj
491
492 #endif
493