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