Directory: | ./ |
---|---|
File: | include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh |
Date: | 2025-03-18 04:20:50 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 68 | 76 | 89.5% |
Branches: | 165 | 338 | 48.8% |
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 |