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