GCC Code Coverage Report


Directory: ./
File: include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 163 169 96.4%
Branches: 442 896 49.3%

Line Branch Exec Source
1 /*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Pierre Fernbach
4 */
5
6 #ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
8
9 #include <hpp/bezier-com-traj/data.hh>
10
11 namespace bezier_com_traj {
12 namespace c0_dc0_ddc0_dc1_c1 {
13
14 static const ConstraintFlag flag =
15 INIT_POS | INIT_VEL | INIT_ACC | END_VEL | END_POS;
16
17 /// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
18 /// INIT ACCELERATION (DEGREE = 5)
19 ///
20 /**
21 * @brief evaluateCurveAtTime compute the expression of the point on the curve
22 * at t, defined by the waypoint pi and one free waypoint (x)
23 * @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
24 * @param t param (normalized !)
25 * @return the expression of the waypoint such that wp.first . x + wp.second =
26 * point on curve
27 */
28 64 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
29
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
64 coefs_t wp;
30 64 double t2 = t * t;
31 64 double t3 = t2 * t;
32 64 double t4 = t3 * t;
33 64 double t5 = t4 * t;
34 // equation found with sympy
35 64 wp.first = 10.0 * t5 - 20.0 * t4 + 10.0 * t3;
36
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] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 +
37
8/16
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 64 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 64 times.
✗ Branch 14 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 10.0 * pi[0] * t2 - 5.0 * pi[0] * t + 1.0 * pi[0] +
38
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.
128 5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 -
39
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.
128 20.0 * pi[1] * t2 + 5.0 * pi[1] * t - 10.0 * pi[2] * t5 +
40
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.
128 30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
41
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.
128 5.0 * pi[4] * t5 + 5.0 * pi[4] * t4 + 1.0 * pi[5] * t5;
42 128 return wp;
43 }
44
45 212 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
46 double T, double t) {
47
1/2
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
212 coefs_t wp;
48 212 double alpha = 1. / (T * T);
49 212 double t2 = t * t;
50 212 double t3 = t2 * t;
51 // equation found with sympy
52 212 wp.first = (200.0 * t3 - 240.0 * t2 + 60.0 * t) * alpha;
53
1/2
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
212 wp.second = 1.0 *
54
9/18
✓ 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 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.
✓ Branch 28 taken 212 times.
✗ Branch 29 not taken.
212 (-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t +
55
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 12 taken 212 times.
✗ Branch 13 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.
424 20.0 * pi[0] + 100.0 * pi[1] * t3 - 240.0 * pi[1] * t2 +
56
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.
424 180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 +
57
8/16
✓ Branch 2 taken 212 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 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.
424 360.0 * pi[2] * t2 - 180.0 * pi[2] * t + 20.0 * pi[2] -
58
9/18
✓ 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 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.
✓ Branch 28 taken 212 times.
✗ Branch 29 not taken.
636 100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
59
1/2
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
212 alpha;
60 424 return wp;
61 }
62
63 38 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
64 double T) {
65 // equation for constraint on initial and final position and velocity and
66 // initial acceleration(degree 5, 5 constant waypoint and one free (p3))
67 // first, compute the constant waypoints that only depend on pData :
68 38 double n = 5.;
69 38 std::vector<point_t> pi;
70
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
38 pi.push_back(pData.c0_); // p0
71
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
72
6/12
✓ 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.
✓ Branch 16 taken 38 times.
✗ Branch 17 not taken.
76 pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
73
4/8
✓ 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.
76 (2. * pData.dc0_ * T / n) + pData.c0_); // p2
74
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()); // x
75
6/12
✓ 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.
✓ Branch 16 taken 38 times.
✗ Branch 17 not taken.
38 pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
76
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
38 pi.push_back(pData.c1_); // p5
77 76 return pi;
78 }
79
80 10 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
81 double T) {
82
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 bezier_wp_t::t_point_t wps;
83 10 const int DIM_POINT = 6;
84 10 const int DIM_VAR = 3;
85
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
86 10 std::vector<Matrix3> Cpi;
87
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 10 times.
70 for (std::size_t i = 0; i < pi.size(); ++i) {
88
3/6
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 60 times.
✗ Branch 9 not taken.
60 Cpi.push_back(skew(pi[i]));
89 }
90
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
10 const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
91
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);
92 10 const double T2 = T * T;
93 10 const double alpha = 1 / (T2);
94
95 // equation of waypoints for curve w found with sympy
96
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
97
8/16
✓ 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.
10 w0.second.head<3>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
98
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w0.second.tail<3>() =
99
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
100
10/20
✓ 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 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 10 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 10 times.
✗ Branch 34 not taken.
20 (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) *
101
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
102
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w0);
103
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
104
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 w1.first.block<3, 3>(0, 0) = 8.57142857142857 * alpha * Matrix3::Identity();
105
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 w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
106
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w1.second.head<3>() = 1.0 *
107
4/8
✓ 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.
10 (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] -
108
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 5.71428571428572 * pi[2]) *
109
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
110
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w1.second.tail<3>() =
111
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
112
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 (0.285714285714286 * Cg * T2 * pi[0] +
113
7/14
✓ 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.
20 0.714285714285714 * Cg * T2 * pi[1] - 20.0 * Cpi[0] * pi[2] +
114
3/6
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
20 14.2857142857143 * Cpi[1] * pi[2]) *
115
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
116
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w1);
117
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
118
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) = 5.71428571428571 * alpha * Matrix3::Identity();
119
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w2.first.block<3, 3>(3, 0) =
120
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.
20 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
121
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w2.second.head<3>() = 1.0 *
122
4/8
✓ 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.
10 (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] +
123
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 2.85714285714286 * pi[4]) *
124
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
125
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w2.second.tail<3>() =
126
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
127
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 (0.0476190476190479 * Cg * T2 * pi[0] +
128
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.
20 0.476190476190476 * Cg * T2 * pi[1] +
129
7/14
✓ 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.
20 0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] -
130
3/6
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
20 14.2857142857143 * Cpi[1] * pi[2]) *
131
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
132
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w2);
133
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
134
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) = -2.85714285714286 * alpha * Matrix3::Identity();
135
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w3.first.block<3, 3>(3, 0) =
136
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
137
5/10
✓ 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.
10 (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] +
138
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 11.4285714285714 * Cpi[2]) *
139
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
140
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w3.second.head<3>() = 1.0 *
141
4/8
✓ 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.
10 (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] -
142
4/8
✓ 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.
20 11.4285714285714 * pi[2] + 5.71428571428571 * pi[4] +
143
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 0.571428571428571 * pi[5]) *
144
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
145
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w3.second.tail<3>() =
146
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
147
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 (0.142857142857143 * Cg * T2 * pi[1] +
148
7/14
✓ 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.
20 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
149
6/12
✓ 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.
30 0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
150
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
151
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w3);
152
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
153
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 w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
154
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w4.first.block<3, 3>(3, 0) =
155
7/14
✓ 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.
20 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
156
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w4.second.head<3>() = 1.0 *
157
4/8
✓ 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.
10 (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] -
158
4/8
✓ 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.
20 2.85714285714286 * pi[2] + 5.71428571428571 * pi[4] +
159
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 2.28571428571429 * pi[5]) *
160
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
161
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w4.second.tail<3>() =
162
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
163
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 (0.285714285714286 * Cg * T2 * pi[2] +
164
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.
20 0.142857142857143 * Cg * T2 * pi[4] -
165
6/12
✓ 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.
20 0.571428571428572 * Cpi[0] * pi[5] - 8.57142857142857 * Cpi[1] * pi[4] +
166
6/12
✓ 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.
30 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
167
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
168
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w4);
169
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
170
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 w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
171
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w5.first.block<3, 3>(3, 0) =
172
7/14
✓ 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.
20 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
173
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w5.second.head<3>() = 1.0 *
174
4/8
✓ 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.
10 (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] +
175
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 5.71428571428571 * pi[5]) *
176
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
177
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w5.second.tail<3>() =
178
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 1.0 *
179
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 (0.476190476190476 * Cg * T2 * pi[4] +
180
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.
20 0.0476190476190476 * Cg * T2 * pi[5] -
181
6/12
✓ 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.
20 2.85714285714286 * Cpi[1] * pi[5] - 14.2857142857143 * Cpi[2] * pi[4] +
182
3/6
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
20 8.57142857142857 * Cpi[2] * pi[5]) *
183
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
184
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w5);
185
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
186
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 w6.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
187
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w6.first.block<3, 3>(3, 0) =
188
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.
20 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
189
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w6.second.head<3>() = 1.0 *
190
4/8
✓ 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.
10 (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] +
191
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
20 11.4285714285714 * pi[5]) *
192
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
193
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
20 w6.second.tail<3>() = 1.0 *
194
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 (0.714285714285714 * Cg * T2 * pi[4] +
195
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.
20 0.285714285714286 * Cg * T2 * pi[5] -
196
3/6
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
20 8.57142857142858 * Cpi[2] * pi[5]) *
197
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 alpha;
198
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w6);
199
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
200
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 w7.first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
201
5/10
✓ 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.
10 w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
202
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 w7.second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
203
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 w7.second.tail<3>() =
204
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[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
205
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
10 wps.push_back(w7);
206 20 return wps;
207 10 }
208
209 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
210 coefs_t v;
211 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
212 // equation found with sympy
213 v.first = 0.;
214 v.second = (-5.0 * pi[4] + 5.0 * pi[5]) / T;
215 return v;
216 }
217
218 } // namespace c0_dc0_ddc0_dc1_c1
219 } // namespace bezier_com_traj
220
221 #endif
222