fix minjerk interpolation sometimes returns large acceleration

[k-okada]

closes https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457

• add test to cehck https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457
• run graph-view if the function is defined

1) you need to set velocity and acceleration to run interpolation, specially if your input data is something like #f(0) #f(1) #f(2) #f(3), if your data is #f(0) #f(1) #f(0) #f(1), veocity of all sample point is #f(0), so default velocity/acceleration data works.

↓ without velocity and acceleration data

↓ with velocity and acceleration data

2) fixed the case when your time-list is not algined with dt,

(draw-grph 0.001), (draw-grph 0.0013), (draw-grph 0.005) of https://gist.github.com/k-okada/c575606258a822971e3dc6dbe3eb337e is as follows.

note: @708yamaguchi

• I think it is better to use [deg] and [msesc], to avoid the precision problem
• the graph still have large acceleration, and I'm not confident with this result, please check with your robot
• I suppose you create angle-vector-sequence with MoveIt! and then use minjerk interpolator to create the data, but did you try create interpolated data from normal euslisp style input like (send *ri* :angle-vector (send *robot* :reset-pose))?. I think we should test this case as a first try, because it is a much simpler case.

c.f. https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/a228cc7d3bb5b26b92ed25c7d55f32a64d9d7825/pr2eus/robot-interface.l#L567-L576, https://github.com/RethinkRobotics/baxter_interface/pull/72

[k-okada]

@708yamaguchi, please compare (draw-graph 0.0005) and (draw-graph 0.0015). I think both outputs a negative acceleration value at the beginning (t = 0.00150 with acceleration -125000.0) , In case of dt = 0.0015 it just skips large positive value when t is 0.0005 and 0.00100. So I think this is the limitation of the interpolation method, you need to small dt against input data time series.

0.00000   0.000           0.0           0.0
0.00050   0.352        1453.1     1375000.0
0.00100   1.000        1000.0      500000.0
0.00150   1.508         984.4     -125000.0
0.00200   2.000        1000.0           0.0
0.00250   2.500        1000.0           0.0
0.00300   3.000        1000.0          -0.0
0.00350   3.500        1000.0           0.0
0.00400   4.000        1000.0          -0.0
0.00450   4.500        1000.0           0.0
0.00500   5.000        1000.0          -0.0
0.00550   5.500        1000.0          -0.0
0.00600   6.000        1000.0           0.0
0.00650   6.500        1000.0          -0.0
0.00700   7.000        1000.0           0.0
0.00750   7.492         984.4      125000.0
0.00800   8.000        1000.0     -500000.0
0.00850   8.648        1453.1    -1375000.0
0.00900   9.000          -0.0           0.0

0.00000   0.000           0.0           0.0
0.00150   1.508         984.4     -125000.0
0.00300   3.000        1000.0           0.0
0.00450   4.500        1000.0          -0.0
0.00600   6.000        1000.0           0.0
0.00750   7.492         984.4      125000.0
0.00900   9.000          -0.0           0.0

[708yamaguchi]

@k-okada

Thank your very much for your debugging!

I understand that we need to set default velocity/acceleration data. I also understand that some data is skipped when we set big dt. I checked that this PR improves minjerk interpolation performance.

With your commits, I can move kinova arm with/without MoveIt!