GCC Code Coverage Report

Directory: ./
File: include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
Date: 2025-04-18 04:07:36
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