6#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_H
7#define BEZIER_COM_TRAJ_c0_dc0_ddc0_H
12namespace c0_dc0_ddc0 {
35 wp.second = t3 * (3 * (pi[1] - pi[2]) - pi[0]) +
36 t2 * (3 * (pi[0] + pi[2]) - 6 * pi[1]) + 3 * t * (pi[1] - pi[0]) +
44 double alpha = 1. / (T * T);
48 wp.first = 6.0 * t * alpha;
49 wp.second = (18. * (pi[1] - pi[2]) - 6. * pi[0]) * alpha * t +
50 (6. * (pi[0] + pi[2]) - 12.0 * pi[1]) * alpha;
60 std::vector<point_t> pi;
61 pi.push_back(pData.
c0_);
62 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
63 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) +
64 (2. * pData.
dc0_ * T / n) + pData.
c0_);
65 pi.push_back(point_t::Zero());
71 bezier_wp_t::t_point_t wps;
73 const int DIM_VAR = 3;
75 std::vector<Matrix3> Cpi;
76 for (std::size_t i = 0; i < pi.size(); ++i) {
77 Cpi.push_back(
skew(pi[i]));
80 const Matrix3 Cg =
skew(g), Id = Matrix3::Identity();
81 const double T2 = T * T;
82 const double alpha = 1 / (T2);
86 w0.second.head<3>() = (6 * pi[0] - 12 * pi[1] + 6 * pi[2]) * alpha;
89 (1.0 * Cg * T2 * pi[0] - 12.0 * Cpi[0] * pi[1] + 6.0 * Cpi[0] * pi[2]) *
93 w1.first.block<3, 3>(0, 0) = 2.0 * alpha * Id;
94 w1.first.block<3, 3>(3, 0) = 2.0 * Cpi[0] * alpha;
95 w1.second.head<3>() = 1.0 * (4.0 * pi[0] - 6.0 * pi[1]) * alpha;
98 (1.0 * Cg * T2 * pi[1] - 6.0 * Cpi[0] * pi[2] + 6.0 * Cpi[1] * pi[2]) *
102 w2.first.block<3, 3>(0, 0) = 4.0 * alpha * Id;
103 w2.first.block<3, 3>(3, 0) = 1.0 * (-2.0 * Cpi[0] + 6.0 * Cpi[1]) * alpha;
104 w2.second.head<3>() = 1.0 * (2.0 * pi[0] - 6.0 * pi[2]) * alpha;
105 w2.second.tail<3>() =
106 1.0 * (1.0 * Cg * T2 * pi[2] - 6.0 * Cpi[1] * pi[2]) * alpha;
109 w3.first.block<3, 3>(0, 0) = 6 * alpha * Id;
110 w3.first.block<3, 3>(3, 0) =
111 1.0 * (1.0 * Cg * T2 - 6.0 * Cpi[1] + 12.0 * Cpi[2]) * alpha;
112 w3.second.head<3>() = (6 * pi[1] - 12 * pi[2]) * alpha;
124 v.second = -3. * pi[2] / T;
INIT_VEL
Definition: flags.hh:21
INIT_ACC
Definition: flags.hh:22
INIT_POS
Definition: flags.hh:20
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:26
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0.hh:54
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0.hh:41
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0.hh:118
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0.hh:69
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 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 ddc0_
Definition: data.hh:107
point_t c0_
Definition: data.hh:107