HKUST PhD Chronicle, Week 13, Taming the Jerk

This week was a blast. I finally managed to implement an algorithm which takes a list of waypoints( $q_1, q_2, q_3,q_4,q_5,q_6,q_7$ ) as input, and outputs a time-parameterized trajectory that respects velocity, acceleration, and jerk limit.

Hats off to the great open sourced project MoveIt2 which provided the majority of my reference material.

Diagram helps me out…

The first attempt involved taking the results directly from cuRobo: CUDA Accelerated Robot Library and use pantor/ruckig: Motion Generation for Robots and Machines. Real-time. Jerk-constrained. Time-optimal. to generate the trajectory at 1KHz frequency (the control rate of Franka Research 3). However, the result looks very ugly.

The motion turns out to be very jerky. I even tried feeding the waypoints to ruckig using its Cloud API mode in Python and then playback the trajectory on Franka.

Sadly, the generated trajectory was still very stuttering… It turns out that ruckig, while powerful, is not designed for naive waypoint interpolation when the constraints are tight.

With this result, my heart skipped a beat. There was no way it should be this difficult!

OSS rocks!

Then I started to ponder: How do others solve this problem? I believe this is not rocket science, especially since so many research institutes use the Franka Research 3… The only difference is that I was not using ROS…

I decided to dig into the MoveIt2 source code, and luckily, I found exactly what I needed: the logic to generate time-parameterized trajectories with velocity, acceleration, and jerk constraints.

Looking at the ompl_planning.yaml, we can see that the trajectory is generated via a pipeline of adapters.

request_adapters: >-
  default_planner_request_adapters/AddRuckigTrajectorySmoothing
  default_planner_request_adapters/AddTimeOptimalParameterization
  ...

The key insight is that ruckig is used as a smoother, not a generator. In moveit2, the waypoints are first processed via an algorithm called TOTG. This generates a trajectory that complies with velocity and acceleration constraint but not yet support jerk limits. This is problematic, as high jerk values can cause the robot to shudder and eventually damage the hardware.

Moveit2 employs a brilliant design using ruckig to smooth this trajectory to fit within jerk limit.

std::deque<double> duration;
std::deque<WayPoints> waypoints;

The algorithm uses a std::deque to track the duration between each waypoint. It sets the $t_n$ waypoint as the current position and the $t_{n+1}$ waypoint as the target position. If ruckig returns a Result::Working state (instead of Result::Finished):

ruckig.update(input, output) == Result::Working;

It means the duration allocated between these 2 waypoints is not sufficient to reach the target within jerk limits. We need to extend it

However, simply extending the duration isn’t enough. Since the duration between waypoints corresponds to control cycle (e.g., 1ms for Franka Research 3), stretching the time requires us to re-evaluate where the robot should be. In moveit2, an interpolation is applied here to calculate the correct intermediate waypoint for the new, stretched duration.