hpp-bezier-com-traj  4.9.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_ddc0.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_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_H
8 
10 
11 namespace bezier_com_traj {
12 namespace c0_dc0_ddc0 {
13 
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC;
15 
17 
24 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
25  coefs_t wp;
26  double t2 = t * t;
27  double t3 = t2 * t;
28  // equation found with sympy
29  // (-1.0*pi[0] + 3.0*pi[1] - 3.0*pi[2] + 1.0*x)*t**3 + (3.0*pi[0] - 6.0*pi[1] + 3.0*pi[2])*T2 +
30  // (-3.0*pi[0] + 3.0*pi[1])*t + 1.0*pi[0],
31  wp.first = t3;
32  wp.second =
33  t3 * (3 * (pi[1] - pi[2]) - pi[0]) + t2 * (3 * (pi[0] + pi[2]) - 6 * pi[1]) + 3 * t * (pi[1] - pi[0]) + pi[0];
34  return wp;
35 }
36 
37 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
38  coefs_t wp;
39  double alpha = 1. / (T * T);
40  // equation found with sympy
41  // 6.0*t*alpha*x + (-6.0*pi[0] + 18.0*pi[1] - 18.0*pi[2])/T2*t + (6.0*pi[0] - 12.0*pi[1] + 6.0*pi[2])/T2
42  wp.first = 6.0 * t * alpha;
43  wp.second = (18. * (pi[1] - pi[2]) - 6. * pi[0]) * alpha * t + (6. * (pi[0] + pi[2]) - 12.0 * pi[1]) * alpha;
44  return wp;
45 }
46 
47 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
48  // equation for constraint on initial position, velocity and acceleration, and only final position (degree =
49  // 4)(degree 4, 4 constant waypoint and one free (p3)) first, compute the constant waypoints that only depend on
50  // pData :
51  double n = 3.;
52  std::vector<point_t> pi;
53  pi.push_back(pData.c0_); // pi[0]
54  pi.push_back((pData.dc0_ * T / n) + pData.c0_); // pi[1]
55  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // pi[2]
56  pi.push_back(point_t::Zero()); // x
57  return pi;
58 }
59 
60 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
61  bezier_wp_t::t_point_t wps;
62  const int DIM_POINT = 6;
63  const int DIM_VAR = 3;
64  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
65  std::vector<Matrix3> Cpi;
66  for (std::size_t i = 0; i < pi.size(); ++i) {
67  Cpi.push_back(skew(pi[i]));
68  }
69  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
70  const Matrix3 Cg = skew(g), Id = Matrix3::Identity();
71  const double T2 = T * T;
72  const double alpha = 1 / (T2);
73 
74  // equation of waypoints for curve w found with sympy
75  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
76  w0.second.head<3>() = (6 * pi[0] - 12 * pi[1] + 6 * pi[2]) * alpha;
77  w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1] + 6.0 * Cpi[0] * pi[2]) * alpha;
78  wps.push_back(w0);
79  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
80  w1.first.block<3, 3>(0, 0) = 2.0 * alpha * Id;
81  w1.first.block<3, 3>(3, 0) = 2.0 * Cpi[0] * alpha;
82  w1.second.head<3>() = 1.0 * (4.0 * pi[0] - 6.0 * pi[1]) * alpha;
83  w1.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[1] - 6.0 * Cpi[0] * pi[2] + 6.0 * Cpi[1] * pi[2]) * alpha;
84  wps.push_back(w1);
85  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
86  w2.first.block<3, 3>(0, 0) = 4.0 * alpha * Id;
87  w2.first.block<3, 3>(3, 0) = 1.0 * (-2.0 * Cpi[0] + 6.0 * Cpi[1]) * alpha;
88  w2.second.head<3>() = 1.0 * (2.0 * pi[0] - 6.0 * pi[2]) * alpha;
89  w2.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[2] - 6.0 * Cpi[1] * pi[2]) * alpha;
90  wps.push_back(w2);
91  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
92  w3.first.block<3, 3>(0, 0) = 6 * alpha * Id;
93  w3.first.block<3, 3>(3, 0) = 1.0 * (1.0 * Cg * T2 - 6.0 * Cpi[1] + 12.0 * Cpi[2]) * alpha;
94  w3.second.head<3>() = (6 * pi[1] - 12 * pi[2]) * alpha;
95  // w3.second.head<3>() = 0;
96  wps.push_back(w3);
97  return wps;
98 }
99 
100 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
101  coefs_t v;
102  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
103  // equation found with sympy
104  // 3.0*(-pi[2] + x)/T
105  v.first = 3. / T;
106  v.second = -3. * pi[2] / T;
107  return v;
108 }
109 
110 } // namespace c0_dc0_ddc0
111 } // namespace bezier_com_traj
112 
113 #endif
INIT_ACC
INIT_ACC
Definition: flags.hh:22
bezier_com_traj::c0_dc0_ddc0::computeWwaypoints
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0.hh:60
bezier_com_traj::initwp
T initwp()
bezier_com_traj::waypoint_t
Definition: utils.hh:27
bezier_com_traj::w1
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:22
bezier_com_traj::c0_dc0_ddc0::evaluateCurveAtTime
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.hh:24
bezier_com_traj::coefs_t
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
bezier_com_traj::ProblemData::dc0_
point_t dc0_
Definition: data.hh:103
bezier_com_traj::c0_dc0_ddc0::computeConstantWaypoints
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0.hh:47
bezier_com_traj::Vector3
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:21
bezier_com_traj::c0_dc0_ddc0::evaluateAccelerationCurveAtTime
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0.hh:37
bezier_com_traj::c0_dc0_ddc0::computeFinalVelocityPoint
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0.hh:100
bezier_com_traj::Matrix3
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:16
INIT_VEL
INIT_VEL
Definition: flags.hh:21
bezier_com_traj::ProblemData::c0_
point_t c0_
Definition: data.hh:103
bezier_com_traj::w3
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:42
bezier_com_traj::ProblemData::ddc0_
point_t ddc0_
Definition: data.hh:103
bezier_com_traj::DIM_POINT
const int DIM_POINT
Definition: solve_end_effector.hh:15
bezier_com_traj::w2
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:32
bezier_com_traj::skew
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
data.hh
bezier_com_traj::w0
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
Definition: common_solve_methods.hh:16
bezier_com_traj::ProblemData
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:88
bezier_com_traj::ProblemData::contacts_
std::vector< ContactData > contacts_
Definition: data.hh:102
INIT_POS
INIT_POS
Definition: flags.hh:20