Constant time step trajectories, sometimes have an insconsistent timestep

[tony-p]

Description

Using ompl with trajextory smoothing, we expect to have all waypoints having the same duration (ignoring first and last) and by default this is 0.1s (as is also mentioned in the docs). However every now and again we get a time step of 0.11s in the middle of a path. It is not consistently reproducible. Using 1 robot installation as an example, we saw this occur 3 times in a day.

ROS Distro

Jazzy

OS and version

24.04

Source or binary build?

Binary

If binary, which release version?

2.12.3 (robostack)

If source, which branch?

No response

Which RMW are you using?

CycloneDDS

Steps to Reproduce

Planner config

planning_plugins:
  - ompl_interface/OMPLPlanner
request_adapters:
  - default_planning_request_adapters/ResolveConstraintFrames
  - default_planning_request_adapters/CheckStartStateBounds
  - default_planning_request_adapters/CheckStartStateCollision
  - default_planning_request_adapters/ValidateWorkspaceBounds
response_adapters:
 - default_planning_response_adapters/AddTimeOptimalParameterization
 - default_planning_response_adapters/AddRuckigTrajectorySmoothing

Plan trajectories with move group

Expected behavior

(except first and last poiint) all waypoint durations should be consistent (and typically 0.1s)

Actual behavior

Every now and again we get a path with a time step in the middle of 0.11s

Trajectory points have varying durations (compared to: 0.100000): {0.000000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.110000 <<<<<
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.100000
0.064077}

Backtrace or Console output

1773364604.8672612 [moveit2_planner-6] [INFO] [1773364604.867008106] [moveit2_planner.moveit.ros.move_group_interface]: MoveGroup action client/server ready
1773364604.8675873 [move_group-4] [INFO] [1773364604.867411763] [move_group.moveit.moveit.ros.move_group.move_action]: MoveGroupMoveAction: Received request
1773364604.8677616 [moveit2_planner-6] [INFO] [1773364604.867614628] [moveit2_planner.moveit.ros.move_group_interface]: Planning request accepted
1773364604.8740001 [move_group-4] [INFO] [1773364604.873770935] [move_group.moveit.moveit.ros.move_group.move_action]: executing..
1773364604.8743570 [move_group-4] [INFO] [1773364604.874194593] [move_group.moveit.moveit.ros.move_group.move_action]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
1773364604.8744507 [move_group-4] [INFO] [1773364604.874241182] [move_group.moveit.moveit.ros.move_group.capability]: Using planning pipeline 'ompl'
1773364604.8746774 [move_group-4] [INFO] [1773364604.874574782] [move_group]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
1773364604.8748834 [move_group-4] [INFO] [1773364604.874673346] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
1773364604.8750725 [move_group-4] [INFO] [1773364604.874779168] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
1773364604.9203026 [move_group-4] [INFO] [1773364604.920086452] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
1773364604.9204049 [move_group-4] [WARN] [1773364604.920134573] [move_group.moveit.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
1773364604.9205694 [move_group-4] [INFO] [1773364604.920485538] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'manipulator[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
1773364604.9206889 [move_group-4] [INFO] [1773364604.920552654] [move_group]: Calling Planner 'OMPL'
1773364604.9687419 [move_group-4] [INFO] [1773364604.968469251] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: manipulator[RRTConnectkConfigDefault]: Allocating specialized state sampler for state space
1773364604.9723790 [move_group-4] [INFO] [1773364604.972223509] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: manipulator[RRTConnectkConfigDefault]: Allocating specialized state sampler for state space
1773364604.9749815 [move_group-4] [INFO] [1773364604.974863311] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: manipulator[RRTConnectkConfigDefault]: Allocating specialized state sampler for state space
1773364604.9764707 [move_group-4] [INFO] [1773364604.976314092] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: manipulator[RRTConnectkConfigDefault]: Allocating specialized state sampler for state space
1773364605.0230508 [move_group-4] [INFO] [1773364605.022860138] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: manipulator[RRTConnectkConfigDefault]: Allocating specialized state sampler for state space
1773364605.1873317 [move_group-4] [INFO] [1773364605.187179880] [move_group]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
1773364605.1898572 [move_group-4] [INFO] [1773364605.189772058] [move_group]: Calling PlanningResponseAdapter 'AddRuckigTrajectorySmoothing'
1773364605.1909330 [move_group-4] [INFO] [1773364605.190844749] [move_group.moveit.moveit.ros.move_group.move_action]: Motion plan was computed successfully.
1773364605.1931057 [moveit2_planner-6] [INFO] [1773364605.192918800] [moveit2_planner.moveit.ros.move_group_interface]: Planning request complete!
1773364605.1933768 [moveit2_planner-6] [INFO] [1773364605.193072536] [moveit2_planner.moveit.ros.move_group_interface]: time taken to generate plan: 0.265991 seconds
1773364605.1944008 [moveit2_planner-6] [INFO] [1773364605.194289994] [moveit2_planner.moveit.ros.move_group_interface]: Execute request accepted
1773364605.1999478 [move_group-4] [INFO] [1773364605.199824227] [move_group.moveit.moveit.ros.move_group.clear_octomap_service]: Execution request received
1773364605.2001228 [move_group-4] [INFO] [1773364605.199881772] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
1773364605.2001781 [move_group-4] [INFO] [1773364605.199901804] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
1773364605.2002041 [move_group-4] [INFO] [1773364605.199947216] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
1773364605.2002277 [move_group-4] [INFO] [1773364605.200009998] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ...
1773364605.2003217 [move_group-4] [INFO] [1773364605.200102585] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
1773364605.2003555 [move_group-4] [INFO] [1773364605.200139040] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
1773364605.2005866 [move_group-4] [INFO] [1773364605.200434456] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller
1773364605.2010040 [move_group-4] [INFO] [1773364605.200857706] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution
1773364605.2010651 [move_group-4] [INFO] [1773364605.200875715] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!