6 #ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
7 #define BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
12 namespace c0_dc0_ddc0_ddc1_dc1_c1 {
33 wp.first = -20.0 * t6 + 60.0 * t5 - 60.0 * t4 + 20.0 * t3;
34 wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 - 20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 -
35 6.0 * pi[0] * t + 1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 - 60.0 * pi[1] * t4 +
36 60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 + 6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
37 90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 + 15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 +
38 15.0 * pi[4] * t4 - 6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
44 double alpha = 1. / (T * T);
49 wp.first = 1.0 * (-600.0 * t4 + 1200.0 * t3 - 720.0 * t2 + 120.0 * t) * alpha;
51 (30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 - 120.0 * pi[0] * t + 30.0 * pi[0] -
52 180.0 * pi[1] * t4 + 600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t - 60.0 * pi[1] +
53 450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 + 1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
54 450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 - 180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 +
64 std::vector<point_t> pi;
65 pi.push_back(pData.
c0_);
66 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
67 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) + (2. * pData.
dc0_ * T / n) + pData.
c0_);
68 pi.push_back(point_t::Zero());
69 pi.push_back((pData.
ddc1_ * T * T / (n * (n - 1))) - (2 * pData.
dc1_ * T / n) + pData.
c1_);
70 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
71 pi.push_back(pData.
c1_);
76 bezier_wp_t::t_point_t wps;
78 const int DIM_VAR = 3;
80 std::vector<Matrix3> Cpi;
81 for (std::size_t i = 0; i < pi.size(); ++i) {
82 Cpi.push_back(
skew(pi[i]));
86 const double T2 = T * T;
87 const double alpha = 1 / (T2);
91 w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
92 w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
95 w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
96 w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
97 w1.second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
98 w1.second.tail<3>() = 1.0 *
99 (0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
100 30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
104 w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
105 w2.first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
106 w2.second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
107 w2.second.tail<3>() = 1.0 *
108 (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
109 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
113 w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
114 w3.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
115 w3.second.head<3>() = 1.0 *
116 (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
117 7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
119 w3.second.tail<3>() = 1.0 *
120 (0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
121 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
122 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
126 w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
127 w4.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
128 w4.second.head<3>() = 1.0 *
129 (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
130 4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
132 w4.second.tail<3>() =
134 (0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
135 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
136 12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
140 w5.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
141 w5.
first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
142 w5.
second.head<3>() = 1.0 *
143 (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
144 3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
148 (+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
149 0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
150 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
154 w6.
first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
155 w6.
first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
156 w6.
second.head<3>() = 1.0 *
157 (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
158 7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
160 w6.
second.tail<3>() = 1.0 *
161 (0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
162 0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
163 12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
167 w7.
first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
168 w7.
first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
169 w7.
second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
170 w7.
second.tail<3>() = 1.0 *
171 (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
172 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
176 w8.
first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
177 w8.
first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
178 w8.
second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
179 w8.
second.tail<3>() = 1.0 *
180 (0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
181 20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
185 w9.
second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
186 w9.
second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
196 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;