hpp-bezier-com-traj  4.13.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_ddc0_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_dc1_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
8 
10 
11 namespace bezier_com_traj {
12 namespace c0_dc0_ddc0_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  // equation found with sympy
35  wp.first = 10.0 * t5 - 20.0 * t4 + 10.0 * t3;
36  wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 +
37  10.0 * pi[0] * t2 - 5.0 * pi[0] * t + 1.0 * pi[0] +
38  5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 -
39  20.0 * pi[1] * t2 + 5.0 * pi[1] * t - 10.0 * pi[2] * t5 +
40  30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
41  5.0 * pi[4] * t5 + 5.0 * pi[4] * t4 + 1.0 * pi[5] * t5;
42  return wp;
43 }
44 
45 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
46  double T, double t) {
47  coefs_t wp;
48  double alpha = 1. / (T * T);
49  double t2 = t * t;
50  double t3 = t2 * t;
51  // equation found with sympy
52  wp.first = (200.0 * t3 - 240.0 * t2 + 60.0 * t) * alpha;
53  wp.second = 1.0 *
54  (-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t +
55  20.0 * pi[0] + 100.0 * pi[1] * t3 - 240.0 * pi[1] * t2 +
56  180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 +
57  360.0 * pi[2] * t2 - 180.0 * pi[2] * t + 20.0 * pi[2] -
58  100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
59  alpha;
60  return wp;
61 }
62 
63 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  double n = 5.;
69  std::vector<point_t> pi;
70  pi.push_back(pData.c0_); // p0
71  pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
72  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
73  (2. * pData.dc0_ * T / n) + pData.c0_); // p2
74  pi.push_back(point_t::Zero()); // x
75  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
76  pi.push_back(pData.c1_); // p5
77  return pi;
78 }
79 
80 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
81  double T) {
82  bezier_wp_t::t_point_t wps;
83  const int DIM_POINT = 6;
84  const int DIM_VAR = 3;
85  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
86  std::vector<Matrix3> Cpi;
87  for (std::size_t i = 0; i < pi.size(); ++i) {
88  Cpi.push_back(skew(pi[i]));
89  }
90  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
91  const Matrix3 Cg = skew(g);
92  const double T2 = T * T;
93  const double alpha = 1 / (T2);
94 
95  // equation of waypoints for curve w found with sympy
96  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
97  w0.second.head<3>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
98  w0.second.tail<3>() =
99  1.0 *
100  (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) *
101  alpha;
102  wps.push_back(w0);
103  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
104  w1.first.block<3, 3>(0, 0) = 8.57142857142857 * alpha * Matrix3::Identity();
105  w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
106  w1.second.head<3>() = 1.0 *
107  (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] -
108  5.71428571428572 * pi[2]) *
109  alpha;
110  w1.second.tail<3>() =
111  1.0 *
112  (0.285714285714286 * Cg * T2 * pi[0] +
113  0.714285714285714 * Cg * T2 * pi[1] - 20.0 * Cpi[0] * pi[2] +
114  14.2857142857143 * Cpi[1] * pi[2]) *
115  alpha;
116  wps.push_back(w1);
117  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
118  w2.first.block<3, 3>(0, 0) = 5.71428571428571 * alpha * Matrix3::Identity();
119  w2.first.block<3, 3>(3, 0) =
120  1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
121  w2.second.head<3>() = 1.0 *
122  (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] +
123  2.85714285714286 * pi[4]) *
124  alpha;
125  w2.second.tail<3>() =
126  1.0 *
127  (0.0476190476190479 * Cg * T2 * pi[0] +
128  0.476190476190476 * Cg * T2 * pi[1] +
129  0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] -
130  14.2857142857143 * Cpi[1] * pi[2]) *
131  alpha;
132  wps.push_back(w2);
133  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
134  w3.first.block<3, 3>(0, 0) = -2.85714285714286 * alpha * Matrix3::Identity();
135  w3.first.block<3, 3>(3, 0) =
136  1.0 *
137  (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] +
138  11.4285714285714 * Cpi[2]) *
139  alpha;
140  w3.second.head<3>() = 1.0 *
141  (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] -
142  11.4285714285714 * pi[2] + 5.71428571428571 * pi[4] +
143  0.571428571428571 * pi[5]) *
144  alpha;
145  w3.second.tail<3>() =
146  1.0 *
147  (0.142857142857143 * Cg * T2 * pi[1] +
148  0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
149  0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
150  alpha;
151  wps.push_back(w3);
152  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
153  w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
154  w4.first.block<3, 3>(3, 0) =
155  1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
156  w4.second.head<3>() = 1.0 *
157  (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] -
158  2.85714285714286 * pi[2] + 5.71428571428571 * pi[4] +
159  2.28571428571429 * pi[5]) *
160  alpha;
161  w4.second.tail<3>() =
162  1.0 *
163  (0.285714285714286 * Cg * T2 * pi[2] +
164  0.142857142857143 * Cg * T2 * pi[4] -
165  0.571428571428572 * Cpi[0] * pi[5] - 8.57142857142857 * Cpi[1] * pi[4] +
166  2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
167  alpha;
168  wps.push_back(w4);
169  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
170  w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
171  w5.first.block<3, 3>(3, 0) =
172  1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
173  w5.second.head<3>() = 1.0 *
174  (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] +
175  5.71428571428571 * pi[5]) *
176  alpha;
177  w5.second.tail<3>() =
178  1.0 *
179  (0.476190476190476 * Cg * T2 * pi[4] +
180  0.0476190476190476 * Cg * T2 * pi[5] -
181  2.85714285714286 * Cpi[1] * pi[5] - 14.2857142857143 * Cpi[2] * pi[4] +
182  8.57142857142857 * Cpi[2] * pi[5]) *
183  alpha;
184  wps.push_back(w5);
185  waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
186  w6.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
187  w6.first.block<3, 3>(3, 0) =
188  1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
189  w6.second.head<3>() = 1.0 *
190  (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] +
191  11.4285714285714 * pi[5]) *
192  alpha;
193  w6.second.tail<3>() = 1.0 *
194  (0.714285714285714 * Cg * T2 * pi[4] +
195  0.285714285714286 * Cg * T2 * pi[5] -
196  8.57142857142858 * Cpi[2] * pi[5]) *
197  alpha;
198  wps.push_back(w6);
199  waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
200  w7.first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
201  w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
202  w7.second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
203  w7.second.tail<3>() =
204  1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
205  wps.push_back(w7);
206  return wps;
207 }
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
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_dc1_c1.hh:209
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_dc1_c1.hh:28
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
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
point_t c0_
Definition: data.hh:107
MatrixXX first
Definition: utils.hh:26
END_POS
Definition: flags.hh:23
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
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
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_dc1_c1.hh:80
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_dc1_c1.hh:45
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
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_dc1_c1.hh:63
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