hpp-bezier-com-traj
4.9.0
Multi contact trajectory generation for the COM using Bezier curves
|
Go to the documentation of this file.
6 #ifndef BEZIER_COM_TRAJ_C0DC0C1_H
7 #define BEZIER_COM_TRAJ_C0DC0C1_H
28 wp.first = -3.0 * t3 + 3.0 * t2;
29 wp.second = -1.0 * pi[0] * t3 + 3.0 * pi[0] * t2 - 3.0 * pi[0] * t + 1.0 * pi[0] + 3.0 * pi[1] * t3 -
30 6.0 * pi[1] * t2 + 3.0 * pi[1] * t + 1.0 * pi[3] * t3;
36 double alpha = 1. / (T * T);
38 wp.first = (-18.0 * t + 6.0) * alpha;
39 wp.second = (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1] + 6.0 * pi[3] * t) * alpha;
47 std::vector<point_t> pi;
48 pi.push_back(pData.
c0_);
49 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
50 pi.push_back(point_t::Zero());
51 pi.push_back(pData.
c1_);
57 bezier_wp_t::t_point_t wps;
59 const int DIM_VAR = 3;
61 std::vector<Matrix3> Cpi;
62 for (std::size_t i = 0; i < pi.size(); ++i) {
63 Cpi.push_back(
skew(pi[i]));
67 const double T2 = T * T;
68 const double alpha = 1 / (T2);
71 w0.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
72 w0.first.block<3, 3>(3, 0) = 6.0 * Cpi[0] * alpha;
73 w0.second.head<3>() = (6 * pi[0] - 12 * pi[1]) * alpha;
74 w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1]) * alpha;
77 w1.first.block<3, 3>(3, 0) = 1.0 * (-6.0 * Cpi[0] + 6.0 * Cpi[1]) * alpha;
78 w1.second.head<3>() = 1.0 * (4.0 * pi[0] - 6.0 * pi[1] + 2.0 * pi[3]) * alpha;
79 w1.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[1] + 2.0 * Cpi[0] * pi[3]) * alpha;
82 w2.first.block<3, 3>(0, 0) = -6.0 * alpha * Matrix3::Identity();
83 w2.first.block<3, 3>(3, 0) = 1.0 * (1.0 * Cg * T2 - 6.0 * Cpi[1]) * alpha;
84 w2.second.head<3>() = 1.0 * (2.0 * pi[0] + 4.0 * pi[3]) * alpha;
85 w2.second.tail<3>() = 1.0 * (-2.0 * Cpi[0] * pi[3] + 6.0 * Cpi[1] * pi[3]) * alpha;
88 w3.first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity();
89 w3.first.block<3, 3>(3, 0) = -12.0 * Cpi[3] * alpha;
90 w3.second.head<3>() = (6 * pi[1] + 6 * pi[3]) * alpha;
91 w3.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[3] - 6.0 * Cpi[1] * pi[3]) * alpha;
100 v.second = 3. * pData.
c1_ / T;
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_c1.hh:34
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
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_c1.hh:43
END_POS
Definition: flags.hh:23
point_t dc0_
Definition: data.hh:103
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:21
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:16
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_c1.hh:23
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_c1.hh:56
INIT_VEL
Definition: flags.hh:21
point_t c0_
Definition: data.hh:103
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
point_t c1_
Definition: data.hh:103
const int DIM_POINT
Definition: solve_end_effector.hh:15
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
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_c1.hh:96
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
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
Definition: common_solve_methods.hh:16
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:88
std::vector< ContactData > contacts_
Definition: data.hh:102
INIT_POS
Definition: flags.hh:20