GCC Code Coverage Report

Directory: ./
File: include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0.hh
Date: 2025-05-18 04:15:09
Exec Total Coverage
Lines: 68 76 89.5%
Functions: 4 5 80.0%
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