6 #ifndef BEZIER_COM_TRAJ_C0DC0D1_H
7 #define BEZIER_COM_TRAJ_C0DC0D1_H
12 namespace c0_dc0_dc1 {
28 wp.first = -2.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;
36 double alpha = 1. / (T * T);
38 wp.first = (-12.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]) * alpha;
46 if (pData.
dc1_.norm() != 0.)
throw std::runtime_error(
"Capturability not implemented for spped different than 0");
47 std::vector<point_t> pi;
48 pi.push_back(pData.
c0_);
49 pi.push_back((pData.
dc0_ * T / 3.) + pData.
c0_);
50 pi.push_back(point_t::Zero());
51 pi.push_back(point_t::Zero());
56 bezier_wp_t::t_point_t wps;
58 const int DIM_VAR = 3;
60 std::vector<Matrix3> Cpi;
61 for (std::size_t i = 0; i < pi.size(); ++i) {
62 Cpi.push_back(
skew(pi[i]));
66 const double T2 = T * T;
67 const double alpha = 1. / (T2);
72 w0.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
73 w0.first.block<3, 3>(3, 0) = 6.0 * Cpi[0] * alpha;
74 w0.second.head<3>() = (6 * pi[0] - 12 * pi[1]) * alpha;
75 w0.second.tail<3>() = (-Cpi[0]) * (12.0 * pi[1] * alpha + g);
78 w1.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity();
79 w1.first.block<3, 3>(3, 0) =
skew(1.5 * (3 * pi[1] - pi[0])) * alpha;
80 w1.second.head<3>() = 1.5 * alpha * (3 * pi[0] - 5 * pi[1]);
81 w1.second.tail<3>() = (3 * alpha * pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3 * pi[1] + pi[0]));
84 w2.first.block<3, 3>(0, 0) = 0 * alpha * Matrix3::Identity();
85 w2.first.block<3, 3>(3, 0) =
skew(0.5 * g - 3 * alpha * pi[0] + 3 * alpha * pi[1]);
86 w2.second.head<3>() = 3 * alpha * (pi[0] - pi[1]);
87 w2.second.tail<3>() = 0.5 * Cg * pi[1];
90 w3.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity();
91 w3.first.block<3, 3>(3, 0) =
skew(g - 1.5 * alpha * (pi[1] + pi[0]));
92 w3.second.head<3>() = 1.5 * alpha * (pi[1] + pi[0]);
95 w4.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity();
96 w4.first.block<3, 3>(3, 0) =
skew(g - 6 * alpha * pi[1]);
97 w4.second.head<3>() = 6 * pi[1] * alpha;
107 v.second = point3_t::Zero();