How to perform a gradual stop with the Joint Trajectory Controller

[jacob-kruse]

Hello,

I am utilizing the Joint Trajectory Controller along with the MoveIt2 Python API to motion plan with a Kuka LBR iisy 3 r760 robotic manipulator. I was wondering if there was a way to interrupt the robot while it is executing a trajectory and have it come to a gradual stop. Basically, I am trying to execute a controlled stop on command without having to use the Emergency Stop (effective, but rough on the robot). I have tried cancelling the goal and have found that although this works, it comes to an immediate, abrupt stop which I am trying to avoid. Any suggestions?

Jacob Kruse

[christophfroehlich]

This is currently not possible out-of-the box.

But soonish we will have joint-limit enforcement directly from the resource_manager of ros2_control, see ros-controls/ros2_control#1989
This should at least limit the output to the jerk and acceleration limits of the URDF (from the ros2_control tag because there are no such tags in the original URDF spec, see ros-controls/ros2_control#1472).

Could you compile the version of ros2_control from the PR from source, adapt your ros2_control settings in the URDF, and test if this improves your behavior? There is no documentation of that feature yet, have a look at the URDF description of the tests and you will see the desired syntax.

[jacob-kruse]

This is currently not possible out-of-the box.

But soonish we will have joint-limit enforcement directly from the resource_manager of ros2_control, see ros-controls/ros2_control#1989 This should at least limit the output to the jerk and acceleration limits of the URDF (from the ros2_control tag because there are no such tags in the original URDF spec, see ros-controls/ros2_control#1472).

Could you compile the version of ros2_control from the PR from source, adapt your ros2_control settings in the URDF, and test if this improves your behavior? There is no documentation of that feature yet, have a look at the URDF description of the tests and you will see the desired syntax.

Thanks for the response! I will try and implement this and let you know the results.

[jacob-kruse]

@christophfroehlich
Just to make sure I understand, by compiling the PR you tagged, I have the ability to set acceleration and jerk limits. Does that mean when I cancel a goal or stop the execution of the trajectory, the controller should abide by these acceleration and jerk limits and come to a gradual stop? If so, is the proper course of action to cancel the goal through the action server?

[christophfroehlich]

cancelling from the action client, that is one possibility yes. ros2_control does not have error/emergency stop handling yet.

[jacob-kruse]

@christophfroehlich
I downloaded the source code from ros-controls/ros2_control#1989. After looking at the ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp from ros-controls/ros2_control#1472, I modified my URDF file below:

lbr_iisy_ros2_control.xacro

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="kuka_lbr_iisy_ros2_control" params="name prefix client_ip controller_ip use_fake_hardware qos_config_file roundtrip_time">
    <ros2_control name="${name}" type="system">
      <hardware>
        <xacro:if value="${use_fake_hardware}">
          <plugin>kuka_mock_hardware_interface::KukaMockHardwareInterface</plugin>
          <param name="cycle_time_ms">4</param>
          <param name="roundtrip_time_micro">${roundtrip_time}</param>
          <param name="mock_sensor_commands">false</param>
          <param name="state_following_offset">0.0</param>
        </xacro:if>
        <xacro:unless value="${use_fake_hardware}">
          <!-- Read QoS profile parameters  -->
          <xacro:if value="${qos_config_file == ''}">
            <xacro:property name="qos_config" value="$(find kuka_iiqka_eac_driver)/config/qos_config.yaml"/>
          </xacro:if>
          <xacro:unless value="${qos_config_file == ''}">
            <xacro:property name="qos_config" value="${qos_config_file}"/>
          </xacro:unless>
          <xacro:property name="qos_config_dict" value="${xacro.load_yaml(qos_config)}"/>
          <xacro:property name="consequent_lost_packets" value="${qos_config_dict['rt_packet_loss_profile']['consequent_lost_packets']}"/>
          <xacro:property name="lost_packets_in_timeframe" value="${qos_config_dict['rt_packet_loss_profile']['lost_packets_in_timeframe']}"/>
          <xacro:property name="timeframe_ms" value="${qos_config_dict['rt_packet_loss_profile']['timeframe_ms']}"/>
          <plugin>kuka_eac::KukaEACHardwareInterface</plugin>
          <param name="client_ip">${client_ip}</param>
          <param name="controller_ip">${controller_ip}</param>
          <param name="consequent_lost_packets">${consequent_lost_packets}</param>
          <param name="lost_packets_in_timeframe">${lost_packets_in_timeframe}</param>
          <param name="timeframe_ms">${timeframe_ms}</param>
        </xacro:unless>
      </hardware>
      <!-- define joints and command/state interfaces for each joint -->
      <joint name="${prefix}joint_1">
        <command_interface name="position"/>
        <command_interface name="stiffness"/>
        <command_interface name="damping"/>
        <command_interface name="effort"/>
        <command_interface name="acceleration">
          <param name="min">-17.567841</param>
          <param name="max">17.567841</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <state_interface name="effort">
          <param name="initial_value">0.0</param>
        </state_interface>
      </joint>
      <joint name="${prefix}joint_2">
        <command_interface name="position"/>
        <command_interface name="stiffness"/>
        <command_interface name="damping"/>
        <command_interface name="effort"/>
        <command_interface name="acceleration">
          <param name="min">-2.101232</param>
          <param name="max">2.101232</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">-1.5708</param>
        </state_interface>
        <state_interface name="effort">
          <param name="initial_value">0.0</param>
        </state_interface>
      </joint>
      <joint name="${prefix}joint_3">
        <command_interface name="position"/>
        <command_interface name="stiffness"/>
        <command_interface name="damping"/>
        <command_interface name="effort"/>
        <command_interface name="acceleration">
          <param name="min">-10.738869</param>
          <param name="max">10.738869</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">1.5708</param>
        </state_interface>
        <state_interface name="effort">
          <param name="initial_value">0.0</param>
        </state_interface>
      </joint>
      <joint name="${prefix}joint_4">
        <command_interface name="position"/>
        <command_interface name="stiffness"/>
        <command_interface name="damping"/>
        <command_interface name="effort"/>
        <command_interface name="acceleration">
          <param name="min">-27.276282</param>
          <param name="max">27.276282</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <state_interface name="effort">
          <param name="initial_value">0.0</param>
        </state_interface>
      </joint>
      <joint name="${prefix}joint_5">
        <command_interface name="position"/>
        <command_interface name="stiffness"/>
        <command_interface name="damping"/>
        <command_interface name="effort"/>
        <command_interface name="acceleration">
          <param name="min">-21.786787</param>
          <param name="max">21.786787</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <state_interface name="effort">
          <param name="initial_value">0.0</param>
        </state_interface>
      </joint>
      <joint name="${prefix}joint_6">
        <command_interface name="position"/>
        <command_interface name="stiffness"/>
        <command_interface name="damping"/>
        <command_interface name="effort"/>
        <command_interface name="acceleration">
          <param name="min">-112.291689</param>
          <param name="max">112.291689</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <state_interface name="effort">
          <param name="initial_value">0.0</param>
        </state_interface>
      </joint>
    </ros2_control>
  </xacro:macro>
</robot>

The file above is included in my robot's URDF macro.

However, when I run my launch file, I get this error:

[control_node-1] [FATAL] [1738688048.843594823] [KukaEACHardwareInterface]: expecting exactly 4 command interface
[control_node-1] [ERROR] [1738688048.843659594] [controller_manager.resource_manager]: Failed to initialize hardware 'lbr_iisy3_r760'
[control_node-1] [WARN] [1738688048.843682849] [controller_manager.resource_manager]: System hardware component 'lbr_iisy3_r760' from plugin 'kuka_eac::KukaEACHardwareInterface' failed to initialize.
[control_node-1] [WARN] [1738688048.843911731] [controller_manager]: Could not load and initialize hardware. Please check previous output for more details. After you have corrected your URDF, try to publish robot description again.

I tried to comment out the damping command interface so that there are only four command interfaces like it says in the message, but then I receive this error:

[control_node-1] [FATAL] [1738689616.104033973] [KukaEACHardwareInterface]: expecting 'DAMPING' command interface as third
[control_node-1] [ERROR] [1738689616.104107438] [controller_manager.resource_manager]: Failed to initialize hardware 'lbr_iisy3_r760'
[control_node-1] [WARN] [1738689616.104131965] [controller_manager.resource_manager]: System hardware component 'lbr_iisy3_r760' from plugin 'kuka_eac::KukaEACHardwareInterface' failed to initialize.
[control_node-1] [WARN] [1738689616.104375210] [controller_manager]: Could not load and initialize hardware. Please check previous output for more details. After you have corrected your URDF, try to publish robot description again

From this I can assume that my current hardware interface (KukaEACHardwareInterface) does not allow for the acceleration command interface. Any suggestions?

[christophfroehlich]

I see your point. @saikishor can the joint_limits plugin consider acceleration if no acceleration command_interface exists, e.g., velocity only? How can one give the parameters then?

[jacob-kruse]

@christophfroehlich
I have looked into other solutions for my desired functionality. One forum talked about the stop_trajectory_duration parameter from the ROS version of the joint_trajectory_controller. Do you know if this parameter will ever come to the ROS2 version or if there is something similar already implemented?

Also, even though the robot does not have an acceleration command interface, it does have stiffness and damping command interfaces. Does the code from the PR enforce these command limits as well. If so, is there any way I could use these with the limiting method you initially suggested to simulate a gradual stop?

[jacob-kruse]

I have found a workaround solution for the time being, although the stop produced is still too abrupt for my liking.

See this discussion thread.

[christophfroehlich]

@christophfroehlich I have looked into other solutions for my desired functionality. One forum talked about the stop_trajectory_duration parameter from the ROS version of the joint_trajectory_controller. Do you know if this parameter will ever come to the ROS2 version or if there is something similar already implemented?

I was not aware that this existed with ROS. You are very welcome to port this to ROS 2 and open a PR :)

