How to cancel a MoveIt2 generated trajectory with a controlled stop?

[jacob-kruse]

Hello,

I am utilizing 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

[jacob-kruse]

I have found a workaround solution for the time being, although the stop produced is still too abrupt for my liking. For those that may want to implement this as well, my code is shown below.

#!/usr/bin/env python3

import time
import rclpy
import argparse
from threading import Thread
from rclpy.logging import get_logger
from ament_index_python import get_package_share_directory

from moveit_configs_utils import MoveItConfigsBuilder
from moveit.planning import MoveItPy, PlanRequestParameters, MultiPipelinePlanRequestParameters
from geometry_msgs.msg import PoseStamped

# Plans the motion based on pose goals and executes the plan
def plan_and_execute(robot, planning_component, logger, single_plan_parameters=None, multi_plan_parameters=None, sleep_time=0.0):
    
    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")
    
    # Define arguments for test script
    desc = "Script to test the functionality of MoveItPy Pose Planning"
    parser = argparse.ArgumentParser(description=desc)
    parser.add_argument('--sleep_time', dest='sleep_time', default='0.0', help='Specifies the delay in between motion requests (float)')
    parser.add_argument('--gazebo', dest='gazebo', action='store_true', help='Tells script to use sim time and Gazebo URDF (bool)')
    args = parser.parse_args()
    
    # Define variables from passed arguments
    sleep_time = float(args.sleep_time)
    gazebo = args.gazebo
    
    # Logic for loading correct URDF
    if gazebo:
        urdf_file = 'gazebo_combined.urdf.xacro'
        
    else:
        urdf_file = 'lbr_iisy3_r760_combined.urdf.xacro'
    
    # Define MoveIt Configuration
    moveit_config = (
        MoveItConfigsBuilder('kuka_lbr_iisy')
        .robot_description(
            file_path=f'/urdf/{urdf_file}')
        .robot_description_semantic(
            file_path=(get_package_share_directory('kuka_irl_project')) + '/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()
    )

    # Convert MoveIt Configuration to a dictionary
    moveit = moveit_config.to_dict()
    
    # If using Gazebo, use sim time
    if gazebo:
        moveit.update({"use_sim_time": True})
        moveit.update({'qos_overrides./clock.subscription.durability': 'volatile'})
        moveit.update({'qos_overrides./clock.subscription.history': 'keep_last'})
        moveit.update({'qos_overrides./clock.subscription.depth': 10})
        moveit.update({'qos_overrides./clock.subscription.reliability': 'reliable'})

    # 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")
    
    # Define a list of actions that describe a goal pose or gripper action, pose_link represents which part of the robot will be sent to the pose
    actions=[
                {'frame_id': 'world', 'pose_link': 'gripper_base_link','planner': 'multi',
                'x_position': 0.5, 'y_position': 0.0, 'z_position': 0.99,
                'w_orientation': 1.0, 'x_orientation': 0.0, 'y_orientation': 0.0, 'z_orientation': 0.0},
                
                {'frame_id': 'world', 'pose_link': 'gripper_base_link','planner': 'multi',
                'x_position': 0.40111662283352756, 'y_position': -0.08802419243477377, 'z_position': 1.009233221679309,
                'w_orientation': 1.0, 'x_orientation': 0.0, 'y_orientation': 0.0, 'z_orientation': 0.0},
                
                {'frame_id': 'world', 'pose_link': 'gripper_base_link','planner': 'multi',
                'x_position': 0.5, 'y_position': 0.0, 'z_position': 1.1,
                'w_orientation': 1.0, 'x_orientation': 0.0, 'y_orientation': 0.0, 'z_orientation': 0.0},
                ]
    
    # Loop that occurs for each action above
    for action in actions:
          kuka.set_start_state_to_current_state()
          
          pose_goal = PoseStamped()
          pose_goal.header.frame_id = action['frame_id']
          pose_goal.pose.position.x = action['x_position']
          pose_goal.pose.position.y = action['y_position']
          pose_goal.pose.position.z = action['z_position']
          pose_goal.pose.orientation.w = action['w_orientation']
          pose_goal.pose.orientation.x = action['x_orientation']
          pose_goal.pose.orientation.y = action['y_orientation']
          pose_goal.pose.orientation.z = action['z_orientation']
          
          if action['planner'] == 'ompl':
              single_plan_parameters = PlanRequestParameters(
                  kuka_move, "ompl_rrtc_solo"
              )
              multi_plan_parameters = None
              
          elif action['planner'] == 'pilz':
              single_plan_parameters = PlanRequestParameters(
                  kuka_move, "pilz_lin_solo"
              )
              multi_plan_parameters = None
              
          elif action['planner'] == 'stomp':
              single_plan_parameters = PlanRequestParameters(
                  kuka_move, "stomp_solo"
              )
              multi_plan_parameters = None
              
          elif action['planner'] == 'multi':
              single_plan_parameters = None
              multi_plan_parameters = MultiPipelinePlanRequestParameters(
                  kuka_move, ["ompl_rrtc_multi", "pilz_lin_multi", "stomp_multi"]
              )
              
          else:
              single_plan_parameters = None
              multi_plan_parameters = None
          
          kuka.set_goal_state(pose_stamped_msg=pose_goal, pose_link=action['pose_link'])
          
          execution_thread = Thread(target=plan_and_execute, args=(kuka_move, kuka, logger, single_plan_parameters, multi_plan_parameters, sleep_time))
          execution_thread.start()

          
          '''This stops the robot when the script is killed with "Ctrl+C"'''
          try:
              while execution_thread.is_alive():
                  time.sleep(0.1)
          except KeyboardInterrupt:
              trajectory_manager = kuka_move.get_trajectory_execution_manager()
              trajectory_manager.stop_execution()
              
          
          '''This allows the robot to be stopped with "Ctrl+D" and a user input is required to go to the next point (finicky)'''
          # try:
          #     while execution_thread.is_alive():
          #         user_input = input("Enter something: ")
          #         time.sleep(0.1)
          # except EOFError:
          #     trajectory_manager = kuka_move.get_trajectory_execution_manager()
          #     trajectory_manager.stop_execution()
          
          
          '''This is the original function call before implementing stop execution with Threading'''
          # plan_and_execute(kuka_move, kuka, logger, sleep_time=sleep_time, single_plan_parameters=single_plan_parameters, multi_plan_parameters=multi_plan_parameters)


if __name__ == "__main__":
    main()

The most important thing to note is the last couple of lines where I implement an execution Thread. You may wonder why I am executing the robot motion in another Thread, but this is necessary due to MoveItPy's handling of a Keyboard Interrupt. When you use the MoveItPy execute() function, the robot will execute the planned trajectory. If you press Ctr+C while the robot is executing the motion, the robot will finish the trajectory before coming to a stop. This is undesirable, as I want the robot to stop immediately for safety reasons. To work around this, I am running the execute() function in a separate thread so that when I want to stop the robot with Ctrl+C, it instead handles the Keyboard Interrupt with my try-except statement. This leads to the Trajectory Execution Manager's stop_execution() function being executed to stop the robot immediately.

You may also notice that I have another try-except statement that handles an EOFError. This has the same functionality as I previously described, but Ctrl-D needs to be pressed during the execution to stop the robot. Something interesting about this implementation is that it will pause after executing one of the actions and wait for a keyboard input before executing the next action. This is very useful for testing and adds another layer of safety. This method has its flaws, but I thought it would be useful to add as well.