TimeOptimalTrajectoryGeneration first point velocity / acceleration change between Humble and Jazzy

[mrjogo]

Description

I'm creating a UR5e trajectory by:

1. Defining an approach and a retreat vector of Eigen::Isometry3d waypoints
2. Passing each through CartesianInterpolator::computeCartesianPath
3. Parameterizing each with TimeOptimalTrajectoryGeneration::computeTimeStamps
4. Appending them together with a pause in between

I just upgraded from Humble to Jazzy, and the formerly functional code started creating an unexpected motion. I narrowed the problem down to a change in computeTimeStamps (see https://github.com/ros-controls/ros2_controllers/issues/1298 for more information on the observed motion).

In Humble, computeTimeStamps assigns 0 velocity / acceleration to the first point, while in Jazzy it assigns non-zero values. Therefore, when my code appends the two trajectories together with a pause, it ends up with sequential points that are the pause length apart with identical positions, but the first has 0 velocity / acceleration and the second does not.

The (slightly cleaned up) code for steps 3 & 4 is:

  trajectory_processing::TimeOptimalTrajectoryGeneration totg;
  totg.computeTimeStamps(*robot_trajectory, 1.0, 1.0);
  totg.computeTimeStamps(retreat_trajectory, 1.0, 1.0);

  // Hack to pause trajectories at final point because TOTG gives non-zero accelerations
  const std::vector<int>& idx = robot_trajectory->getGroup()->getVariableIndexList();
  for (size_t i = 0; i < idx.size(); i++)
  {
    robot_trajectory->getLastWayPointPtr()->setVariableAcceleration(idx[i], 0);
    retreat_trajectory.getLastWayPointPtr()->setVariableAcceleration(idx[i], 0);
  }

  robot_trajectory->append(retreat_trajectory, 3.5);

As you can see I already added a hack because in Humble the last waypoint had non-zero velocities and accelerations (I have not verified if that is still true in Jazzy). I could add the same hack for the first waypoint, but this seems like incorrect (or at best undefined) behavior.

I also suspect the problem is usually masked because normally 1) it's the first point in a trajectory so the robot is starting from rest anyway, and 2) the next point is only a few milliseconds later, so the controller blends them together into a qualitatively correct motion.

ROS Distro

Jazzy

OS and version

Ubuntu 24.04

Source or binary build?

Binary

If binary, which release version?

2.10.0

If source, which branch?

No response

Which RMW are you using?

FastRTPS

Steps to Reproduce

1. Generate a trajectory with CartesianInterpolator::computeCartesianPath
2. Parameterize it with TimeOptimalTrajectoryGeneration::computeTimeStamps

Expected behavior

The first (and last?) points should have 0 velocity and acceleration? I'm not sure which one is actually "correct", but I couldn't find any Pull Requests that address the change.

Actual behavior

The first (and last?) points have nonzero velocity and acceleration.

Backtrace or Console output

No response