add minjerk interpolation method #72

k-okada · 2016-07-16T07:00:54Z

currently baxter's joint trajectory action uses Bezier interpolation function and that would jerky motion for some inputs, for example, if we inputs https://gist.github.com/k-okada/b17850cde70f426aa84e0b8be98ed893 files, we'll get

The problem is second peak, where time=5, the input is

angle  time_from_start
1.0      2.0
0.0      3.0
1.0      4.0
1.1      4.1
0.0      5.0

when when the program generate interpolated trajectory from (angle, time) = (1.0, 4.0) to (1.1, 4.1)` it will generate too peaky outputs, We at JSK, have been using "Minimum Jerk" trajectory generation a.k.a "Hoff Arbib" for many years, which is basically to generate trajectory which minimize derivative of acceleration and ensure the continuousness of position, velocity and acceleration. See http://www.shadmehrlab.org/book/minimumjerk.pdf for background.

Wen we use min-jerk, we can get figure below for same input

I know this is huge change and need more intensive testing, but hope this PR would be good starting point for discussions

Cc: @pazeshun

Release 1.2.0

k-okada · 2016-07-16T07:17:53Z

Another concern about output of Bezier function is it overshoot, for example , the example code outputs Trapezoid trajectory, but upper part is straight in minjerk and overshoot in the bezier function. For example
if we consider 1 joint arm and start with angle 0 and you can reach to some position at the angle 90, we'll send command 0, and 90, after we control gripper, then we'll send backward motion from 90 to 0, in that case, current interpolation function outputs angle > 90, if the target is object on the table of button on the wall, it means the robot gripper is trying to penetrate to the obstacles, and it is not what we expected.

IanTheEngineer · 2016-07-18T17:48:59Z

@k-okada Wow! Really interesting work that you've done and contributed. I want to find a way to incorporate these changes, but agree they will require substantial testing. Have you run this modified action server on your Baxter? As always, your contributions are incredibly appreciated.

k-okada · 2016-07-19T10:03:59Z

Yes, I have just tested on our Baxter robot and here is the results; (I have also used #73 as well)
I have added -i switch
https://github.com/RethinkRobotics/baxter_interface/pull/72/files#diff-d1acf285ad84d35b2efa98b2c88a19bbR109
, so new interpolation method is not activated unless you run rosrun baxter_interface joint_trajectory_action_server.py -i minjerk

Bezier function;

MinJerk function

I have noticed that current Bezier interpolation moves the arm if we send same goal positions for several times, see the first three goal message.

JTA message for this example;
https://gist.github.com/k-okada/f04885bc9403897621dd39e95b01228f

roseus program to genrate JTA message

(load "package://baxtereus/baxter-interface.l")
(baxter-init)

(defun straight-motion2 ()
  (let ((pos0 #f(900 -500 350))
        (pos1 #f(900    0 350))
        (i 0) avs)
    (while (<= i pi)
      (print (list (sin i) (make-coords :pos (midpoint (sin i) pos0 pos1))))
      (send *baxter* :rarm :inverse-kinematics (make-coords :pos (midpoint (sin\
 i) pos0 pos1)))
      (push (send *baxter* :angle-vector) avs)
      (incf i 0.1))
    (setq avs (nreverse avs))
    (dotimes (i 3)
      (send *ri* :angle-vector (car avs) 3000)
      (send *ri* :wait-interpolation)
      (unix:sleep 1))
    (send *ri* :angle-vector-sequence avs 300)
    (send *ri* :wait-interpolation)

    (send *ri* :angle-vector (send *baxter* :rarm :inverse-kinematics (make-coords :pos pos0)) 5000)
    (send *ri* :wait-interpolation)
    (send *ri* :angle-vector (send *baxter* :rarm :inverse-kinematics (make-coords :pos pos1)) 5000)
    (send *ri* :wait-interpolation)
    (send *ri* :angle-vector (send *baxter* :rarm :inverse-kinematics (make-coords :pos pos0)) 5000)
    (send *ri* :wait-interpolation)
    (unix:sleep 3)

    (send *ri* :angle-vector-sequence
          (list (send *baxter* :rarm :inverse-kinematics (make-coords :pos pos0))
                (send *baxter* :rarm :inverse-kinematics (make-coords :pos pos1))
                (send *baxter* :rarm :inverse-kinematics (make-coords :pos pos0)))
          (list 5000 5000 5000))
    (send *ri* :wait-interpolation)
    ))

k-okada · 2016-07-19T10:46:57Z

another examples -> jsk-ros-pkg/jsk_robot#635

IanTheEngineer · 2016-07-22T15:41:43Z

src/joint_trajectory_action/joint_trajectory_action.py

+            else:
+                b_matrix = self._compute_bezier_coeff(joint_names,
+                                                      trajectory_points,
+                                                      dimensions_dict)
        except Exception as ex:
            rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"


We should either remove the Bezier adjective or substitute the current interpolation method in there.

k-okada · 2016-07-22T16:06:59Z

thanks for comment, fixed and updated the PR.

IanTheEngineer · 2016-07-22T16:20:00Z

@k-okada, your work here is fantastic. Based on your plots and description of the issues experienced by using the bezier trajectories, minjerk might be the better default interpolation in the future. For now, it's fine to leave bezier default. Can you target this PR at the development branch as well?

k-okada · 2016-07-23T09:14:59Z

ok, rebased against development branch. Just for note, comparison between
bezier and minjerk is not fair, I have been using minjerk for many years,
so I know how to configure that (for example
https://github.com/RethinkRobotics/baxter_interface/pull/72/files#diff-fc2fc667f0344438888d1adafa3170e6R125),
and I just use bezier for default settings, so it may have chance to work
well if you propery configure control point of the bezier curve, it may
work well. But I myself has no knowledge on that.

◉ Kei Okada

On Sat, Jul 23, 2016 at 1:20 AM, Ian McMahon [email protected]
wrote:

@k-okada https://github.com/k-okada, your work here is fantastic. Based
on your plots and description of the issues experienced by using the
bezier trajectories, minjerk might be the better default interpolation in
the future. For now, it's fine to leave bezier default. Can you target
this PR at the development branch as well?

—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
#72 (comment),
or mute the thread
https://github.com/notifications/unsubscribe-auth/AAeG3GOtGvvethurmwaYC4tJ--bFtfBRks5qYO2wgaJpZM4JN8pP
.

wkentaro · 2016-09-23T10:07:53Z

What is the status of this PR?

@k-okada

From RethinkRobotics/baxter_interface#72 @k-okada currently baxter's joint trajectory action uses Bezier interpolation function and that would jerky motion f ![screenshot from 2016-07-16 16 08 39](https://cloud.githubusercontent.com/assets/493276/16893337/a2ad667e- The problem is second peak, where time=5, the input is ``` angle time_from_start 1.0 2.0 0.0 3.0 1.0 4.0 1.1 4.1 0.0 5.0 ``` when when the program generate interpolated trajectory from `(angle, time) = (1.0, 4.0)` to (1.1, 4.1)` it Wen we use min-jerk, we can get figure below for same input ![screenshot from 2016-07-16 16 05 44](https://cloud.githubusercontent.com/assets/493276/16893336/a02ae520- I know this is huge change and need more intensive testing, but hope this PR would be good starting point Cc: @pazeshun

pazeshun · 2016-12-06T07:20:37Z

@k-okada Please target development branch

k-okada · 2016-12-06T07:30:14Z

I think this is already targeting development branch

pazeshun · 2016-12-06T07:40:19Z

I mean base branch of this PR is master

k-okada · 2016-12-06T07:44:01Z

I see. See #78,

Merge pull request RethinkRobotics#71 from RethinkRobotics/release-1.2.0

bc76d5e

Release 1.2.0

k-okada force-pushed the minjerk branch 2 times, most recently from 39a9cf0 to e83d119 Compare July 19, 2016 09:52

k-okada mentioned this pull request Jul 19, 2016

use (unreleasead) minjerk JTA start-jsk/jsk_apc#1854

Closed

IanTheEngineer reviewed Jul 22, 2016
View reviewed changes

k-okada force-pushed the minjerk branch 2 times, most recently from f05f433 to 6abbebd Compare July 22, 2016 16:06

add minjerk interpolation method

d5c9988

k-okada force-pushed the minjerk branch from 6abbebd to d5c9988 Compare July 23, 2016 08:50

wkentaro mentioned this pull request Sep 16, 2016

[baxtereus] Way to use baxtereus from source with current HEAD version jsk-ros-pkg/jsk_robot#659

Open

k-okada mentioned this pull request Dec 6, 2016

add minjerk interpolation method #78

Merged

k-okada mentioned this pull request Jan 21, 2021

fix minjerk interpolation sometimes returns large acceleration euslisp/jskeus#596

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

add minjerk interpolation method #72

add minjerk interpolation method #72

k-okada commented Jul 16, 2016 •

edited

Loading

k-okada commented Jul 16, 2016

IanTheEngineer commented Jul 18, 2016

k-okada commented Jul 19, 2016

k-okada commented Jul 19, 2016

IanTheEngineer Jul 22, 2016

k-okada commented Jul 22, 2016

IanTheEngineer commented Jul 22, 2016

k-okada commented Jul 23, 2016

wkentaro commented Sep 23, 2016 •

edited

Loading

pazeshun commented Dec 6, 2016

k-okada commented Dec 6, 2016

pazeshun commented Dec 6, 2016

k-okada commented Dec 6, 2016

add minjerk interpolation method #72

Are you sure you want to change the base?

add minjerk interpolation method #72

Conversation

k-okada commented Jul 16, 2016 • edited Loading

k-okada commented Jul 16, 2016

IanTheEngineer commented Jul 18, 2016

k-okada commented Jul 19, 2016

k-okada commented Jul 19, 2016

IanTheEngineer Jul 22, 2016

Choose a reason for hiding this comment

k-okada commented Jul 22, 2016

IanTheEngineer commented Jul 22, 2016

k-okada commented Jul 23, 2016

wkentaro commented Sep 23, 2016 • edited Loading

pazeshun commented Dec 6, 2016

k-okada commented Dec 6, 2016

pazeshun commented Dec 6, 2016

k-okada commented Dec 6, 2016

k-okada commented Jul 16, 2016 •

edited

Loading

wkentaro commented Sep 23, 2016 •

edited

Loading