totg timing algorithm sometimes gives invalid accelerations

[Dale-Koenig]

Description

Totg sometimes gives acceleration values on the order of 10^10 or higher despite acceleration joint limits on the order of 1 to 10

Your environment

• ROS Distro: Melodic
• OS Version: e.g. Ubuntu 18.04
• Built from source using master, rebuilt occasionally. The most recent commit on the local branch is 8fcff1d from the end of August

Steps to reproduce

1. See test posted below

Expected behaviour

Accelerations within joint limits

Actual behaviour

One waypoint (usually around the 5th or 6th) will have accelerations on the order of +- 10^11
The acceleration values are normal before the call to totg, and there are no (related) problems with iptp or itsp

Backtrace or Console output

The offending part of the trajectory

[henningkayser]

@Dale-Koenig thanks or reporting this. This is indeed strange behavior but I don't see any way how this could be related to the robot hardware. Can you provide a full example (trajectory with populated robot state and velocity/acceleration factors) that allows us to reproduce this issue? I think that some configuration must be different when you launch the real robot, i.e. joint limits. I guess it's worth looking at the robot model. TOTG only reads the robot trajectory and acceleration/velocity factors which depend on setup and planner, not on the used hardware or controller.

[Dale-Koenig]

@henningkayser
Here is a full trajectory for which it happens, although I think it may be hard to reproduce. It happens very frequently on the real robot though, once every few times.
https://gist.github.com/Dale-Koenig/f68ecc71e12465ee1e0a65558674c702

The code run on this trajectory is essentially:

robot_trajectory::RobotTrajectoryPtr robot_traj(new robot_trajectory::RobotTrajectory(robot_model_, joint_model_group_));
 // Clear the timestamps
 ros::Duration time_from_start(1);
 for (auto& point : trajectory.joint_trajectory.points)
 {
   point.time_from_start = time_from_start;
   time_from_start += ros::Duration(1);
 }
 robot_traj->setRobotTrajectoryMsg(start_robot_state, trajectory);

totg_trajectory_timing_algorithm_.computeTimeStamps(*trajectory, velocity_scaling_factor,
                                                              acceleration_scaling_factor);

After which we convert back to a message and immediately validate the accelerations and find that a couple of the times have ridiculous values like mentioned above. I'm pretty sure I printed out the start robot state's position/velocity/acceleration values at some point before and they matched up with the first trajectory state. I'm not sure how much other aspects of the robot model/start robot state affect totg though.

[Dale-Koenig]

@henningkayser I made a test where totg generates nan values for one of the accelerations

https://github.com/Dale-Koenig/moveit/tree/test_totg

path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, *path_iterator2));

The nan values seem to be caused by the fact that this line can create length 0 line segments, and calculating the tangent of a length 0 line segment has division by 0. It's not completely clear this is the same as the original problem though, because as far as I can tell this can only happen at the very end of the path.

[JafarAbdi]

The reason why it's giving nan is that you added the last waypoint 3 times in 1, 2, and 3

This's a simpler test that reproduces the error link

When constructing Path it assumes that we don't pass two consecutive identical waypoints, the TimeOptimalTrajectoryGeneration handles this situation See

Regarding the acceleration values, these two issues could be related 1 and 2.

[Dale-Koenig]

I see, I suppose I added a couple extra waypoints when making the test. I can, however testify that creating a Path with the exact same function arguments (without the duplicated waypoints) is what gave the large accelerations when running on the real robot. It doesn't seem to cause the same problem when running the test without the real robot, so somehow there is a strange hardware/similar dependence without the totg code itself.

…

On Sun, Oct 20, 2019 at 3:21 AM Jafar Abdi ***@***.***> wrote: The reason why it's giving nan is that you added the last waypoint 3 times in 1 <https://github.com/Dale-Koenig/moveit/blob/9889c3519508228985a9656bbbbf17efcbc0985f/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp#L248>, 2 <https://github.com/Dale-Koenig/moveit/blob/test_totg/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp#L246>, and 3 <https://github.com/Dale-Koenig/moveit/blob/test_totg/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp#L244> This's a simpler test that reproduces the error link <https://gist.github.com/JafarAbdi/aed3dc2766a18f22ad9228eeefbad37f> The TimeOptimalTrajectoryGeneration handles this situation See <https://github.com/ros-planning/moveit/blob/master/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp#L952> Regarding the acceleration values, these two issues could be related 1 <tobiaskunz/trajectories#1> and 2 <tobiaskunz/trajectories#2>. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#1665>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AMAEPFVARWHEZD6JDSWZBO3QPNFZ3ANCNFSM4IV7UL2Q> .

