hpp-bezier-com-traj  4.13.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_ddc0_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_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
8 
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 
18 
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
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
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_c1.hh:72
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_c1.hh:142
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
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_c1.hh:26
point_t c0_
Definition: data.hh:107
MatrixXX first
Definition: utils.hh:26
END_POS
Definition: flags.hh:23
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
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:17
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_c1.hh:41
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:92
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_c1.hh:56
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