[BUG] - Robot Manager Node not Deactivating on Error

[jacob-kruse]

Description

Description

I am running the moveit_planning_example.launch.py to control my Kuka LBR iisy 3 R760 robot with MoveIt2's Python API MoveItPy. Sometimes if the velocity/acceleration scaling factor is set too high in my configuration for MoveIt2, the velocity/acceleration limits will be exceeded, leading to an error and the protective stop engaging. The robot_manager node is supposed to be deactivated when an error occurs, allowing the user to reactivate the robot_manager node and clear the stop flag that is being raised. However, this is not the case.

See related discussion: #200

KUKA robot OS

iiQKA

KUKA robot OS version

iiQKA OS 1.2.13

KUKA external interface version

iiQKA External API 1.0.3

Affected robot model(s)

Kuka LBR iisy 3 R760

Version or commit hash of the driver

master

Setup

Setup:
Ubuntu 24.04.1
ROS2 Jazzy Jalisco (https://docs.ros.org/en/jazzy/Installation.html)
MoveIt2 Binary Installation for Jazzy (https://moveit.ros.org/install-moveit2/binary/)
kroshu/kuka-external-control-sdk (master)
kroshu/kuka_drivers (master)
kroshu/kuka_robot_descriptions (master)

Launch Files:
`kuka_drivers/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py`
`kuka_drivers/kuka_iiqka_eac_driver/launch/startup.launch.py`
Note: Make sure to change the controller_ip and client_ip arguments in `kuka_iiqka_eac_driver/launch/startup.launch.py` for your setup.

Executable:
`kuka_irl_project/scripts/test_one.py`
'''
#!/usr/bin/env python3

import time
import rclpy
from rclpy.logging import get_logger
from ament_index_python import get_package_share_directory

from moveit_configs_utils import MoveItConfigsBuilder
from moveit.core.robot_state import RobotState
from moveit.planning import MoveItPy, PlanRequestParameters, MultiPipelinePlanRequestParameters
from moveit.core.kinematic_constraints import construct_joint_constraint

# Plans the motion based on joint goals and executes the plan
def plan_and_execute(robot, planning_component, logger, single_plan_parameters=None, multi_plan_parameters=None, sleep_time=0.1):
    
    logger.info("Planning trajectory")
    
    # Formulate the plan for different planning scenarios
    if multi_plan_parameters is not None:
        plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters)
        
    elif single_plan_parameters is not None:
        plan_result = planning_component.plan(single_plan_parameters=single_plan_parameters)
        
    else:
        plan_result = planning_component.plan()

    # Execute Plan
    if plan_result:
        logger.info("Executing plan")
        robot_trajectory = plan_result.trajectory
        robot.execute(robot_trajectory, controllers=[])
    else:
        logger.error("Planning failed")

    time.sleep(sleep_time)


def main():
    
    rclpy.init()
    logger = get_logger("moveit_py.pose_goal")
    
    # Set sleep time
    sleep_time = 0.5

    moveit_config = (
        MoveItConfigsBuilder('kuka_lbr_iisy')
        .robot_description(
            file_path=(get_package_share_directory("kuka_lbr_iisy_support") + "/urdf/lbr_iisy3_r760.urdf.xacro"),
            mappings={
                "x": 0,
                "y": 0,
                "z": 0,
                "roll": 0,
                "pitch": 0,
                "yaw": 0,
                "prefix": "_"
            },
        )
        .robot_description_semantic(
            file_path=(get_package_share_directory('kuka_lbr_iisy_moveit_config')) +'/urdf/lbr_iisy3_r760.srdf')
        .joint_limits(
            file_path=get_package_share_directory("kuka_lbr_iisy_support")+ "/config/lbr_iisy3_r760_joint_limits.yaml")
        .moveit_cpp(
            file_path=get_package_share_directory('kuka_irl_project') + '/config/planning.yaml')
        .to_moveit_configs()
    )
    
    # Define moveit configuration
    moveit = moveit_config.to_dict()
    
    # Instantiate MoveItPy with the manipulator component
    kuka_move = MoveItPy(node_name="moveit_py", config_dict=moveit)
    kuka = kuka_move.get_planning_component("manipulator")  
    logger.info("MoveItPy instance created")

    # Get the Robot Model and create a variable to represent its state
    robot_model = kuka_move.get_robot_model()
    robot_state = RobotState(robot_model)


    # Function that takes joint values, formulates the goal, and sends to execute function
    def execute_joint_goals(joint_values, planner):
        
        # Set the current state as the start for planning and assign joint values to robot state
        kuka.set_start_state_to_current_state()
        robot_state.joint_positions = joint_values
        
        # Use the robot state and model to construct the goal
        joint_constraint = construct_joint_constraint(
            robot_state=robot_state,
            joint_model_group=kuka_move.get_robot_model().get_joint_model_group("manipulator"),
        )

        if planner == 'ompl':
            single_plan_parameters = PlanRequestParameters(
                kuka_move, "ompl_rrtc_solo"
            )
            multi_plan_parameters = None
            
        elif planner == 'pilz':
            single_plan_parameters = PlanRequestParameters(
                kuka_move, "pilz_lin_solo"
            )
            multi_plan_parameters = None
            
        elif planner == 'chomp':
            single_plan_parameters = PlanRequestParameters(
                kuka_move, "chomp_planner_solo"
            )
            multi_plan_parameters = None
            
        elif planner == 'multi':
            single_plan_parameters = None
            multi_plan_parameters = MultiPipelinePlanRequestParameters(
                kuka_move, ['ompl_rrtc_multi', 'pilz_lin_multi', 'chomp_planner_multi']
            )
            
        else:
            single_plan_parameters = None
            multi_plan_parameters = None
        
        # Set the goal state of the joints and execute the motion plan
        kuka.set_goal_state(motion_plan_constraints=[joint_constraint])
        plan_and_execute(kuka_move, kuka, logger, single_plan_parameters, multi_plan_parameters, sleep_time)


    # A list of actions that will be sent to the robot one by one, either a gripper action or
    # a list of goal joint values->[joint1, joint2, joint3, joint4, joint5, joint6, planner]
    actions = [
                [0.0875, -1.937, 1.5375, 0.0414, 1.882, -0.842, 'multi'],
                [1.024, -0.7903, 1.255, 0.0, 1.092169, -0.9, 'multi'],
                [1.024, -1.343, 1.436, 0.0, 1.343, -0.042, 'multi'],
                [0.0875, -1.937, 1.5375, 0.0414, 1.882, -0.842, 'multi'],
                ]
    
    # Loop occurs for all actions above, executes gripper action or assigns the list of values to the corresponding joints
    for action in actions:

        joint_values = {
            "joint_1": action[0],
            "joint_2": action[1],
            "joint_3": action[2],
            "joint_4": action[3],
            "joint_5": action[4],
            "joint_6": action[5],
        }
        
        # Send the current joint goal to the execute joint goals function
        execute_joint_goals(joint_values, action[6])

if __name__ == "__main__":
    main()
'''
Note: Make sure to change the file path for `.moveit_cpp` in the MoveItConfigsBuilder for the path of the configuration file below.

Configuration File:
`kuka_irl_project/config/planning.yaml`
'''
planning_scene_monitor_options:
  name: "planning_scene_monitor"
  robot_description: "robot_description"
  joint_state_topic: "/joint_states"
  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
  publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
  wait_for_initial_state_timeout: 10.0

planning_pipelines:
  pipeline_names: ["ompl", "pilz_industrial_motion_planner", 'chomp']

ompl_rrtc_multi:
  plan_request_params:
    planning_attempts: 10
    planning_pipeline: ompl
    planner_id: "RRTConnectkConfigDefault"
    max_velocity_scaling_factor: 0.6
    max_acceleration_scaling_factor: 0.6
    planning_time: 2.0

pilz_lin_multi:
  plan_request_params:
    planning_attempts: 10
    planning_pipeline: pilz_industrial_motion_planner
    planner_id: "PTP"
    max_velocity_scaling_factor: 0.6
    max_acceleration_scaling_factor: 0.6
    planning_time: 2.0

chomp_planner_multi:
  plan_request_params:
    planning_attempts: 10
    planning_pipeline: chomp
    max_velocity_scaling_factor: 0.6                
    max_acceleration_scaling_factor: 0.6
    planning_time: 2.0
'''
Note: You may have to increase the velocity/acceleration scaling to trigger the protective stop depending on the motion script you are utilizing. The current values will cause an error in the Python script I provided above.

Here is my project's src folder:
src
├── kuka_robot_descriptions
├── kuka_drivers
|   ├── kuka_iiqka_eac_driver
|   |   └── launch
|   |       └── startup.launch.py
|   └── examples
|       └── iiqka_moveit_example
|           └── launch
|               └── moveit_planning_example.launch.py
└── kuka_irl_project
    ├── config
    |   └── planning.yaml
    └── scripts
        └── test_one.py

Reproduction steps

First, start the example launch for moveit:
ros2 launch iiqka_moveit_example moveit_planning_example.launch.py

Then, configure and activate the `robot_manager` node in another terminal:
ros2 lifecycle set robot_manager configure
ros2 lifecycle set robot_manager activate

Finally, run my Python test script or another motion script with high velocity/acceleration scaling factors in the configuration file to trigger the protective stop:
ros2 run kuka_irl_project test_one.py

Logs

After exceeding the joint velocity/acceleration limit, the following error prints in the terminal running the `moveit_planning_example.launch.py`:
[control_node-1] [ERROR] [1733417126.120470650] [KukaEACHardwareInterface]: External control stopped by an error
[control_node-1] [ERROR] [1733417126.120567490] [KukaEACHardwareInterface]: Session suspended

When an error is received, the `robot_manager` node is supposed to deactivate. The proper course of action to reset the stop flag is to then reactivate the `robot_manager` node. However, the following occurs:
ros2 lifecycle set robot_manager activate
Unknown transition requested, available ones are:
- deactivate [4]
- shutdown [7]

If we acquire the current state of the `robot_manager` node:
ros2 service call /robot_manager/get_state lifecycle_msgs/srv/GetState
requester: making request: lifecycle_msgs.srv.GetState_Request()

response:
lifecycle_msgs.srv.GetState_Response(current_state=lifecycle_msgs.msg.State(id=3, label='active'))

The `robot_manager` node cannot be activated because it is still active.

[Svastits]

Thanks for the detailed description, I will try to reproduce

Meanwhile, could you monitor the topic "kuka_event_broadcaster/hardware_event" when such an error occurs? The hardware interface error event should be published to this topic, it would help me localize the issue if I knew if whether the message arrives there

[jacob-kruse]

I monitored the "/kuka_event_broadcaster/hardware_event" topic and reproduced the error, but nothing published. After inspecting the topics using "rqt_graph", I see that nothing is publishing to the topic. This is likely the reason that the robot_manager node is not deactivating properly.

[jacob-kruse]

I recently discovered some odd behavior that may be causing the issue. When I first start the moveit_planning_example.launch.py , the "hardware_event" topic is listed as "/kuka_event_broadcaster/hardware_event". However, after configuring the robot_manager node, the topic seems to switch to "/event_broadcaster/hardware_event". After exceeding the velocity/acceleration limits, I can see that the topic prints a "6", which corresponds to an "ERROR" hardware event.

[Svastits]

Thanks for the hint, I could also reproduce this error and it seems that there is indeed a mismatch in the topic names.
We will try to improve our automatic testing to avoid such bugs in the future :)
Fixed in #204