Ruckig smoothing for longer trajectories

[christophfroehlich]

Description

I tried to use TOTG + ruckig smoother in a cpp project, where I want them to generate a smooth trajectory for a given path (generated without moveit). But it only changes one point, the other waypoints remain the same.

Maybe I understand something wrong with the API, do I have to iterate over every waypoint manually?

Your environment

• ROS Distro: Rolling
• OS Version: Ubuntu 22.04
• Source, main branch

Steps to reproduce

• Create and populate a robot model and robot state class
• Create a robot_trajectory from the robot state and add waypoints (position only)
• Run TOTG on the trajectory with TimeOptimalTrajectoryGeneration::computeTimeStamps
• Resulting trajectory is feasible with given joint limits (velocity + acceleration)
• Run ruckig smoother on the same trajectory-variable with RuckigSmoothing::applySmoothing
• Analyze the waypoints from the trajectory

For demonstration, I created a new test without TOTG in a fork of this repo, which describes my problem quite well.

Expected behaviour

A new trajectory parameterization considering also the jerk limits, i.e., a "streched" version of the result of the input trajectory.

Actual behaviour

Only the last point is changed, all other points remain the same.

The output from the test I linked above:

11: Original trajectory:
11: t: 0.1, p: 0
11: t: 0.2, p: -0.05
11: t: 0.3, p: 0
11: t: 0.4, p: -0.05
11: t: 0.5, p: 0
11: t: 0.6, p: -0.05
11: t: 0.7, p: 0
11: t: 0.8, p: -0.05
11: t: 0.9, p: 0
11: t: 1, p: -0.05
11: t: 1.1, p: 0
11: After ruckig smoothing:
11: t: 0.1, p: 0
11: t: 0.2, p: -0.05
11: t: 0.3, p: 0
11: t: 0.4, p: -0.05
11: t: 0.5, p: 0
11: t: 0.6, p: -0.05
11: t: 0.7, p: 0
11: t: 0.8, p: -0.05
11: t: 0.9, p: 0
11: t: 1, p: -0.05
11: t: 1.15177, p: 0

Setting mitigate_overshoot does not change this (only the time of the last point).

Same for giving explicit vel_limits, accel_limits, jerk_limits (considering the warning of the first test)

11: [ RUN ] RuckigTests.basic_trajectory
11: [INFO] [1701113459.795717082] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
11: [WARN] [1701113459.806200614] [moveit_trajectory_processing.ruckig_traj_smoothing]: Joint acceleration limits are not defined. Using the default 10 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
11: [WARN] [1701113459.806210176] [moveit_trajectory_processing.ruckig_traj_smoothing]: Joint jerk limits are not defined. Using the default 1000 rad/s^3. You can define jerk limits in joint_limits.yaml.

[christophfroehlich]

👀 @henningkayser this is the problem I tried to explain at ROSConDE last week. Thanks in advance for having a look!

[AndyZe]

I'll just mention, there were some updates to the Ruckig smoother in MoveIt1 that haven't been ported to MoveIt2 yet. I don't know if they would help here, though.

[marioprats]

I wonder if maybe those input waypoints can be reached with the given limits (related to your warning).
What happens if you manually set limits to something lower, or reduce time within waypoints / increase distance in such a way jerk needs to be limited?

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[christophfroehlich]

Sorry, it took me some time to get back for further discussion

I adapted the provided example from above and print now also JointGroupVelocities and Accelerations. They are zero, maybe this is why ruckig does not work?! But why should it change the last point then?

1: Original trajectory:
1: t: 0.1, p: 0, v: 0, a: 0
1: t: 0.2, p: -0.05, v: 0, a: 0
1: t: 0.3, p: 0, v: 0, a: 0
1: t: 0.4, p: -0.05, v: 0, a: 0
1: t: 0.5, p: 0, v: 0, a: 0
1: t: 0.6, p: -0.05, v: 0, a: 0
1: t: 0.7, p: 0, v: 0, a: 0
1: t: 0.8, p: -0.05, v: 0, a: 0
1: t: 0.9, p: 0, v: 0, a: 0
1: t: 1, p: -0.05, v: 0, a: 0
1: t: 1.1, p: 0, v: 0, a: 0
1: [WARN] [1708692045.849966211] [moveit_3996409459.ruckig_traj_smoothing]: Joint acceleration limits are not defined. Using the default 10 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
1: [WARN] [1708692045.849977278] [moveit_3996409459.ruckig_traj_smoothing]: Joint jerk limits are not defined. Using the default 1000 rad/s^3. You can define jerk limits in joint_limits.yaml.
1: After ruckig smoothing:
1: t: 0.1, p: 0, v: 0, a: 0
1: t: 0.2, p: -0.05, v: 0, a: 0
1: t: 0.3, p: 0, v: 0, a: 0
1: t: 0.4, p: -0.05, v: 0, a: 0
1: t: 0.5, p: 0, v: 0, a: 0
1: t: 0.6, p: -0.05, v: 0, a: 0
1: t: 0.7, p: 0, v: 0, a: 0
1: t: 0.8, p: -0.05, v: 0, a: 0
1: t: 0.9, p: 0, v: 0, a: 0
1: t: 1, p: -0.05, v: 0, a: 0
1: t: 1.15177, p: 0, v: 0, a: 0

Then I added another test with TOTG parameterization and ruckig smoothing afterwards (that what I actually want to do in my project):

TimeOptimalTrajectoryGeneration totg;
trajectory_processing::RuckigSmoothing smoother_;
totg.computeTimeStamps(*trajectory_, vel_limits, accel_limits));
smoother_.applySmoothing(*trajectory_, vel_limits, accel_limits, jerk_limits);

I set the jerk_limits of panda_joint1 to 1.0, but I get jerks over 20rad/s^3 (I calculated them by a simple difference quotient)
double jerk = i > 0 ? (joint_accelerations.at(JOINT_IDX) - old_acc) / trajectory_->getWayPointDurationFromPrevious(i) : 0.0;
Only two points got changed, one at 0.9s and the very last point (like in the test above).

What am I doing wrong?

[buckleytoby]

any updates to this issue?