GCC Code Coverage Report

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