Ruckig smoothing for longer trajectories

christophfroehlich commented 10 months ago

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 commented 10 months ago

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

AndyZe commented 10 months ago

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 commented 10 months ago

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[bot] commented 8 months ago

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 commented 7 months ago

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?

moveit / moveit2