6 #ifndef BEZIER_COM_TRAJ_C0DC0D1C1_H 7 #define BEZIER_COM_TRAJ_C0DC0D1C1_H 12 namespace c0_dc0_dc1_c1 {
29 wp.first = (6.0 * t4 - 12.0 * t3 + 6.0 * t2);
30 wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 - 4.0 * pi[0] * t + 1.0 * pi[0] -
31 4.0 * pi[1] * t4 + 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t - 4.0 * pi[3] * t4 +
32 4.0 * pi[3] * t3 + 1.0 * pi[4] * t4;
38 double alpha = 1. / (T * T);
40 wp.first = (72.0 * t * t - 72.0 * t + 12.0) * alpha;
41 wp.second = (12.0 * pi[0] * t * t - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t * t + 72.0 * pi[1] * t -
42 24.0 * pi[1] - 48.0 * pi[3] * t * t + 24.0 * pi[3] * t + 12.0 * pi[4] * t * t) *
51 std::vector<point_t> pi;
52 pi.push_back(pData.
c0_);
53 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
54 pi.push_back(point_t::Zero());
55 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
56 pi.push_back(pData.
c1_);
61 bezier_wp_t::t_point_t wps;
63 const int DIM_VAR = 3;
65 std::vector<Matrix3> Cpi;
66 for (std::size_t i = 0; i < pi.size(); ++i) {
67 Cpi.push_back(
skew(pi[i]));
71 const double T2 = T * T;
72 const double alpha = 1 / (T2);
75 w0.
first.block<3, 3>(0, 0) = 12. * alpha * Matrix3::Identity();
76 w0.
first.block<3, 3>(3, 0) = 12. * alpha * Cpi[0];
77 w0.
second.head<3>() = (12. * pi[0] - 24. * pi[1]) * alpha;
78 w0.
second.tail<3>() = 1.0 * Cg * pi[0] - (24.0 * Cpi[0] * pi[1]) * alpha;
81 w1.
first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
82 w1.
first.block<3, 3>(3, 0) = (-12.0 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
83 w1.
second.head<3>() = (7.2 * pi[0] - 9.6 * pi[1] + 4.8 * pi[3]) * alpha;
84 w1.
second.tail<3>() = (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] + 4.8 * Cpi[0] * pi[3]) * alpha;
87 w2.
first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
88 w2.
first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[1]) * alpha;
89 w2.
second.head<3>() = (3.6 * pi[0] + 4.8 * pi[3] + 1.2 * pi[4]) * alpha;
91 (0.4 * Cg * T2 * pi[1] - 4.8 * Cpi[0] * pi[3] + 1.2 * Cpi[0] * pi[4] + 9.6 * Cpi[1] * pi[3]) * alpha;
94 w3.
first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
95 w3.
first.block<3, 3>(3, 0) = (0.6 * Cg * T2 - 9.6 * Cpi[3]) * alpha;
96 w3.
second.head<3>() = (1.2 * pi[0] + 4.8 * pi[1] + 3.6 * pi[4]) * alpha;
98 (0.4 * Cg * T2 * pi[3] - 1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[3] + 4.8 * Cpi[1] * pi[4]) * alpha;
101 w4.
first.block<3, 3>(0, 0) = -2.4 * alpha * Matrix3::Identity();
102 w4.
first.block<3, 3>(3, 0) = (9.6 * Cpi[3] - 12.0 * Cpi[4]) * alpha;
103 w4.
second.head<3>() = (4.8 * pi[1] - 9.6 * pi[3] + 7.2 * pi[4]) * alpha;
104 w4.
second.tail<3>() = (0.8 * Cg * T2 * pi[3] + 0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4]) * alpha;
107 w5.
first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();
108 w5.
first.block<3, 3>(3, 0) = 12.0 * Cpi[4] * alpha;
109 w5.
second.head<3>() = (-24 * pi[3] + 12 * pi[4]) * alpha;
110 w5.
second.tail<3>() = (Cg * T2 * pi[4] + 24.0 * Cpi[3] * pi[4]) * alpha;
120 v.second = (-4.0 * pi[3] + 4.0 * pi[4]) / T;
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1_c1.hh:47
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
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:21
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
VectorX second
Definition: utils.hh:29
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1_c1.hh:115
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
INIT_VEL
Definition: flags.hh:21
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1_c1.hh:60
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:52
point_t c0_
Definition: data.hh:103
MatrixXX first
Definition: utils.hh:28
END_POS
Definition: flags.hh:23
point_t dc1_
Definition: data.hh:103
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
point_t dc0_
Definition: data.hh:103
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
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_dc1_c1.hh:36
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:16
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:88
Definition: common_solve_methods.hh:16
INIT_POS
Definition: flags.hh:20
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
coefs_t evaluateCurveAtTime(const std::vector< point_t > &pi, double t)
EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE = 4)
Definition: waypoints_c0_dc0_dc1_c1.hh:23
std::vector< ContactData > contacts_
Definition: data.hh:102
point_t c1_
Definition: data.hh:103
const int DIM_POINT
Definition: solve_end_effector.hh:15