[Iron] - JTC produces jump at the end of the trajectory if chained with admittance controller

[firesurfer]

Describe the bug

I have a setup as described in #1181 where I have a chained admittance controller with a JTC on top. For some reason the the JTC commands a jump in the joint positions at the end of the trajectory:

controller_state/output/positions:

Configuration:

Note: This is the scaled_jtc from the ur package.

joint_trajectory_controller_admittance_top:
  ros__parameters:
    joints:
      - ur_top/shoulder_pan_joint
      - ur_top/shoulder_lift_joint
      - ur_top/elbow_joint
      - ur_top/wrist_1_joint
      - ur_top/wrist_2_joint
      - ur_top/wrist_3_joint
    command_joints:
      - admittance_controller_top/ur_top/shoulder_pan_joint
      - admittance_controller_top/ur_top/shoulder_lift_joint
      - admittance_controller_top/ur_top/elbow_joint
      - admittance_controller_top/ur_top/wrist_1_joint
      - admittance_controller_top/ur_top/wrist_2_joint
      - admittance_controller_top/ur_top/wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.0
      goal_time: 0.0
      admittance_controller_top/ur_top/shoulder_pan_joint: { trajectory: 0.0, goal: 0.000 }
      admittance_controller_top/ur_top/shoulder_lift_joint: { trajectory: 0.0, goal: 0.000 }
      admittance_controller_top/ur_top/elbow_joint: { trajectory: 0.0, goal: 0.000 }
      admittance_controller_top/ur_top/wrist_1_joint: { trajectory: 0.0, goal: 0.000 }
      admittance_controller_top/ur_top/wrist_2_joint: { trajectory: 0.0, goal: 0.000 }
      admittance_controller_top/ur_top/wrist_3_joint: { trajectory: 0.0, goal: 0.000 }
    use_speed_scaling_topic_instead: true                                           #Addition for the scaled jtc from the ur package for multiple arms
    speed_scaling_topic_name: "/runtime_speed_scaling_factor"

To Reproduce

Have an admittance controller + jtc and have it drive a trajectory. There will be a jump at the end of the trajectory

Expected behavior

No jump

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version Iron binary install
  • rclcpp with race condition fix (Issue also happens without it) + scaled jtc instead of default jtc

[christophfroehlich]

scaled JTC is not directly maintained by ros2_control team, but @fmauch trys to keep it up to date afaik.

Maybe https://github.com/ros-controls/ros2_controllers/pull/877 is not included there, which could explain your behavior.

[firesurfer]

@christophfroehlich You are absolutely right. I thought the scaled JTC reuses most of the upstream JTC but this is not the case especially when it comes to the goal conditions. With the upstream JTC it works fine.

@fmauch Does it make sense to backport the mentioned fix ?

[christophfroehlich]

I'd rather suggest bringing https://github.com/ros-controls/ros2_controllers/pull/301 up-to-date and merging it ;)