hpp-bezier-com-traj  4.15.1
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 =
16 
20 
28 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
29  coefs_t wp;
30  double t2 = t * t;
31  double t3 = t2 * t;
32  double t4 = t3 * t;
33  double t5 = t4 * t;
34  double t6 = t5 * t;
35  // equation found with sympy
36  wp.first = -20.0 * t6 + 60.0 * t5 - 60.0 * t4 + 20.0 * t3;
37  wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 -
38  20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 - 6.0 * pi[0] * t +
39  1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 -
40  60.0 * pi[1] * t4 + 60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 +
41  6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
42  90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 +
43  15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 + 15.0 * pi[4] * t4 -
44  6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
45  return wp;
46 }
47 
48 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
49  double T, double t) {
50  coefs_t wp;
51  double alpha = 1. / (T * T);
52  double t2 = t * t;
53  double t3 = t2 * t;
54  double t4 = t3 * t;
55  // equation found with sympy
56  wp.first = 1.0 * (-600.0 * t4 + 1200.0 * t3 - 720.0 * t2 + 120.0 * t) * alpha;
57  wp.second = 1.0 *
58  (30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 -
59  120.0 * pi[0] * t + 30.0 * pi[0] - 180.0 * pi[1] * t4 +
60  600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t -
61  60.0 * pi[1] + 450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 +
62  1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
63  450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 -
64  180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 + 30.0 * pi[6] * t4) *
65  alpha;
66  return wp;
67 }
68 
69 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
70  double T) {
71  // equation for constraint on initial and final position and velocity and
72  // initial acceleration(degree 5, 5 constant waypoint and one free (p3))
73  // first, compute the constant waypoints that only depend on pData :
74  double n = 6.;
75  std::vector<point_t> pi;
76  pi.push_back(pData.c0_); // p0
77  pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
78  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
79  (2. * pData.dc0_ * T / n) + pData.c0_); // p2
80  pi.push_back(point_t::Zero()); // x
81  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
82  (2 * pData.dc1_ * T / n) + pData.c1_);
83  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
84  pi.push_back(pData.c1_); // p5
85  return pi;
86 }
87 
88 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
89  double T) {
90  bezier_wp_t::t_point_t wps;
91  const int DIM_POINT = 6;
92  const int DIM_VAR = 3;
93  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
94  std::vector<Matrix3> Cpi;
95  for (std::size_t i = 0; i < pi.size(); ++i) {
96  Cpi.push_back(skew(pi[i]));
97  }
98  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
99  const Matrix3 Cg = skew(g);
100  const double T2 = T * T;
101  const double alpha = 1 / (T2);
102 
103  // equation of waypoints for curve w found with sympy
104  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
105  w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
106  w0.second.tail<3>() =
107  1.0 *
108  (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
109  alpha;
110  wps.push_back(w0);
111  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
112  w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
113  w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
114  w1.second.head<3>() =
115  1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
116  w1.second.tail<3>() = 1.0 *
117  (0.333333333333333 * Cg * T2 * pi[0] +
118  0.666666666666667 * Cg * T2 * pi[1] -
119  30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
120  alpha;
121  wps.push_back(w1);
122  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
123  w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
124  w2.first.block<3, 3>(3, 0) =
125  1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
126  w2.second.head<3>() =
127  1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
128  w2.second.tail<3>() =
129  1.0 *
130  (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
131  0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
132  20.0 * Cpi[1] * pi[2]) *
133  alpha;
134  wps.push_back(w2);
135  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
136  w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
137  w3.first.block<3, 3>(3, 0) = 1.0 *
138  (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
139  14.2857142857143 * Cpi[2]) *
140  alpha;
141  w3.second.head<3>() = 1.0 *
142  (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
143  14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
144  1.42857142857143 * pi[5]) *
145  alpha;
146  w3.second.tail<3>() =
147  1.0 *
148  (0.0119047619047619 * Cg * T2 * pi[0] +
149  0.214285714285714 * Cg * T2 * pi[1] +
150  0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
151  1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
152  alpha;
153  wps.push_back(w3);
154  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
155  w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
156  w4.first.block<3, 3>(3, 0) =
157  1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
158  w4.second.head<3>() = 1.0 *
159  (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
160  3.57142857142857 * pi[2] + 5.0 * pi[4] +
161  4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
162  alpha;
163  w4.second.tail<3>() =
164  1.0 *
165  (0.0476190476190471 * Cg * T2 * pi[1] +
166  0.357142857142857 * Cg * T2 * pi[2] +
167  0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
168  0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
169  5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
170  alpha;
171  wps.push_back(w4);
172  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
173  w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
174  w5.first.block<3, 3>(3, 0) =
175  1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
176  w5.second.head<3>() = 1.0 *
177  (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
178  5.0 * pi[2] - 3.57142857142857 * pi[4] +
179  7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
180  alpha;
181  w5.second.tail<3>() =
182  1.0 *
183  (+0.11904761904762 * Cg * T2 * pi[2] +
184  0.357142857142857 * Cg * T2 * pi[4] +
185  0.0476190476190476 * Cg * T2 * pi[5] -
186  0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
187  1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
188  12.8571428571429 * Cpi[2] * pi[5]) *
189  alpha;
190  wps.push_back(w5);
191  waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
192  w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
193  w6.first.block<3, 3>(3, 0) = 1.0 *
194  (0.238095238095238 * Cg * T2 +
195  14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
196  alpha;
197  w6.second.head<3>() = 1.0 *
198  (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
199  14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
200  3.57142857142857 * pi[6]) *
201  alpha;
202  w6.second.tail<3>() =
203  1.0 *
204  (0.535714285714286 * Cg * T2 * pi[4] +
205  0.214285714285714 * Cg * T2 * pi[5] +
206  0.0119047619047619 * Cg * T2 * pi[6] -
207  1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
208  5.0 * Cpi[2] * pi[6]) *
209  alpha;
210  wps.push_back(w6);
211  waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
212  w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
213  w7.first.block<3, 3>(3, 0) =
214  1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
215  w7.second.head<3>() =
216  1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
217  w7.second.tail<3>() =
218  1.0 *
219  (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
220  0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
221  20.0 * Cpi[4] * pi[5]) *
222  alpha;
223  wps.push_back(w7);
224  waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
225  w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
226  w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
227  w8.second.head<3>() =
228  1.0 *
229  (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
230  alpha;
231  w8.second.tail<3>() = 1.0 *
232  (0.666666666666667 * Cg * T2 * pi[5] +
233  0.333333333333333 * Cg * T2 * pi[6] -
234  20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
235  alpha;
236  wps.push_back(w8);
237  waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
238  w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
239  w9.second.tail<3>() =
240  1.0 *
241  (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
242  alpha;
243  wps.push_back(w9);
244  return wps;
245 }
246 
247 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
248  coefs_t v;
249  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
250  // equation found with sympy
251  v.first = 0.;
252  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
253  return v;
254 }
255 
256 } // namespace c0_dc0_ddc0_ddc1_dc1_c1
257 
258 } // namespace bezier_com_traj
259 
260 #endif
point_t ddc0_
Definition: data.hh:107
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:34
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:22
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:23
VectorX second
Definition: utils.hh:27
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:45
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:88
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:28
Definition: utils.hh:25
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:56
END_ACC
Definition: flags.hh:25
point_t c0_
Definition: data.hh:107
MatrixXX first
Definition: utils.hh:26
END_POS
Definition: flags.hh:23
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:247
point_t dc1_
Definition: data.hh:107
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:107
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:69
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:107
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:48
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:17
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:92
Definition: common_solve_methods.hh:15
INIT_POS
Definition: flags.hh:20
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:62
std::vector< ContactData > contacts_
Definition: data.hh:106
point_t c1_
Definition: data.hh:107
const int DIM_POINT
Definition: solve_end_effector.hh:15