hpp-bezier-com-traj  4.15.1
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_dc1.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_C0DC0D1_H
7 #define BEZIER_COM_TRAJ_C0DC0D1_H
8 
10 
11 namespace bezier_com_traj {
12 namespace c0_dc0_dc1 {
13 
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | 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  // equation found with sympy
30  wp.first = -2.0 * t3 + 3.0 * t2;
31  wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t +
32  1.0 * pi[0] + 3.0 * pi[1] * t3 - 6.0 * pi[1] * t2 +
33  3.0 * pi[1] * t;
34  return wp;
35 }
36 
37 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
38  double T, double t) {
39  coefs_t wp;
40  double alpha = 1. / (T * T);
41  // equation found with sympy
42  wp.first = (-12.0 * t + 6.0) * alpha;
43  wp.second =
44  (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1]) *
45  alpha;
46  return wp;
47 }
48 
49 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
50  double T) {
51  // equation for constraint on initial and final position and velocity (degree
52  // 3, 2 constant waypoints and two free (p2 = p3)) first, compute the constant
53  // waypoints that only depend on pData :
54  if (pData.dc1_.norm() != 0.)
55  throw std::runtime_error(
56  "Capturability not implemented for spped different than 0");
57  std::vector<point_t> pi;
58  pi.push_back(pData.c0_); // p0
59  pi.push_back((pData.dc0_ * T / 3.) + pData.c0_); // p1
60  pi.push_back(point_t::Zero()); // p2 = x
61  pi.push_back(point_t::Zero()); // p3 = x
62  return pi;
63 }
64 
65 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
66  double T) {
67  bezier_wp_t::t_point_t wps;
68  const int DIM_POINT = 6;
69  const int DIM_VAR = 3;
70  std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData, T);
71  std::vector<Matrix3> Cpi;
72  for (std::size_t i = 0; i < pi.size(); ++i) {
73  Cpi.push_back(skew(pi[i]));
74  }
75  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
76  const Matrix3 Cg = skew(g);
77  const double T2 = T * T;
78  const double alpha = 1. / (T2);
79  // equation of waypoints for curve w found with sympy
80  // TODO Apparently sympy equations are false ...
81 
82  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
83  w0.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
84  w0.first.block<3, 3>(3, 0) = 6.0 * Cpi[0] * alpha;
85  w0.second.head<3>() = (6 * pi[0] - 12 * pi[1]) * alpha;
86  w0.second.tail<3>() = (-Cpi[0]) * (12.0 * pi[1] * alpha + g);
87  wps.push_back(w0);
88  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
89  w1.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity();
90  w1.first.block<3, 3>(3, 0) = skew(1.5 * (3 * pi[1] - pi[0])) * alpha;
91  w1.second.head<3>() = 1.5 * alpha * (3 * pi[0] - 5 * pi[1]);
92  w1.second.tail<3>() =
93  (3 * alpha * pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3 * pi[1] + pi[0]));
94  wps.push_back(w1);
95  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
96  w2.first.block<3, 3>(0, 0) = 0 * alpha * Matrix3::Identity();
97  w2.first.block<3, 3>(3, 0) =
98  skew(0.5 * g - 3 * alpha * pi[0] + 3 * alpha * pi[1]);
99  w2.second.head<3>() = 3 * alpha * (pi[0] - pi[1]);
100  w2.second.tail<3>() = 0.5 * Cg * pi[1];
101  wps.push_back(w2);
102  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
103  w3.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity();
104  w3.first.block<3, 3>(3, 0) = skew(g - 1.5 * alpha * (pi[1] + pi[0]));
105  w3.second.head<3>() = 1.5 * alpha * (pi[1] + pi[0]);
106  wps.push_back(w3);
107  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
108  w4.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity();
109  w4.first.block<3, 3>(3, 0) = skew(g - 6 * alpha * pi[1]);
110  w4.second.head<3>() = 6 * pi[1] * alpha;
111  wps.push_back(w4);
112  return wps;
113 }
114 
115 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
116  coefs_t v;
117  std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData, T);
118  // equation found with sympy
119  v.first = 0.;
120  v.second = point3_t::Zero();
121  return v;
122 }
123 
124 } // namespace c0_dc0_dc1
125 } // namespace bezier_com_traj
126 
127 #endif
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
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_dc1.hh:37
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_dc1.hh:65
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
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.hh:25
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
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1.hh:115
point_t dc1_
Definition: data.hh:107
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
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
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
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1.hh:49
const int DIM_POINT
Definition: solve_end_effector.hh:15