| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Copyright 2018, LAAS-CNRS | ||
| 3 | * Author: Pierre Fernbach | ||
| 4 | */ | ||
| 5 | |||
| 6 | #ifndef BEZIER_COM_TRAJ_C0DC0D1_H | ||
| 7 | #define BEZIER_COM_TRAJ_C0DC0D1_H | ||
| 8 | |||
| 9 | #include <hpp/bezier-com-traj/data.hh> | ||
| 10 | |||
| 11 | namespace bezier_com_traj { | ||
| 12 | namespace c0_dc0_dc1 { | ||
| 13 | |||
| 14 | static const ConstraintFlag flag = INIT_POS | INIT_VEL | END_VEL; | ||
| 15 | |||
| 16 | /// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE | ||
| 17 | /// = 4) | ||
| 18 | /** @brief evaluateCurveAtTime compute the expression of the point on the curve | ||
| 19 | * at t, defined by the waypoint pi and one free waypoint (x) | ||
| 20 | * @param pi constant waypoints of the curve, assume p0 p1 x p2 p3 | ||
| 21 | * @param t param (normalized !) | ||
| 22 | * @return the expression of the waypoint such that wp.first . x + wp.second = | ||
| 23 | * point on curve | ||
| 24 | */ | ||
| 25 | 72 | inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) { | |
| 26 |
1/2✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
|
72 | coefs_t wp; |
| 27 | 72 | double t2 = t * t; | |
| 28 | 72 | double t3 = t2 * t; | |
| 29 | // equation found with sympy | ||
| 30 | 72 | wp.first = -2.0 * t3 + 3.0 * t2; | |
| 31 |
9/18✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 72 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 72 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 72 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 72 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 72 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 72 times.
✗ Branch 29 not taken.
|
72 | wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t + |
| 32 |
8/16✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 72 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 72 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 72 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 72 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 72 times.
✗ Branch 26 not taken.
|
144 | 1.0 * pi[0] + 3.0 * pi[1] * t3 - 6.0 * pi[1] * t2 + |
| 33 |
3/6✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 72 times.
✗ Branch 9 not taken.
|
144 | 3.0 * pi[1] * t; |
| 34 | 144 | return wp; | |
| 35 | } | ||
| 36 | |||
| 37 | 116 | inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, | |
| 38 | double T, double t) { | ||
| 39 |
1/2✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
|
116 | coefs_t wp; |
| 40 | 116 | double alpha = 1. / (T * T); | |
| 41 | // equation found with sympy | ||
| 42 | 116 | wp.first = (-12.0 * t + 6.0) * alpha; | |
| 43 | wp.second = | ||
| 44 |
10/20✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 116 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 116 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 116 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 116 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 116 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 116 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 116 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 116 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 116 times.
✗ Branch 33 not taken.
|
116 | (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1]) * |
| 45 |
1/2✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
|
116 | alpha; |
| 46 | 232 | return wp; | |
| 47 | } | ||
| 48 | |||
| 49 | 20 | inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, | |
| 50 | double T) { | ||
| 51 | // equation for constraint on initial and final position and velocity (degree | ||
| 52 | // 3, 2 constant waypoints and two free (p2 = p3)) first, compute the constant | ||
| 53 | // waypoints that only depend on pData : | ||
| 54 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
|
20 | if (pData.dc1_.norm() != 0.) |
| 55 | throw std::runtime_error( | ||
| 56 | ✗ | "Capturability not implemented for spped different than 0"); | |
| 57 | 20 | std::vector<point_t> pi; | |
| 58 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | pi.push_back(pData.c0_); // p0 |
| 59 |
5/10✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
|
20 | pi.push_back((pData.dc0_ * T / 3.) + pData.c0_); // p1 |
| 60 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
20 | pi.push_back(point_t::Zero()); // p2 = x |
| 61 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
20 | pi.push_back(point_t::Zero()); // p3 = x |
| 62 | 20 | return pi; | |
| 63 | ✗ | } | |
| 64 | |||
| 65 | 5 | inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, | |
| 66 | double T) { | ||
| 67 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | bezier_wp_t::t_point_t wps; |
| 68 | 5 | const int DIM_POINT = 6; | |
| 69 | 5 | const int DIM_VAR = 3; | |
| 70 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData, T); |
| 71 | 5 | std::vector<Matrix3> Cpi; | |
| 72 |
2/2✓ Branch 1 taken 20 times.
✓ Branch 2 taken 5 times.
|
25 | for (std::size_t i = 0; i < pi.size(); ++i) { |
| 73 |
3/6✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
|
20 | Cpi.push_back(skew(pi[i])); |
| 74 | } | ||
| 75 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity; |
| 76 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | const Matrix3 Cg = skew(g); |
| 77 | 5 | const double T2 = T * T; | |
| 78 | 5 | const double alpha = 1. / (T2); | |
| 79 | // equation of waypoints for curve w found with sympy | ||
| 80 | // TODO Apparently sympy equations are false ... | ||
| 81 | |||
| 82 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | waypoint_t w0 = initwp(DIM_POINT, DIM_VAR); |
| 83 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | w0.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity(); |
| 84 |
4/8✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
|
5 | w0.first.block<3, 3>(3, 0) = 6.0 * Cpi[0] * alpha; |
| 85 |
6/12✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
|
5 | w0.second.head<3>() = (6 * pi[0] - 12 * pi[1]) * alpha; |
| 86 |
7/14✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 5 times.
✗ Branch 22 not taken.
|
5 | w0.second.tail<3>() = (-Cpi[0]) * (12.0 * pi[1] * alpha + g); |
| 87 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | wps.push_back(w0); |
| 88 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | waypoint_t w1 = initwp(DIM_POINT, DIM_VAR); |
| 89 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | w1.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity(); |
| 90 |
8/16✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 5 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 5 times.
✗ Branch 25 not taken.
|
5 | w1.first.block<3, 3>(3, 0) = skew(1.5 * (3 * pi[1] - pi[0])) * alpha; |
| 91 |
6/12✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
|
5 | w1.second.head<3>() = 1.5 * alpha * (3 * pi[0] - 5 * pi[1]); |
| 92 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | w1.second.tail<3>() = |
| 93 |
9/18✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 5 times.
✗ Branch 30 not taken.
|
10 | (3 * alpha * pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3 * pi[1] + pi[0])); |
| 94 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | wps.push_back(w1); |
| 95 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | waypoint_t w2 = initwp(DIM_POINT, DIM_VAR); |
| 96 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | w2.first.block<3, 3>(0, 0) = 0 * alpha * Matrix3::Identity(); |
| 97 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | w2.first.block<3, 3>(3, 0) = |
| 98 |
8/16✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 5 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 5 times.
✗ Branch 25 not taken.
|
10 | skew(0.5 * g - 3 * alpha * pi[0] + 3 * alpha * pi[1]); |
| 99 |
4/8✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
|
5 | w2.second.head<3>() = 3 * alpha * (pi[0] - pi[1]); |
| 100 |
4/8✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
|
5 | w2.second.tail<3>() = 0.5 * Cg * pi[1]; |
| 101 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | wps.push_back(w2); |
| 102 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | waypoint_t w3 = initwp(DIM_POINT, DIM_VAR); |
| 103 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | w3.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity(); |
| 104 |
7/14✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 5 times.
✗ Branch 22 not taken.
|
5 | w3.first.block<3, 3>(3, 0) = skew(g - 1.5 * alpha * (pi[1] + pi[0])); |
| 105 |
4/8✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
|
5 | w3.second.head<3>() = 1.5 * alpha * (pi[1] + pi[0]); |
| 106 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | wps.push_back(w3); |
| 107 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | waypoint_t w4 = initwp(DIM_POINT, DIM_VAR); |
| 108 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | w4.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity(); |
| 109 |
6/12✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
|
5 | w4.first.block<3, 3>(3, 0) = skew(g - 6 * alpha * pi[1]); |
| 110 |
4/8✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
|
5 | w4.second.head<3>() = 6 * pi[1] * alpha; |
| 111 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | wps.push_back(w4); |
| 112 | 10 | return wps; | |
| 113 | 5 | } | |
| 114 | |||
| 115 | ✗ | inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) { | |
| 116 | ✗ | coefs_t v; | |
| 117 | ✗ | std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData, T); | |
| 118 | // equation found with sympy | ||
| 119 | ✗ | v.first = 0.; | |
| 120 | ✗ | v.second = point3_t::Zero(); | |
| 121 | ✗ | return v; | |
| 122 | ✗ | } | |
| 123 | |||
| 124 | } // namespace c0_dc0_dc1 | ||
| 125 | } // namespace bezier_com_traj | ||
| 126 | |||
| 127 | #endif | ||
| 128 |