robotis_trajectory_calculator.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /*
18  * robotis_trajectory_calculator.cpp
19  *
20  * Created on: June 7, 2016
21  * Author: SCH
22  */
23 
24 #include "robotis_math/robotis_trajectory_calculator.h"
25 
26 namespace robotis_framework
27 {
28 
29 Eigen::MatrixXd calcMinimumJerkTra(
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
33  )
34 /*
35  simple minimum jerk trajectory
36 
37  pos_start : position at initial state
38  vel_start : velocity at initial state
39  accel_start : acceleration at initial state
40 
41  pos_end : position at final state
42  vel_end : velocity at final state
43  accel_end : acceleration at final state
44 
45  smp_time : sampling time
46  mov_time : movement time
47  */
48 
49 {
50  Eigen::MatrixXd poly_matrix(3,3);
51  Eigen::MatrixXd poly_vector(3,1);
52 
53  poly_matrix <<
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);
57 
58  poly_vector <<
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 ;
62 
63  Eigen::Matrix<double,3,1> poly_coeff = poly_matrix.inverse() * poly_vector;
64 
65  double time_steps = mov_time/smp_time;
66  int all_time_steps = round(time_steps+1);
67 
68  Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1);
69  Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1);
70 
71  for (int step=0; step<all_time_steps; step++)
72  time.coeffRef(step,0) = step*smp_time;
73 
74  for (int step=0; step<all_time_steps; step++)
75  {
76  minimum_jerk_tra.coeffRef( step , 0 ) =
77  pos_start +
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);
83  }
84 
85  return minimum_jerk_tra;
86 }
87 
88 Eigen::MatrixXd calcMinimumJerkTraPlus(
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
92  )
93 /*
94  simple minimum jerk trajectory
95 
96  pos_start : position at initial state
97  vel_start : velocity at initial state
98  accel_start : acceleration at initial state
99 
100  pos_end : position at final state
101  vel_end : velocity at final state
102  accel_end : acceleration at final state
103 
104  smp_time : sampling time
105  mov_time : movement time
106  */
107 
108 {
109  Eigen::MatrixXd poly_matrix(3,3);
110  Eigen::MatrixXd poly_vector(3,1);
111 
112  poly_matrix <<
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);
116 
117  poly_vector <<
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 ;
121 
122  Eigen::Matrix<double,3,1> poly_coeff = poly_matrix.inverse() * poly_vector;
123 
124  double time_steps = mov_time/smp_time;
125  int all_time_steps = round(time_steps+1);
126 
127  Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1);
128  Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,3);
129 
130  for (int step=0; step<all_time_steps; step++)
131  time.coeffRef(step,0) = step*smp_time;
132 
133  for (int step=0; step<all_time_steps; step++)
134  {
135  minimum_jerk_tra.coeffRef( step , 0 ) =
136  pos_start +
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);
142 
143  minimum_jerk_tra.coeffRef( step , 1 ) =
144  vel_start +
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);
149 
150  minimum_jerk_tra.coeffRef( step , 2 ) =
151  accel_start +
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);
155  }
156 
157  return minimum_jerk_tra;
158 }
159 
160 Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num,
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 )
165 /*
166  minimum jerk trajectory with via-points
167  (via-point constraints: position and velocity at each point)
168 
169  via_num : the number of via-points
170 
171  pos_start : position at initial state
172  vel_start : velocity at initial state
173  accel_start : acceleration at initial state
174 
175  pos_via : position matrix at via-points state ( size : n x 1 )
176  vel_via : velocity matrix at via-points state ( size : n x 1 )
177  accel_via : acceleration matrix at via-points state ( size : n x 1 )
178 
179  pos_end : position at final state
180  vel_end : velocity at final state
181  accel_end : acceleration at final state
182 
183  smp_time : sampling time
184 
185  via_time : time matrix passing through via-points state ( size : n x 1 )
186  via_time : movement time
187  */
188 
189 {
190  // int i,j,k ;
191 
192  Eigen::MatrixXd poly_vector = Eigen::MatrixXd::Zero(3*via_num+3,1);
193 
194  for (int i=1; i<=via_num; i++)
195  {
196  poly_vector.coeffRef(3*i-3,0) =
197  pos_via.coeff(i-1,0) -
198  pos_start -
199  vel_start*via_time.coeff(i-1,0) -
200  (accel_start/2)*pow(via_time.coeff(i-1,0),2) ;
201 
202  poly_vector.coeffRef(3*i-2,0) =
203  vel_via.coeff(i-1,0) -
204  vel_start -
205  accel_start*via_time.coeff(i-1,0) ;
206 
207  poly_vector.coeffRef(3*i-1,0) =
208  accel_via.coeff(i-1,0) -
209  accel_start;
210  }
211 
212  poly_vector.coeffRef(3*via_num,0) =
213  pos_end -
214  pos_start -
215  vel_start*mov_time -
216  (accel_start/2)*pow(mov_time,2) ;
217 
218  poly_vector.coeffRef(3*via_num+1,0) =
219  vel_end -
220  vel_start -
221  accel_start*mov_time ;
222 
223  poly_vector.coeffRef(3*via_num+2,0) =
224  accel_end -
225  accel_start ;
226 
227  Eigen::MatrixXd poly_matrix_part1 = Eigen::MatrixXd::Zero(3*via_num,3);
228 
229  for (int i=1; i<=via_num; i++)
230  {
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) ;
234 
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) ;
238 
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) ;
242  }
243 
244  Eigen::MatrixXd poly_matrix_part2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num);
245 
246  for (int i=1; i<=via_num; i++)
247  {
248  for (int j=1; j<=via_num; j++)
249  {
250  int k;
251 
252  if (i > j)
253  k = i ;
254  else
255  k = j ;
256 
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 ;
260 
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 ;
264 
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 ;
268  }
269  }
270 
271 
272  Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3);
273 
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);
277 
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);
281 
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);
285 
286  for (int i=1; i<=via_num; i++)
287  {
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) ;
291 
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 ;
295 
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 ;
299  }
300 
301  Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3);
302 
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 ;
306 
307  Eigen::MatrixXd poly_coeff(3*via_num+3,1);
308  //C = A.inverse()*B;
309  poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector);
310 
311  int all_time_steps;
312  all_time_steps = round(mov_time/smp_time) ;
313 
314  Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1);
315 
316  for (int i=1; i<=all_time_steps+1; i++)
317  time_vector.coeffRef(i-1,0) = (i-1)*smp_time;
318 
319  Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1);
320 
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;
323 
324  Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1);
325 
326  for (int i=1; i<=all_time_steps+1; i++)
327  {
328  minimum_jerk_tra_with_via_points.coeffRef(i-1,0) =
329  pos_start +
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) ;
335  }
336 
337  for (int i=1; i<=via_num; i++)
338  {
339  for (int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++)
340  {
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 ;
346 
347  }
348  }
349 
350  return minimum_jerk_tra_with_via_points;
351 }
352 
353 Eigen::MatrixXd calcMinimumJerkTraWithViaPointsPosition(int via_num,
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)
358 
359 /* n is the number of via-points.
360 
361  x0, v0 and a0 are initial values of
362  joint angle, angular velocity and angular acceleration.
363 
364  x (Matirx) and dx (Matirx) are
365  joint angles and angular velocities at the via-points.
366 
367  xf, vf and af are final values of
368  joint angle, angular velocity and angular acceleration.
369 
370  smp is sampling time.
371 
372  t (Matirx) is the time values passing via-points.
373 
374  tf is movement time */
375 
376 {
377  int i,j,k ;
378 
379  /* Calculation Matrix B */
380 
381  Eigen::MatrixXd poly_vector = Eigen::MatrixXd::Zero(via_num+3,1);
382 
383  for (i=1; i<=via_num; i++){
384  poly_vector.coeffRef(i-1,0) =
385  pos_via.coeff(i-1,0) -
386  pos_start -
387  vel_start*via_time.coeff(i-1,0) -
388  (accel_start/2)*pow(via_time.coeff(i-1,0),2) ;
389  }
390 
391  poly_vector.coeffRef(via_num,0) =
392  pos_end -
393  pos_start -
394  vel_start*mov_time -
395  (accel_start/2)*pow(mov_time,2) ;
396 
397  poly_vector.coeffRef(via_num+1,0) =
398  vel_end -
399  vel_start -
400  accel_start*mov_time ;
401 
402  poly_vector.coeffRef(via_num+2,0) =
403  accel_end -
404  accel_start ;
405 
406  /* Calculation Matrix A */
407 
408  Eigen::MatrixXd poly_part1 = Eigen::MatrixXd::Zero(via_num, 3);
409 
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);
414  }
415 
416  Eigen::MatrixXd poly_part2 = Eigen::MatrixXd::Zero(via_num, via_num);
417 
418  for (i = 1; i <= via_num; i++) {
419  for (j = 1; j <= via_num; j++) {
420  if (i > j) {
421  k = i;
422  } else {
423  k = j;
424  }
425  poly_part2.coeffRef(j-1, i-1) = pow(
426  (via_time.coeff(k-1,0) - via_time.coeff(i-1,0)),5)/120;
427  }
428  }
429 
430  Eigen::MatrixXd poly_part3 = Eigen::MatrixXd::Zero(3,via_num+3);
431 
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);
435 
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);
439 
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);
443 
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;
448  }
449 
450  Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(via_num+3,via_num+3);
451 
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;
455 
456  /* Calculation Matrix C (coefficient of polynomial function) */
457 
458  Eigen::MatrixXd poly_inv_matrix(2*via_num+3,1);
459  //C = A.inverse() * B;
460  poly_inv_matrix = poly_matrix.colPivHouseholderQr().solve(poly_vector);
461 
462  /* Time */
463 
464  int all_time_steps;
465  all_time_steps = round( int (mov_time/smp_time) ) ;
466 
467  Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1);
468 
469  for (i=1; i<=all_time_steps+1; i++)
470  time_vector.coeffRef(i-1,0) = (i-1)*smp_time;
471 
472  /* Time_via */
473  Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1);
474 
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;
477 
478  /* Minimum Jerk Trajectory with Via-points */
479 
480  Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1);
481 
482  for (i=1; i<=all_time_steps+1; i++)
483  {
484  minimum_jerk_tra_with_via_points.coeffRef(i-1,0) =
485  pos_start +
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) ;
491  }
492 
493  for (i=1; i<=via_num; i++)
494  {
495  for (j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++)
496  {
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 ;
500  }
501  }
502 
503  return minimum_jerk_tra_with_via_points;
504 
505 }
506 
507 Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time,
508  Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point,
509  double rotation_angle, double cross_ratio )
510 {
511  int all_time_steps = int (round(mov_time/smp_time))+1;
512 
513  Eigen::MatrixXd angle_tra = calcMinimumJerkTra(0.0, 0.0, 0.0,
514  rotation_angle, 0.0, 0.0,
515  smp_time, mov_time);
516 
517  Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps);
518 
519  for (int step = 0; step < all_time_steps; step++ )
520  {
521  double time = ((double)step)*smp_time;
522  double theta = angle_tra.coeff(step,0);
523 
524  Eigen::MatrixXd weight_wedge(3,3);
525 
526  weight_wedge <<
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;
530 
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))));
534 
535  arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point;
536  }
537 
538  Eigen::MatrixXd act_tra_trans = arc_tra.transpose();
539 
540  return act_tra_trans;
541 }
542 
543 }
robotis_framework::calcMinimumJerkTraPlus
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)
Definition: robotis_trajectory_calculator.cpp:88
robotis_framework::calcMinimumJerkTra
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)
Definition: robotis_trajectory_calculator.cpp:29
robotis_trajectory_calculator.h
robotis_framework::calcArc3dTra
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)
Definition: robotis_trajectory_calculator.cpp:507
robotis_framework::calcMinimumJerkTraWithViaPointsPosition
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)
Definition: robotis_trajectory_calculator.cpp:353
robotis_framework::calcMinimumJerkTraWithViaPoints
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)
Definition: robotis_trajectory_calculator.cpp:160
robotis_framework
Definition: bezier_curve.h:32

robotis_math
Author(s): SCH , Kayman , Jay Song
autogenerated on Fri Jul 17 2020 03:17:50