6 #ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
12 namespace c0_dc0_ddc0_dc1_c1 {
32 wp.first = 10.0 * t5 - 20.0 * t4 + 10.0 * t3;
33 wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 + 10.0 * pi[0] * t2 - 5.0 * pi[0] * t +
34 1.0 * pi[0] + 5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 - 20.0 * pi[1] * t2 +
35 5.0 * pi[1] * t - 10.0 * pi[2] * t5 + 30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
36 5.0 * pi[4] * t5 + 5.0 * pi[4] * t4 + 1.0 * pi[5] * t5;
42 double alpha = 1. / (T * T);
46 wp.first = (200.0 * t3 - 240.0 * t2 + 60.0 * t) * alpha;
48 (-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t + 20.0 * pi[0] + 100.0 * pi[1] * t3 -
49 240.0 * pi[1] * t2 + 180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 + 360.0 * pi[2] * t2 -
50 180.0 * pi[2] * t + 20.0 * pi[2] - 100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
59 std::vector<point_t> pi;
60 pi.push_back(pData.
c0_);
61 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
62 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) + (2. * pData.
dc0_ * T / n) + pData.
c0_);
63 pi.push_back(point_t::Zero());
64 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
65 pi.push_back(pData.
c1_);
70 bezier_wp_t::t_point_t wps;
72 const int DIM_VAR = 3;
74 std::vector<Matrix3> Cpi;
75 for (std::size_t i = 0; i < pi.size(); ++i) {
76 Cpi.push_back(
skew(pi[i]));
80 const double T2 = T * T;
81 const double alpha = 1 / (T2);
85 w0.second.head<3>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
86 w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) * alpha;
89 w1.first.block<3, 3>(0, 0) = 8.57142857142857 * alpha * Matrix3::Identity();
90 w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
91 w1.second.head<3>() = 1.0 * (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] - 5.71428571428572 * pi[2]) * alpha;
92 w1.second.tail<3>() = 1.0 *
93 (0.285714285714286 * Cg * T2 * pi[0] + 0.714285714285714 * Cg * T2 * pi[1] -
94 20.0 * Cpi[0] * pi[2] + 14.2857142857143 * Cpi[1] * pi[2]) *
98 w2.first.block<3, 3>(0, 0) = 5.71428571428571 * alpha * Matrix3::Identity();
99 w2.first.block<3, 3>(3, 0) = 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
100 w2.second.head<3>() = 1.0 * (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] + 2.85714285714286 * pi[4]) * alpha;
101 w2.second.tail<3>() =
103 (0.0476190476190479 * Cg * T2 * pi[0] + 0.476190476190476 * Cg * T2 * pi[1] +
104 0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] - 14.2857142857143 * Cpi[1] * pi[2]) *
108 w3.first.block<3, 3>(0, 0) = -2.85714285714286 * alpha * Matrix3::Identity();
109 w3.first.block<3, 3>(3, 0) =
110 1.0 * (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] + 11.4285714285714 * Cpi[2]) * alpha;
111 w3.second.head<3>() = 1.0 *
112 (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] - 11.4285714285714 * pi[2] +
113 5.71428571428571 * pi[4] + 0.571428571428571 * pi[5]) *
115 w3.second.tail<3>() =
117 (0.142857142857143 * Cg * T2 * pi[1] + 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
118 0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
122 w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
123 w4.first.block<3, 3>(3, 0) = 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
124 w4.second.head<3>() = 1.0 *
125 (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] - 2.85714285714286 * pi[2] +
126 5.71428571428571 * pi[4] + 2.28571428571429 * pi[5]) *
128 w4.second.tail<3>() =
130 (0.285714285714286 * Cg * T2 * pi[2] + 0.142857142857143 * Cg * T2 * pi[4] - 0.571428571428572 * Cpi[0] * pi[5] -
131 8.57142857142857 * Cpi[1] * pi[4] + 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
135 w5.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
136 w5.
first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
137 w5.
second.head<3>() = 1.0 * (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] + 5.71428571428571 * pi[5]) * alpha;
140 (0.476190476190476 * Cg * T2 * pi[4] + 0.0476190476190476 * Cg * T2 * pi[5] - 2.85714285714286 * Cpi[1] * pi[5] -
141 14.2857142857143 * Cpi[2] * pi[4] + 8.57142857142857 * Cpi[2] * pi[5]) *
145 w6.
first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
146 w6.
first.block<3, 3>(3, 0) = 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
147 w6.
second.head<3>() = 1.0 * (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] + 11.4285714285714 * pi[5]) * alpha;
150 (0.714285714285714 * Cg * T2 * pi[4] + 0.285714285714286 * Cg * T2 * pi[5] - 8.57142857142858 * Cpi[2] * pi[5]) *
154 w7.
first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
155 w7.
first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
156 w7.
second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
157 w7.
second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
167 v.second = (-5.0 * pi[4] + 5.0 * pi[5]) / T;