30 double pos_start,
double vel_start,
double accel_start,
31 double pos_end,
double vel_end,
double accel_end,
32 double smp_time,
double mov_time
50 Eigen::MatrixXd poly_matrix(3,3);
51 Eigen::MatrixXd poly_vector(3,1);
54 pow(mov_time,3), pow(mov_time,4), pow(mov_time,5),
55 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4),
56 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3);
59 pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2,
60 vel_end-vel_start-accel_start*mov_time,
61 accel_end-accel_start ;
63 Eigen::Matrix<double,3,1> poly_coeff = poly_matrix.inverse() * poly_vector;
65 double time_steps = mov_time/smp_time;
66 int all_time_steps = round(time_steps+1);
68 Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1);
69 Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1);
71 for (
int step=0; step<all_time_steps; step++)
72 time.coeffRef(step,0) = step*smp_time;
74 for (
int step=0; step<all_time_steps; step++)
76 minimum_jerk_tra.coeffRef( step , 0 ) =
78 vel_start*time.coeff(step,0) +
79 0.5*accel_start*pow(time.coeff(step,0),2) +
80 poly_coeff.coeff(0,0)*pow(time.coeff(step,0),3) +
81 poly_coeff.coeff(1,0)*pow(time.coeff(step,0),4) +
82 poly_coeff.coeff(2,0)*pow(time.coeff(step,0),5);
85 return minimum_jerk_tra;
89 double pos_start,
double vel_start,
double accel_start,
90 double pos_end,
double vel_end,
double accel_end,
91 double smp_time,
double mov_time
109 Eigen::MatrixXd poly_matrix(3,3);
110 Eigen::MatrixXd poly_vector(3,1);
113 pow(mov_time,3), pow(mov_time,4), pow(mov_time,5),
114 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4),
115 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3);
118 pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2,
119 vel_end-vel_start-accel_start*mov_time,
120 accel_end-accel_start ;
122 Eigen::Matrix<double,3,1> poly_coeff = poly_matrix.inverse() * poly_vector;
124 double time_steps = mov_time/smp_time;
125 int all_time_steps = round(time_steps+1);
127 Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1);
128 Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,3);
130 for (
int step=0; step<all_time_steps; step++)
131 time.coeffRef(step,0) = step*smp_time;
133 for (
int step=0; step<all_time_steps; step++)
135 minimum_jerk_tra.coeffRef( step , 0 ) =
137 vel_start*time.coeff(step,0) +
138 0.5*accel_start*pow(time.coeff(step,0),2) +
139 poly_coeff.coeff(0,0)*pow(time.coeff(step,0),3) +
140 poly_coeff.coeff(1,0)*pow(time.coeff(step,0),4) +
141 poly_coeff.coeff(2,0)*pow(time.coeff(step,0),5);
143 minimum_jerk_tra.coeffRef( step , 1 ) =
145 accel_start*time.coeff(step,0) +
146 3*poly_coeff.coeff(0,0)*pow(time.coeff(step,0),2) +
147 4*poly_coeff.coeff(1,0)*pow(time.coeff(step,0),3) +
148 5*poly_coeff.coeff(2,0)*pow(time.coeff(step,0),4);
150 minimum_jerk_tra.coeffRef( step , 2 ) =
152 6*poly_coeff.coeff(0,0)*time.coeff(step,0) +
153 12*poly_coeff.coeff(1,0)*pow(time.coeff(step,0),2) +
154 20*poly_coeff.coeff(2,0)*pow(time.coeff(step,0),3);
157 return minimum_jerk_tra;
161 double pos_start,
double vel_start,
double accel_start,
162 Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via,
163 double pos_end,
double vel_end,
double accel_end,
164 double smp_time, Eigen::MatrixXd via_time,
double mov_time )
192 Eigen::MatrixXd poly_vector = Eigen::MatrixXd::Zero(3*via_num+3,1);
194 for (
int i=1; i<=via_num; i++)
196 poly_vector.coeffRef(3*i-3,0) =
197 pos_via.coeff(i-1,0) -
199 vel_start*via_time.coeff(i-1,0) -
200 (accel_start/2)*pow(via_time.coeff(i-1,0),2) ;
202 poly_vector.coeffRef(3*i-2,0) =
203 vel_via.coeff(i-1,0) -
205 accel_start*via_time.coeff(i-1,0) ;
207 poly_vector.coeffRef(3*i-1,0) =
208 accel_via.coeff(i-1,0) -
212 poly_vector.coeffRef(3*via_num,0) =
216 (accel_start/2)*pow(mov_time,2) ;
218 poly_vector.coeffRef(3*via_num+1,0) =
221 accel_start*mov_time ;
223 poly_vector.coeffRef(3*via_num+2,0) =
227 Eigen::MatrixXd poly_matrix_part1 = Eigen::MatrixXd::Zero(3*via_num,3);
229 for (
int i=1; i<=via_num; i++)
231 poly_matrix_part1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ;
232 poly_matrix_part1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ;
233 poly_matrix_part1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ;
235 poly_matrix_part1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ;
236 poly_matrix_part1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ;
237 poly_matrix_part1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ;
239 poly_matrix_part1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ;
240 poly_matrix_part1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ;
241 poly_matrix_part1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ;
244 Eigen::MatrixXd poly_matrix_part2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num);
246 for (
int i=1; i<=via_num; i++)
248 for (
int j=1; j<=via_num; j++)
257 poly_matrix_part2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ;
258 poly_matrix_part2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ;
259 poly_matrix_part2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ;
261 poly_matrix_part2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ;
262 poly_matrix_part2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ;
263 poly_matrix_part2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ;
265 poly_matrix_part2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ;
266 poly_matrix_part2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ;
267 poly_matrix_part2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ;
272 Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3);
274 poly_matrix_part3.coeffRef(0,0) = pow(mov_time,3);
275 poly_matrix_part3.coeffRef(0,1) = pow(mov_time,4);
276 poly_matrix_part3.coeffRef(0,2) = pow(mov_time,5);
278 poly_matrix_part3.coeffRef(1,0) = 3*pow(mov_time,2);
279 poly_matrix_part3.coeffRef(1,1) = 4*pow(mov_time,3);
280 poly_matrix_part3.coeffRef(1,2) = 5*pow(mov_time,4);
282 poly_matrix_part3.coeffRef(2,0) = 6*mov_time;
283 poly_matrix_part3.coeffRef(2,1) = 12*pow(mov_time,2);
284 poly_matrix_part3.coeffRef(2,2) = 20*pow(mov_time,3);
286 for (
int i=1; i<=via_num; i++)
288 poly_matrix_part3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ;
289 poly_matrix_part3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ;
290 poly_matrix_part3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ;
292 poly_matrix_part3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ;
293 poly_matrix_part3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ;
294 poly_matrix_part3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ;
296 poly_matrix_part3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ;
297 poly_matrix_part3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ;
298 poly_matrix_part3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ;
301 Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3);
303 poly_matrix.block(0,0,3*via_num,3) = poly_matrix_part1 ;
304 poly_matrix.block(0,3,3*via_num,3*via_num) = poly_matrix_part2 ;
305 poly_matrix.block(3*via_num,0,3,3*via_num+3) = poly_matrix_part3 ;
307 Eigen::MatrixXd poly_coeff(3*via_num+3,1);
309 poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector);
312 all_time_steps = round(mov_time/smp_time) ;
314 Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1);
316 for (
int i=1; i<=all_time_steps+1; i++)
317 time_vector.coeffRef(i-1,0) = (i-1)*smp_time;
319 Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1);
321 for (
int i=1; i<=via_num; i++)
322 via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2;
324 Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1);
326 for (
int i=1; i<=all_time_steps+1; i++)
328 minimum_jerk_tra_with_via_points.coeffRef(i-1,0) =
330 vel_start*time_vector.coeff(i-1,0) +
331 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) +
332 poly_coeff.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) +
333 poly_coeff.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) +
334 poly_coeff.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ;
337 for (
int i=1; i<=via_num; i++)
339 for (
int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++)
341 minimum_jerk_tra_with_via_points.coeffRef(j-1,0) =
342 minimum_jerk_tra_with_via_points.coeff(j-1,0) +
343 poly_coeff.coeff(3*i,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 +
344 poly_coeff.coeff(3*i+1,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 +
345 poly_coeff.coeff(3*i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ;
350 return minimum_jerk_tra_with_via_points;
354 double pos_start,
double vel_start,
double accel_start,
355 Eigen::MatrixXd pos_via,
356 double pos_end,
double vel_end,
double accel_end,
357 double smp_time, Eigen::MatrixXd via_time,
double mov_time)
381 Eigen::MatrixXd poly_vector = Eigen::MatrixXd::Zero(via_num+3,1);
383 for (i=1; i<=via_num; i++){
384 poly_vector.coeffRef(i-1,0) =
385 pos_via.coeff(i-1,0) -
387 vel_start*via_time.coeff(i-1,0) -
388 (accel_start/2)*pow(via_time.coeff(i-1,0),2) ;
391 poly_vector.coeffRef(via_num,0) =
395 (accel_start/2)*pow(mov_time,2) ;
397 poly_vector.coeffRef(via_num+1,0) =
400 accel_start*mov_time ;
402 poly_vector.coeffRef(via_num+2,0) =
408 Eigen::MatrixXd poly_part1 = Eigen::MatrixXd::Zero(via_num, 3);
410 for (i = 1; i <= via_num; i++) {
411 poly_part1.coeffRef(i-1,0) = pow(via_time.coeff(i-1,0),3);
412 poly_part1.coeffRef(i-1,1) = pow(via_time.coeff(i-1,0),4);
413 poly_part1.coeffRef(i-1,2) = pow(via_time.coeff(i-1,0),5);
416 Eigen::MatrixXd poly_part2 = Eigen::MatrixXd::Zero(via_num, via_num);
418 for (i = 1; i <= via_num; i++) {
419 for (j = 1; j <= via_num; j++) {
425 poly_part2.coeffRef(j-1, i-1) = pow(
426 (via_time.coeff(k-1,0) - via_time.coeff(i-1,0)),5)/120;
430 Eigen::MatrixXd poly_part3 = Eigen::MatrixXd::Zero(3,via_num+3);
432 poly_part3.coeffRef(0,0) = pow(mov_time,3);
433 poly_part3.coeffRef(0,1) = pow(mov_time,4);
434 poly_part3.coeffRef(0,2) = pow(mov_time,5);
436 poly_part3.coeffRef(1,0) = 3 * pow(mov_time,2);
437 poly_part3.coeffRef(1,1) = 4 * pow(mov_time,3);
438 poly_part3.coeffRef(1,2) = 5 * pow(mov_time,4);
440 poly_part3.coeffRef(2,0) = 6 * mov_time;
441 poly_part3.coeffRef(2,1) = 12 * pow(mov_time,2);
442 poly_part3.coeffRef(2,2) = 20 * pow(mov_time,3);
444 for (i = 1; i <= via_num; i++) {
445 poly_part3.coeffRef(0, i+2) = pow(mov_time - via_time.coeff(i-1,0),5)/120;
446 poly_part3.coeffRef(1, i+2) = pow(mov_time - via_time.coeff(i-1,0),4)/24;
447 poly_part3.coeffRef(2, i+2) = pow(mov_time - via_time.coeff(i-1,0),3)/6;
450 Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(via_num+3,via_num+3);
452 poly_matrix.block(0,0,via_num,3) = poly_part1;
453 poly_matrix.block(0,3,via_num,via_num) = poly_part2;
454 poly_matrix.block(via_num,0,3,via_num+3) = poly_part3;
458 Eigen::MatrixXd poly_inv_matrix(2*via_num+3,1);
460 poly_inv_matrix = poly_matrix.colPivHouseholderQr().solve(poly_vector);
465 all_time_steps = round(
int (mov_time/smp_time) ) ;
467 Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1);
469 for (i=1; i<=all_time_steps+1; i++)
470 time_vector.coeffRef(i-1,0) = (i-1)*smp_time;
473 Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1);
475 for (i=1; i<=via_num; i++)
476 via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2;
480 Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1);
482 for (i=1; i<=all_time_steps+1; i++)
484 minimum_jerk_tra_with_via_points.coeffRef(i-1,0) =
486 vel_start*time_vector.coeff(i-1,0) +
487 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) +
488 poly_inv_matrix.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) +
489 poly_inv_matrix.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) +
490 poly_inv_matrix.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ;
493 for (i=1; i<=via_num; i++)
495 for (j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++)
497 minimum_jerk_tra_with_via_points.coeffRef(j-1,0) =
498 minimum_jerk_tra_with_via_points.coeff(j-1,0) +
499 poly_inv_matrix.coeff(i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ;
503 return minimum_jerk_tra_with_via_points;
508 Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point,
509 double rotation_angle,
double cross_ratio )
511 int all_time_steps = int (round(mov_time/smp_time))+1;
514 rotation_angle, 0.0, 0.0,
517 Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps);
519 for (
int step = 0; step < all_time_steps; step++ )
521 double time = ((double)step)*smp_time;
522 double theta = angle_tra.coeff(step,0);
524 Eigen::MatrixXd weight_wedge(3,3);
527 0.0, -normal_vector.coeff(2,0), normal_vector.coeff(1,0),
528 normal_vector.coeff(2,0), 0.0, -normal_vector.coeff(0,0),
529 -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0;
531 Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3);
532 Eigen::MatrixXd rotation = identity + weight_wedge*sin(theta) + weight_wedge*weight_wedge*(1-cos(theta));
533 double cross = cross_ratio*(1.0-2.0*(abs(0.5-(time/mov_time))));
535 arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point;
538 Eigen::MatrixXd act_tra_trans = arc_tra.transpose();
540 return act_tra_trans;
Eigen::MatrixXd calcMinimumJerkTraPlus(double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, double rotation_angle, double cross_ratio)
Eigen::MatrixXd calcMinimumJerkTraWithViaPointsPosition(int via_num, double pos_start, double vel_start, double accel_start, Eigen::MatrixXd pos_via, double pos_end, double vel_end, double accel_end, double smp_time, Eigen::MatrixXd via_time, double mov_time)
Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, double pos_start, double vel_start, double accel_start, Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, double pos_end, double vel_end, double accel_end, double smp_time, Eigen::MatrixXd via_time, double mov_time)