| Directory: | ./ |
|---|---|
| File: | src/waypoints_definition.cpp |
| Date: | 2025-04-18 04:07:36 |
| 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 |