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 :)