6 #ifndef BEZIER_COM_TRAJ_C0DC0D1_H
7 #define BEZIER_COM_TRAJ_C0DC0D1_H
12 namespace c0_dc0_dc1 {
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 +
40 double alpha = 1. / (T * T);
42 wp.first = (-12.0 * t + 6.0) * alpha;
44 (-6.0 * pi[0] * t + 6.0 * pi[0] + 18.0 * pi[1] * t - 12.0 * pi[1]) *
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_);
59 pi.push_back((pData.
dc0_ * T / 3.) + pData.
c0_);
60 pi.push_back(point_t::Zero());
61 pi.push_back(point_t::Zero());
67 bezier_wp_t::t_point_t wps;
69 const int DIM_VAR = 3;
71 std::vector<Matrix3> Cpi;
72 for (std::size_t i = 0; i < pi.size(); ++i) {
73 Cpi.push_back(
skew(pi[i]));
77 const double T2 = T * T;
78 const double alpha = 1. / (T2);
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);
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]);
93 (3 * alpha * pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3 * pi[1] + pi[0]));
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];
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]);
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;
120 v.second = point3_t::Zero();
INIT_VEL
Definition: flags.hh:21
END_VEL
Definition: flags.hh:24
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_dc1.hh:25
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_dc1.hh:37
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1.hh:49
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1.hh:115
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_dc1.hh:65
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 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
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 dc1_
Definition: data.hh:107
point_t c0_
Definition: data.hh:107