[pastoriomarco]

I'm also trying to find the best way to get a soft stop on the robot.
For now the closest I've come to it is by sending a one-point trajectory with the current position, zero velocity and a time from start that determines the time the robot has to stop. The robot will "spring back" to the position it was at the time of the call. The further the time from start, the softer the stop, but the larger the move made to stop at a given speed. I found that a time to stop of about 0.5 to 1 seconds seems good enough without being too abrupt, but it may depend on the use case.

You can find the implementation here in "sendControlledStop" function:
https://github.com/pastoriomarco/manymove/blob/humble/manymove_planner/src/move_group_planner.cpp

The downsides are:

• It doesn't guarantee that the original trajectory is respected, but it does try to get back to a point that is part of the original trajectory.
• It still seem to have a sudden spike in acceleration, but it seems more of an issue of how the traj controller handles the transition when the current trajectory is preempted.

To explain myself better on the second point:
using a real robot, when I preempt the trajectory the robots does a sudden "brake" or "bump" in the execution before continuing smoothly on the new trajectory. It's like a spike of deceleration/acceleration. This happens regardless of the fact that the new traj is a single point or a smoother sequence.
I'm currently testing it only on Ufactory cobots, so I don't know if this depends on the joint trajectory controller or on the xarm package implementation.
Since is a spring-back motion, the controller may be just be cranking up the acceleration on the opposite direction of the motion on all axes thus causing an not so smooth transition? Maybe is due to lack of jerk control in the trajectory controller?
Does anyone know if this is common/normal/intended?

PS: I'm using Humble right now, but I plan to port it to Jazzy ASAP.
I also forgot to mention that, in Humble, if I cancel the action on the follow joint trajectory the move still completes, it doesn't stop at all. Is this expected?