6 #ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H
12 namespace c0_dc0_ddc0_c1 {
30 wp.first = -4.0 * t4 + 4.0 * t3;
31 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] -
32 4.0 * pi[1] * t4 + 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t + 6.0 * pi[2] * t4 -
33 12.0 * pi[2] * t3 + 6.0 * pi[2] * t2 + 1.0 * pi[4] * t4;
39 double alpha = 1. / (T * T);
42 wp.first = (-48.0 * t2 + 24.0 * t) * alpha;
43 wp.second = (12.0 * pi[0] * t2 - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t2 + 72.0 * pi[1] * t -
44 24.0 * pi[1] + 72.0 * pi[2] * t2 - 72.0 * pi[2] * t + 12.0 * pi[2] + 12.0 * pi[4] * t2) *
54 std::vector<point_t> pi;
55 pi.push_back(pData.
c0_);
56 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
57 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) + (2. * pData.
dc0_ * T / n) + pData.
c0_);
58 pi.push_back(point_t::Zero());
59 pi.push_back(pData.
c1_);
64 bezier_wp_t::t_point_t wps;
66 const int DIM_VAR = 3;
68 std::vector<Matrix3> Cpi;
69 for (std::size_t i = 0; i < pi.size(); ++i) {
70 Cpi.push_back(
skew(pi[i]));
74 const double T2 = T * T;
75 const double alpha = 1 / (T2);
79 w0.second.head<3>() = (12 * pi[0] - 24 * pi[1] + 12 * pi[2]) * alpha;
80 w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 24.0 * Cpi[0] * pi[1] + 12.0 * Cpi[0] * pi[2]) * alpha;
83 w1.first.block<3, 3>(0, 0) = 4.8 * alpha * Matrix3::Identity();
84 w1.first.block<3, 3>(3, 0) = 4.8 * Cpi[0] * alpha;
85 w1.second.head<3>() = 1.0 * (7.2 * pi[0] - 9.6 * pi[1] - 2.4 * pi[2]) * alpha;
87 1.0 * (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] - 12.0 * Cpi[0] * pi[2] + 9.6 * Cpi[1] * pi[2]) * alpha;
90 w2.first.block<3, 3>(0, 0) = 4.8 * alpha * Matrix3::Identity();
91 w2.first.block<3, 3>(3, 0) = 1.0 * (-4.8 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
92 w2.second.head<3>() = 1.0 * (3.6 * pi[0] - 9.6 * pi[2] + 1.2 * pi[4]) * alpha;
94 1.0 * (0.4 * Cg * T2 * pi[1] + 0.6 * Cg * T2 * pi[2] + 1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[2]) * alpha;
97 w3.first.block<3, 3>(3, 0) = 1.0 * (0.4 * Cg * T2 - 9.6 * Cpi[1] + 9.6 * Cpi[2]) * alpha;
98 w3.second.head<3>() = 1.0 * (1.2 * pi[0] + 4.8 * pi[1] - 9.6 * pi[2] + 3.6 * pi[4]) * alpha;
99 w3.second.tail<3>() = 1.0 * (0.6 * Cg * T2 * pi[2] - 1.2 * Cpi[0] * pi[4] + 4.8 * Cpi[1] * pi[4]) * alpha;
102 w4.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
103 w4.first.block<3, 3>(3, 0) = 1.0 * (0.8 * Cg * T2 - 9.6 * Cpi[2]) * alpha;
104 w4.second.head<3>() = 1.0 * (4.8 * pi[1] - 2.4 * pi[2] + 7.2 * pi[4]) * alpha;
105 w4.second.tail<3>() = 1.0 * (0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4] + 12.0 * Cpi[2] * pi[4]) * alpha;
108 w5.
first.block<3, 3>(0, 0) = -24 * alpha * Matrix3::Identity();
109 w5.
first.block<3, 3>(3, 0) = 1.0 * (-24.0 * Cpi[4]) * alpha;
110 w5.
second.head<3>() = (12 * pi[2] + 12 * pi[4]) * alpha;
111 w5.
second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[4] - 12.0 * Cpi[2] * pi[4]) * alpha;
120 v.second = 4. * pData.
c1_ / T;