hpp-bezier-com-traj  4.14.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_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_C0DC0D1C1_H
7 #define BEZIER_COM_TRAJ_C0DC0D1C1_H
8 
10 
11 namespace bezier_com_traj {
12 namespace c0_dc0_dc1_c1 {
13 
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | END_POS | END_VEL;
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  // equation found with sympy
31  wp.first = (6.0 * t4 - 12.0 * t3 + 6.0 * t2);
32  wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 -
33  4.0 * pi[0] * t + 1.0 * pi[0] - 4.0 * pi[1] * t4 +
34  12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t -
35  4.0 * pi[3] * t4 + 4.0 * pi[3] * t3 + 1.0 * pi[4] * t4;
36  return wp;
37 }
38 
39 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
40  double T, double t) {
41  coefs_t wp;
42  double alpha = 1. / (T * T);
43  // equation found with sympy
44  wp.first = (72.0 * t * t - 72.0 * t + 12.0) * alpha;
45  wp.second = (12.0 * pi[0] * t * t - 24.0 * pi[0] * t + 12.0 * pi[0] -
46  48.0 * pi[1] * t * t + 72.0 * pi[1] * t - 24.0 * pi[1] -
47  48.0 * pi[3] * t * t + 24.0 * pi[3] * t + 12.0 * pi[4] * t * t) *
48  alpha;
49  return wp;
50 }
51 
52 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
53  double T) {
54  // equation for constraint on initial and final position and velocity (degree
55  // 4, 4 constant waypoint and one free (p2)) first, compute the constant
56  // waypoints that only depend on pData :
57  int n = 4;
58  std::vector<point_t> pi;
59  pi.push_back(pData.c0_); // p0
60  pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
61  pi.push_back(point_t::Zero()); // p2 = x
62  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
63  pi.push_back(pData.c1_); // p4
64  return pi;
65 }
66 
67 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
68  double T) {
69  bezier_wp_t::t_point_t wps;
70  const int DIM_POINT = 6;
71  const int DIM_VAR = 3;
72  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
73  std::vector<Matrix3> Cpi;
74  for (std::size_t i = 0; i < pi.size(); ++i) {
75  Cpi.push_back(skew(pi[i]));
76  }
77  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
78  const Matrix3 Cg = skew(g);
79  const double T2 = T * T;
80  const double alpha = 1 / (T2);
81  // equation of waypoints for curve w found with sympy
82  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
83  w0.first.block<3, 3>(0, 0) = 12. * alpha * Matrix3::Identity();
84  w0.first.block<3, 3>(3, 0) = 12. * alpha * Cpi[0];
85  w0.second.head<3>() = (12. * pi[0] - 24. * pi[1]) * alpha;
86  w0.second.tail<3>() = 1.0 * Cg * pi[0] - (24.0 * Cpi[0] * pi[1]) * alpha;
87  wps.push_back(w0);
88  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
89  w1.first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
90  w1.first.block<3, 3>(3, 0) = (-12.0 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
91  w1.second.head<3>() = (7.2 * pi[0] - 9.6 * pi[1] + 4.8 * pi[3]) * alpha;
92  w1.second.tail<3>() =
93  (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] + 4.8 * Cpi[0] * pi[3]) *
94  alpha;
95  wps.push_back(w1);
96  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
97  w2.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
98  w2.first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[1]) * alpha;
99  w2.second.head<3>() = (3.6 * pi[0] + 4.8 * pi[3] + 1.2 * pi[4]) * alpha;
100  w2.second.tail<3>() = (0.4 * Cg * T2 * pi[1] - 4.8 * Cpi[0] * pi[3] +
101  1.2 * Cpi[0] * pi[4] + 9.6 * Cpi[1] * pi[3]) *
102  alpha;
103  wps.push_back(w2);
104  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
105  w3.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
106  w3.first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[3]) * alpha;
107  w3.second.head<3>() = (1.2 * pi[0] + 4.8 * pi[1] + 3.6 * pi[4]) * alpha;
108  w3.second.tail<3>() = (0.4 * Cg * T2 * pi[3] - 1.2 * Cpi[0] * pi[4] -
109  9.6 * Cpi[1] * pi[3] + 4.8 * Cpi[1] * pi[4]) *
110  alpha;
111  wps.push_back(w3);
112  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
113  w4.first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
114  w4.first.block<3, 3>(3, 0) = (9.6 * Cpi[3] - 12.0 * Cpi[4]) * alpha;
115  w4.second.head<3>() = (4.8 * pi[1] - 9.6 * pi[3] + 7.2 * pi[4]) * alpha;
116  w4.second.tail<3>() =
117  (0.8 * Cg * T2 * pi[3] + 0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4]) *
118  alpha;
119  wps.push_back(w4);
120  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
121  w5.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();
122  w5.first.block<3, 3>(3, 0) = 12.0 * Cpi[4] * alpha;
123  w5.second.head<3>() = (-24 * pi[3] + 12 * pi[4]) * alpha;
124  w5.second.tail<3>() = (Cg * T2 * pi[4] + 24.0 * Cpi[3] * pi[4]) * alpha;
125  wps.push_back(w5);
126  return wps;
127 }
128 
129 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
130  coefs_t v;
131  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
132  // equation found with sympy
133  v.first = 0.;
134  v.second = (-4.0 * pi[3] + 4.0 * pi[4]) / T;
135  return v;
136 }
137 
138 } // namespace c0_dc0_dc1_c1
139 } // namespace bezier_com_traj
140 
141 #endif
INIT_VEL
Definition: flags.hh:21
END_VEL
Definition: flags.hh:24
END_POS
Definition: flags.hh:23
INIT_POS
Definition: flags.hh:20
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1_c1.hh:129
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_dc1_c1.hh:39
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1_c1.hh:67
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1_c1.hh:52
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_dc1_c1.hh:25
Definition: common_solve_methods.hh:15
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_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:62
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:17
const int DIM_POINT
Definition: solve_end_effector.hh:15
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:22
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
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
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
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
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
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:92
point_t dc0_
Definition: data.hh:107
std::vector< ContactData > contacts_
Definition: data.hh:106
point_t dc1_
Definition: data.hh:107
point_t c0_
Definition: data.hh:107
point_t c1_
Definition: data.hh:107
Definition: utils.hh:25
VectorX second
Definition: utils.hh:27
MatrixXX first
Definition: utils.hh:26