[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