-- Dale Koenig Robotics Software Engineer Rapyuta Robotics "empowering lives with connected machines" www.rapyuta-robotics.com

[Dale-Koenig]

Anyway I will continue to investigate / put tons of debug messages while running with the real robot. On Sun, Oct 20, 2019 at 11:25 PM Dale Koenig < dale.koenig@rapyuta-robotics.com> wrote:

…

I see, I suppose I added a couple extra waypoints when making the test. I can, however testify that creating a Path with the exact same function arguments (without the duplicated waypoints) is what gave the large accelerations when running on the real robot. It doesn't seem to cause the same problem when running the test without the real robot, so somehow there is a strange hardware/similar dependence without the totg code itself. On Sun, Oct 20, 2019 at 3:21 AM Jafar Abdi ***@***.***> wrote: > The reason why it's giving nan is that you added the last waypoint 3 > times in 1 > <https://github.com/Dale-Koenig/moveit/blob/9889c3519508228985a9656bbbbf17efcbc0985f/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp#L248>, > 2 > <https://github.com/Dale-Koenig/moveit/blob/test_totg/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp#L246>, > and 3 > <https://github.com/Dale-Koenig/moveit/blob/test_totg/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp#L244> > > This's a simpler test that reproduces the error link > <https://gist.github.com/JafarAbdi/aed3dc2766a18f22ad9228eeefbad37f> > > The TimeOptimalTrajectoryGeneration handles this situation See > <https://github.com/ros-planning/moveit/blob/master/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp#L952> > > Regarding the acceleration values, these two issues could be related 1 > <tobiaskunz/trajectories#1> and 2 > <tobiaskunz/trajectories#2>. > > — > You are receiving this because you were mentioned. > Reply to this email directly, view it on GitHub > <#1665>, > or unsubscribe > <https://github.com/notifications/unsubscribe-auth/AMAEPFVARWHEZD6JDSWZBO3QPNFZ3ANCNFSM4IV7UL2Q> > . > -- Dale Koenig Robotics Software Engineer Rapyuta Robotics "empowering lives with connected machines" www.rapyuta-robotics.com

-- Dale Koenig Robotics Software Engineer Rapyuta Robotics "empowering lives with connected machines" www.rapyuta-robotics.com

[Dale-Koenig]

https://github.com/Dale-Koenig/moveit/tree/test_totg

Test updated. It now gives very large (non nan) accelerations on my machine.

[JafarAbdi]

This test is the same test provided by @Dale-Koenig but with commented waypoints which don't affect the test failing (having big acceleration)

Some observations

• the position of TrajectoryStep 300 and 301 are near each other and when calculating the time this causes the difference to be very small
• For this test, the time_step in general is 0.001 but at the failing time it->time_ - previous->time_ is very small which cause the acceleration to be big number (which is a consequence of the previous point)
• The big acceleration bug only happens when having high precision waypoints, I tried to round the doubles to 16 decimal points (the provided waypoints have 17 decimal point precision) and the test passes.

I'll try to add UB sanitizer to the test and see if there's UB when having high precision waypoints

[JafarAbdi]

@Dale-Koenig After diving again in the test you provided the bug happens inside integrateForward function, when the path_pos of a TrajectoryStep is very close to a switching point (for example for the reduced test it's about 1.6098233857064769836e-15) the next path_pos will be larger than the pos of the switching point which will make this condition to be true and will cause two TrajectoryStep that almost have identical path_pos to be added to the trajectory_ what I did is adding a check for this case and if it happens continue without adding it

if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first)
{
  if (path_pos - next_discontinuity->first < 10e-8)
  {
    continue;
  }
  path_vel = old_path_vel +
             (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos);
  path_pos = next_discontinuity->first;
}

The paper has a section which discusses the numerical aspect of the algorithm but seems like it didn't include this case -- see section VIII