hpp-bezier-com-traj  4.12.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2018, LAAS-CNRS
3  * Author: Pierre Fernbach
4  */
5 
6 #ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
8 
10 
11 namespace bezier_com_traj {
12 namespace c0_dc0_ddc0_ddc1_dc1_c1 {
13 
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
15 
18 
25 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
26  coefs_t wp;
27  double t2 = t * t;
28  double t3 = t2 * t;
29  double t4 = t3 * t;
30  double t5 = t4 * t;
31  double t6 = t5 * t;
32  // equation found with sympy
33  wp.first = -20.0 * t6 + 60.0 * t5 - 60.0 * t4 + 20.0 * t3;
34  wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 - 20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 -
35  6.0 * pi[0] * t + 1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 - 60.0 * pi[1] * t4 +
36  60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 + 6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
37  90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 + 15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 +
38  15.0 * pi[4] * t4 - 6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
39  return wp;
40 }
41 
42 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
43  coefs_t wp;
44  double alpha = 1. / (T * T);
45  double t2 = t * t;
46  double t3 = t2 * t;
47  double t4 = t3 * t;
48  // equation found with sympy
49  wp.first = 1.0 * (-600.0 * t4 + 1200.0 * t3 - 720.0 * t2 + 120.0 * t) * alpha;
50  wp.second = 1.0 *
51  (30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 - 120.0 * pi[0] * t + 30.0 * pi[0] -
52  180.0 * pi[1] * t4 + 600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t - 60.0 * pi[1] +
53  450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 + 1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
54  450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 - 180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 +
55  30.0 * pi[6] * t4) *
56  alpha;
57  return wp;
58 }
59 
60 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
61  // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
62  // waypoint and one free (p3)) first, compute the constant waypoints that only depend on pData :
63  double n = 6.;
64  std::vector<point_t> pi;
65  pi.push_back(pData.c0_); // p0
66  pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
67  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
68  pi.push_back(point_t::Zero()); // x
69  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_);
70  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
71  pi.push_back(pData.c1_); // p5
72  return pi;
73 }
74 
75 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
76  bezier_wp_t::t_point_t wps;
77  const int DIM_POINT = 6;
78  const int DIM_VAR = 3;
79  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
80  std::vector<Matrix3> Cpi;
81  for (std::size_t i = 0; i < pi.size(); ++i) {
82  Cpi.push_back(skew(pi[i]));
83  }
84  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
85  const Matrix3 Cg = skew(g);
86  const double T2 = T * T;
87  const double alpha = 1 / (T2);
88 
89  // equation of waypoints for curve w found with sympy
90  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
91  w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
92  w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
93  wps.push_back(w0);
94  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
95  w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
96  w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
97  w1.second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
98  w1.second.tail<3>() = 1.0 *
99  (0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
100  30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
101  alpha;
102  wps.push_back(w1);
103  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
104  w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
105  w2.first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
106  w2.second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
107  w2.second.tail<3>() = 1.0 *
108  (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
109  0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
110  alpha;
111  wps.push_back(w2);
112  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
113  w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
114  w3.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
115  w3.second.head<3>() = 1.0 *
116  (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
117  7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
118  alpha;
119  w3.second.tail<3>() = 1.0 *
120  (0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
121  0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
122  1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
123  alpha;
124  wps.push_back(w3);
125  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
126  w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
127  w4.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
128  w4.second.head<3>() = 1.0 *
129  (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
130  4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
131  alpha;
132  w4.second.tail<3>() =
133  1.0 *
134  (0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
135  0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
136  12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
137  alpha;
138  wps.push_back(w4);
139  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
140  w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
141  w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
142  w5.second.head<3>() = 1.0 *
143  (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
144  3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
145  alpha;
146  w5.second.tail<3>() =
147  1.0 *
148  (+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
149  0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
150  1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
151  alpha;
152  wps.push_back(w5);
153  waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
154  w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
155  w6.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
156  w6.second.head<3>() = 1.0 *
157  (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
158  7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
159  alpha;
160  w6.second.tail<3>() = 1.0 *
161  (0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
162  0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
163  12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
164  alpha;
165  wps.push_back(w6);
166  waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
167  w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
168  w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
169  w7.second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
170  w7.second.tail<3>() = 1.0 *
171  (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
172  0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
173  alpha;
174  wps.push_back(w7);
175  waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
176  w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
177  w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
178  w8.second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
179  w8.second.tail<3>() = 1.0 *
180  (0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
181  20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
182  alpha;
183  wps.push_back(w8);
184  waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
185  w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
186  w9.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
187  wps.push_back(w9);
188  return wps;
189 }
190 
191 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
192  coefs_t v;
193  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
194  // equation found with sympy
195  v.first = 0.;
196  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
197  return v;
198 }
199 
200 } // namespace c0_dc0_ddc0_ddc1_dc1_c1
201 
202 } // namespace bezier_com_traj
203 
204 #endif
point_t ddc0_
Definition: data.hh:103
waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition: solve_0_step.cpp:32
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:21
waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition: solve_0_step.cpp:22
VectorX second
Definition: utils.hh:29
waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha)
Definition: solve_0_step.cpp:42
INIT_VEL
Definition: flags.hh:21
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:75
coefs_t evaluateCurveAtTime(const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi...
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:25
Definition: utils.hh:27
waypoint6_t w4(point_t_tC, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha)
Definition: solve_0_step.cpp:52
END_ACC
Definition: flags.hh:25
point_t c0_
Definition: data.hh:103
MatrixXX first
Definition: utils.hh:28
END_POS
Definition: flags.hh:23
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:191
point_t dc1_
Definition: data.hh:103
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:103
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:60
waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &p0X, const Matrix3 &, const Matrix3 &, const double alpha)
Definition: solve_0_step.cpp:12
point_t ddc1_
Definition: data.hh:103
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:42
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:16
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:88
Definition: common_solve_methods.hh:16
INIT_POS
Definition: flags.hh:20
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
std::vector< ContactData > contacts_
Definition: data.hh:102
point_t c1_
Definition: data.hh:103
const int DIM_POINT
Definition: solve_end_effector.hh:15