Indeterministic FollowJointTrajectory action server. First try: Goal aborted: Second try: Succesful

Ask
1

Hi there,

Goals sent to the FollowJointTrajectory action server via ros2 frequently fail despite lying in the numerical range of the joint DoF.

Below is an example where I

  • first set lift & pitch to 0.3 and 0.4 respectively and it aborts after the goal timeout, and then
  • second, request the same action again which, immediately succeeds.

According to the joint limits topic, pitch should go up as high as 0.45 rad, and sometimes the action even errors out for a pitch target at 0.25.

This is not perfectly replicatable as sometimes it will in fact work on the first try with a 0.4 pitch target.

Is this known behavior? Is there additional configuration/measures available to prevent unexpected aborts?

Thanks!

$ ros2 action send_goal /stretch_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{trajectory: {joint_names: ['joint_lift', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll', 'joint_gripper_finger_left', 'wrist_extension'], points: [{positions: [0.3, 0.0, 0.4, 0.0, 0.0, 0.0], time_from_start: {sec: 0}}]}}"
Waiting for an action server to become available...
Sending goal:
     trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  joint_names:
  - joint_lift
  - joint_wrist_yaw
  - joint_wrist_pitch
  - joint_wrist_roll
  - joint_gripper_finger_left
  - wrist_extension
  points:
  - positions:
    - 0.3
    - 0.0
    - 0.4
    - 0.0
    - 0.0
    - 0.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
multi_dof_trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  joint_names: []
  points: []
path_tolerance: []
component_path_tolerance: []
goal_tolerance: []
component_goal_tolerance: []
goal_time_tolerance:
  sec: 0
  nanosec: 0

Goal accepted with ID: 0d76ad955da444529585df4dda7f9ffb

Result:
    error_code: -4
error_string: Time to execute the current goal point = <trajectory_msgs.msg.JointTrajectoryPoint(positions=[0.3, 0.0, 0.4, 0.0, 0.0, 0.0], velocities=[], accelerations=[], effort=[], time_from_start=builtin_interfaces.msg.Duration(sec=0, nanosec=0))> exceeded the default_goal_timeout = 10.0

Goal finished with status: ABORTED

$ ros2 action send_goal /stretch_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{trajectory: {joint_names: ['joint_lift', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll', 'joint_gripper_finger_left', 'wrist_extension'], points: [{positions: [0.3, 0.0, 0.4, 0.0, 0.0, 0.0], time_from_start: {sec: 0}}]}}"
Waiting for an action server to become available...
Sending goal:
     trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  joint_names:
  - joint_lift
  - joint_wrist_yaw
  - joint_wrist_pitch
  - joint_wrist_roll
  - joint_gripper_finger_left
  - wrist_extension
  points:
  - positions:
    - 0.3
    - 0.0
    - 0.4
    - 0.0
    - 0.0
    - 0.0
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
multi_dof_trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  joint_names: []
  points: []
path_tolerance: []
component_path_tolerance: []
goal_tolerance: []
component_goal_tolerance: []
goal_time_tolerance:
  sec: 0
  nanosec: 0

Goal accepted with ID: e978951008ca4abeb2f90557df7210a1

Result:
    error_code: 0
error_string: Achieved all target points.

Goal finished with status: SUCCEEDED
2

MORE INFO

I have found at least one way of inducing an unexpected error in the action server:

  1. set a self-collision target (e.g., lift 0, extension 0) [EXPECTED ERROR]
  2. set a valid target (e.g., lift 0.4, extension 0.3) [UNEXPECTED ERROR]
  3. set another valid target (e.g., lift 0.5, extension 0.3, or the same valid target) [NO ERROR]

The problem here is that the VALID action 2 errors out based on a historic error. The internal self-collision state machine (or something similar) seems to be influenced by errors in the previous action without recovering properly.

3

Hi @MoKari, thanks for putting this together. I’ve been able to reproduce the issue you’re seeing. I believe I’ve solved it in this pull request: Fix for flaky goal timeouts due to wrist pitch by hello-binit · Pull Request #187 · hello-robot/stretch_ros2 · GitHub

Would you give it a try and let me know if it resolves this issue for you?

4

I’ve gone ahead and merged this. Let me know if the issue persists after you update your ROS workspace.