GCC Code Coverage Report

Directory: ./
File: include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
Date: 2025-05-18 04:15:09
Exec Total Coverage
Lines: 87 95 91.6%
Functions: 4 5 80.0%
Branches: 278 568 48.9%
Line Branch Exec Source
1 /*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Pierre Fernbach
4 */
5
6 #ifndef BEZIER_COM_TRAJ_C0DC0D1C1_H
7 #define BEZIER_COM_TRAJ_C0DC0D1C1_H
8
9 #include <hpp/bezier-com-traj/data.hh>
10
11 namespace bezier_com_traj {
12 namespace c0_dc0_dc1_c1 {
13
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | END_POS | 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 128 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
26
1/2
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
 128 coefs_t wp;
27 128 double t2 = t * t;
28 128 double t3 = t2 * t;
29 128 double t4 = t3 * t;
30 // equation found with sympy
31 128 wp.first = (6.0 * t4 - 12.0 * t3 + 6.0 * t2);
32
9/18
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 128 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 128 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 128 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 128 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 128 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 128 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 128 times.
✗ Branch 29 not taken.
 128 wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 -
33
8/16
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 128 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 128 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 128 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 128 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 128 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 128 times.
✗ Branch 26 not taken.
 256 4.0 * pi[0] * t + 1.0 * pi[0] - 4.0 * pi[1] * t4 +
34
9/18
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 128 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 128 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 128 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 128 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 128 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 128 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 128 times.
✗ Branch 29 not taken.
 256 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t -
35
9/18
✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 128 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 128 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 128 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 128 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 128 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 128 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 128 times.
✗ Branch 29 not taken.
 256 4.0 * pi[3] * t4 + 4.0 * pi[3] * t3 + 1.0 * pi[4] * t4;
36 256 return wp;
37 }
38
39 408 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
40 double T, double t) {
41
1/2
✓ Branch 1 taken 408 times.
✗ Branch 2 not taken.
 408 coefs_t wp;
42 408 double alpha = 1. / (T * T);
43 // equation found with sympy
44 408 wp.first = (72.0 * t * t - 72.0 * t + 12.0) * alpha;
45
9/18
✓ Branch 2 taken 408 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 408 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 408 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 408 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 408 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 408 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 408 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 408 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 408 times.
✗ Branch 29 not taken.
 408 wp.second = (12.0 * pi[0] * t * t - 24.0 * pi[0] * t + 12.0 * pi[0] -
46
9/18
✓ Branch 2 taken 408 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 408 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 408 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 408 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 408 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 408 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 408 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 408 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 408 times.
✗ Branch 29 not taken.
 816 48.0 * pi[1] * t * t + 72.0 * pi[1] * t - 24.0 * pi[1] -
47
11/22
✓ Branch 2 taken 408 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 408 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 408 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 408 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 408 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 408 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 408 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 408 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 408 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 408 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 408 times.
✗ Branch 35 not taken.
 816 48.0 * pi[3] * t * t + 24.0 * pi[3] * t + 12.0 * pi[4] * t * t) *
48
1/2
✓ Branch 1 taken 408 times.
✗ Branch 2 not taken.
 408 alpha;
49 816 return wp;
50 }
51
52 92 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
53 double T) {
54 // equation for constraint on initial and final position and velocity (degree
55 // 4, 4 constant waypoint and one free (p2)) first, compute the constant
56 // waypoints that only depend on pData :
57 92 int n = 4;
58 92 std::vector<point_t> pi;
59
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 pi.push_back(pData.c0_); // p0
60
5/10
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 92 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 92 times.
✗ Branch 14 not taken.
 92 pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
61
3/6
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92 times.
✗ Branch 8 not taken.
 92 pi.push_back(point_t::Zero()); // p2 = x
62
6/12
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 92 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 92 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 92 times.
✗ Branch 17 not taken.
 92 pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
63
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 pi.push_back(pData.c1_); // p4
64 184 return pi;
65 ✗ }
66
67 22 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
68 double T) {
69
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 bezier_wp_t::t_point_t wps;
70 22 const int DIM_POINT = 6;
71 22 const int DIM_VAR = 3;
72
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
73 22 std::vector<Matrix3> Cpi;
74
2/2
✓ Branch 1 taken 110 times.
✓ Branch 2 taken 22 times.
 132 for (std::size_t i = 0; i < pi.size(); ++i) {
75
3/6
✓ Branch 2 taken 110 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 110 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 110 times.
✗ Branch 9 not taken.
 110 Cpi.push_back(skew(pi[i]));
76 }
77
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 22 const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
78
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 22 const Matrix3 Cg = skew(g);
79 22 const double T2 = T * T;
80 22 const double alpha = 1 / (T2);
81 // equation of waypoints for curve w found with sympy
82
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
83
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 w0.first.block<3, 3>(0, 0) = 12. * alpha * Matrix3::Identity();
84
3/6
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
 22 w0.first.block<3, 3>(3, 0) = 12. * alpha * Cpi[0];
85
6/12
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 22 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 22 times.
✗ Branch 19 not taken.
 22 w0.second.head<3>() = (12. * pi[0] - 24. * pi[1]) * alpha;
86
8/16
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 w0.second.tail<3>() = 1.0 * Cg * pi[0] - (24.0 * Cpi[0] * pi[1]) * alpha;
87
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 wps.push_back(w0);
88
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
89
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 w1.first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
90
6/12
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 22 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 22 times.
✗ Branch 19 not taken.
 22 w1.first.block<3, 3>(3, 0) = (-12.0 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
91
8/16
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 w1.second.head<3>() = (7.2 * pi[0] - 9.6 * pi[1] + 4.8 * pi[3]) * alpha;
92
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 w1.second.tail<3>() =
93
11/22
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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 20 taken 22 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 22 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 22 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 22 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 22 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 22 times.
✗ Branch 36 not taken.
 22 (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] + 4.8 * Cpi[0] * pi[3]) *
94
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 alpha;
95
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 wps.push_back(w1);
96
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
97
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 w2.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
98
7/14
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
 22 w2.first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[1]) * alpha;
