[JTC] Commands new targets after trajectory excution has finished

[firesurfer]

Describe the bug

We run a force limited setup where we use a JTC to command a trajectory which the actual actuator might not be able to follow completely (one servo axis). With the current version (3.14.0) we ran multiple times into the issue that after the JTC has finished execution the axis started moving into the opposite direction.
With the previous version (3.13.0) this behavior does not occur.
To Reproduce

Have a setup where the actual goal cannot be reached ( in our case for example by roughly 8cm).
Command that goal.
In like 6 to 10 cases the axis will move slowly in the opposite direction of the target (where it came from) after the JTC has finished execution.

Configuration

servo_position_controller:
  ros__parameters:
    joints:
      - axis_A
      - axis_B
    command_interfaces:
      - position
    state_interfaces:
      - position
     
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: true
    constraints:
      stopped_velocity_tolerance: 0.1
      goal_time: 0.0
      axis_A: { trajectory: 0.0, goal: 0.0 }
      axis_B: { trajectory: 0.0, goal: 0.0 }

Expected behavior

The axis should stay where it is after execution has finished or should have the last point of the trajectory as target

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version ros-iron JTC: 3.14.0

Additional context

Given that 3.13.0 works it has to be one of those commits.

Given that 3.14.0 also introduces #759 I would suggest reverting those commits related to the JTC until a solution was found.

[christophfroehlich]

Do you use the action or topic interface? If the action interface, does it report to have reached the goal?

Could you provide us a rosbag with /joint_states and ~/controller_state, or a complete example to reproduce it? Maybe this can be reproduced with the rrrbot.

[firesurfer]

I am using the action interface and it reports having reached the goal. I try to collect a rosbag with the desired information in the next few days.

[christophfroehlich]

thanks, and please also add the console output.
do you use a real robot or any simulator?

[firesurfer]

Unfortunately I cannot give you a full rosbag of the joint_states and of the controller_state due to confidentially reasons.

Nevertheless I can reproduce the issue pretty consistently and created a plot of the the relevant joints from the /controller_state topic.

As you can see in the image the output position decreases up to a certain point where the sampling of the trajectory has finished. Afterwards it to the current position and then commands steps into the opposite direction

Including the console output

[ros2_control_node-3] [INFO] [1693988064.540833014] [servo_position_controller]: Received new action goal
[ros2_control_node-3] [INFO] [1693988064.540926912] [servo_position_controller]: Accepted new action goa
[ros2_control_node-3] [WARN] [1693988067.407933074] [servo_position_controller]: Aborted due goal_time_tolerance exceeding by 0.800499 seconds
[ros2_control_node-3] [ERROR] [1693988068.211929376] [servo_position_controller]: Exceeded goal_time_tolerance: holding position...
[ros2_control_node-3] [ERROR] [1693988069.013934840] [servo_position_controller]: Exceeded goal_time_tolerance: holding position...
[ros2_control_node-3] [ERROR] [1693988069.817922565] [servo_position_controller]: Exceeded goal_time_tolerance: holding position...

This phenomena happens on a real robot. You might wonder why there is such a large difference between the target and current position, this is intended in our use case as we using this servo as a compliant axis with a rather low current limit.
Usually the axis will move but until the current limit is reached. In this example I stopped the axis by hand for demonstration purposes.

This phenomena does not happen if I increase the current limit and have the axis follow the target more accurately.

Edit: This data was collected with adapted JTC parameters according to my other issue:

servo_position_controller:
  ros__parameters:
    joints:
      - axis_A
      - axis_B
    command_interfaces:
      - position
    state_interfaces:
      - position
     
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: true
    constraints:
      stopped_velocity_tolerance: 0.1
      goal_time: 0.8
      axis_A: { trajectory: 0.0, goal: 0.001 }
      axis_B: { trajectory: 0.0, goal: 0.001 }

[christophfroehlich]

Thanks for the details. The resolution of the image is too low to read the legend, cyan is the reference and magenta the output?
This looks like the issue #757, for which I already proposed a solution with #758.
If you have time I'd appreciate if you could test the solution by building JTC from source.

[firesurfer]

Hi sorry for my late answer. I was on holidays.

I just tested the solution from #758 and seems to work fine!