GCC Code Coverage Report

Directory:	./
File:	include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
Date:	2025-04-18 04:07:36

	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