99
8/16
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 w2.second.head<3>() = (3.6 * pi[0] + 4.8 * pi[3] + 1.2 * pi[4]) * alpha;
100
8/16
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 44 w2.second.tail<3>() = (0.4 * Cg * T2 * pi[1] - 4.8 * Cpi[0] * pi[3] +
101
6/12
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
 44 1.2 * Cpi[0] * pi[4] + 9.6 * Cpi[1] * pi[3]) *
102
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 alpha;
103
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 wps.push_back(w2);
104
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
105
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 w3.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
106
7/14
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
 22 w3.first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[3]) * alpha;
107
8/16
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 w3.second.head<3>() = (1.2 * pi[0] + 4.8 * pi[1] + 3.6 * pi[4]) * alpha;
108
8/16
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 44 w3.second.tail<3>() = (0.4 * Cg * T2 * pi[3] - 1.2 * Cpi[0] * pi[4] -
109
6/12
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
 44 9.6 * Cpi[1] * pi[3] + 4.8 * Cpi[1] * pi[4]) *
110
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 alpha;
111
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 wps.push_back(w3);
112
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
113
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 w4.first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
114
6/12
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 22 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 22 times.
✗ Branch 19 not taken.
 22 w4.first.block<3, 3>(3, 0) = (9.6 * Cpi[3] - 12.0 * Cpi[4]) * alpha;
115
8/16
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 w4.second.head<3>() = (4.8 * pi[1] - 9.6 * pi[3] + 7.2 * pi[4]) * alpha;
116
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 w4.second.tail<3>() =
117
11/22
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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 20 taken 22 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 22 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 22 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 22 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 22 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 22 times.
✗ Branch 36 not taken.
 22 (0.8 * Cg * T2 * pi[3] + 0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4]) *
118
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 alpha;
119
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 wps.push_back(w4);
120
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
121
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 w5.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();
122
4/8
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
 22 w5.first.block<3, 3>(3, 0) = 12.0 * Cpi[4] * alpha;
123
6/12
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 22 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 22 times.
✗ Branch 19 not taken.
 22 w5.second.head<3>() = (-24 * pi[3] + 12 * pi[4]) * alpha;
124
8/16
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 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.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 w5.second.tail<3>() = (Cg * T2 * pi[4] + 24.0 * Cpi[3] * pi[4]) * alpha;
125
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 wps.push_back(w5);
126 44 return wps;
127 22 }
128
129 ✗ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
130 ✗ coefs_t v;
131 ✗ std::vector<point_t> pi = computeConstantWaypoints(pData, T);
132 // equation found with sympy
133 ✗ v.first = 0.;
134 ✗ v.second = (-4.0 * pi[3] + 4.0 * pi[4]) / T;
135 ✗ return v;
136 ✗ }
137
138 } // namespace c0_dc0_dc1_c1
139 } // namespace bezier_com_traj
140
141 #endif
142