Using Pilz planner - Githubissues

gaspatxo commented 6 months ago

Hi, I would like to make use of the pilz planner with with moveit2. I am not sure on how do that.

I added pilz_cartesian_limits.yaml and pilz_industrial_motion_planner_planning.yaml in src/xarm_ros2/xarm_moveit_config/config/ but I imagine that when launching launch files from the xarm_moveit_config the moveitConfigBuilder class is not being used.

penglongxiang commented 6 months ago

Hi, unfortunately we have little experience with pilz planner. We will look into this but it may take some time. Perhaps you may check this launch file and try modifying the 'ompl' related contents to proper 'pilz' planner counterparts.

gaspatxo commented 6 months ago

I will look into it. Right now I am trying to use your custom MoveItConfigsBuilder implementation together with the moveit2 Getting Started tutorial launch file

Is this possible at all? This is my latest attempt:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch.launch_context import LaunchContext
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder

def generate_launch_description():

    # Define xacro mappings for the robot description file
    launch_arguments = {
        "robot_ip": "",
        "use_fake_hardware": "true",
        "dof": "7",
    }

    # Load the robot configuration
    moveit_config = (
        MoveItConfigsBuilder(
            context=LaunchContext(),
            controllers_name="fake_controllers",
            robot_type="xarm",
            dof=7,
            add_other_geometry=True,
            geometry_type="mesh",
            geometry_mesh_filename="../../../../../../my_stuff/meshes/extruder_assembly.stl",
            geometry_mesh_origin_rpy='"0 0 3.655"',
            geometry_mesh_origin_xyz='"0.0203 0.0886 0"',
            geometry_mesh_tcp_xyz='"0 0 0.1124"',
            geometry_mesh_tcp_rpy='"0 0 0"',
            geometry_mass=0.8,
        )
        .robot_description(mappings=launch_arguments)
        .trajectory_execution(file_path="config/xarm7/fake_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
        .to_moveit_configs()
    )

    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict()],
    )

    # RViz for visualization
    # Get the path to the RViz configuration file
    rviz_config_arg = DeclareLaunchArgument(
        "rviz_config",
        default_value="xarm_moveit_config_demo.rviz",
        description="RViz configuration file",
    )
    rviz_base = LaunchConfiguration("rviz_config")
    rviz_config = PathJoinSubstitution(
        [FindPackageShare("moveit2_tutorials"), "launch", rviz_base]
    )

    # Launch RViz
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "base_link"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
    )

    # ros2_control using mock hardware for trajectory execution
    ros2_controllers_path = os.path.join(
        get_package_share_directory("xarm_controller"),
        "config",
        "xarm7_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path],
        remappings=[
            ("/controller_manager/robot_description", "/robot_description"),
        ],
        output="both",
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
    )

    return LaunchDescription(
        [
            rviz_config_arg,
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            arm_controller_spawner,
        ]
    )

Which, when launched, gives me an empty rviz world with the folloing output:

xarm7_moveit_launch.txt

vimior commented 6 months ago

@gaspatxo

Try this

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch.launch_context import LaunchContext
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder

def generate_launch_description():
    # Load the robot configuration
    moveit_config = (
        MoveItConfigsBuilder(
            context=LaunchContext(),
            controllers_name="fake_controllers",
            robot_type="xarm",
            dof=7,
            ros2_control_plugin='uf_robot_hardware/UFRobotFakeSystemHardware',
            add_other_geometry=True,
            geometry_type="mesh",
            geometry_mesh_filename="../../../../../../my_stuff/meshes/extruder_assembly.stl",
            geometry_mesh_origin_rpy='"0 0 3.655"',
            geometry_mesh_origin_xyz='"0.0203 0.0886 0"',
            geometry_mesh_tcp_xyz='"0 0 0.1124"',
            geometry_mesh_tcp_rpy='"0 0 0"',
            geometry_mass=0.8,
        )
        .robot_description()
        .trajectory_execution(file_path="config/xarm7/fake_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
        .to_moveit_configs()
    )

    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict()],
    )

    # RViz for visualization
    # Get the path to the RViz configuration file
    rviz_config_arg = DeclareLaunchArgument(
        "rviz_config",
        default_value="xarm_moveit_config_demo.rviz",
        description="RViz configuration file",
    )
    rviz_base = LaunchConfiguration("rviz_config")
    rviz_config = PathJoinSubstitution(
        [FindPackageShare("moveit2_tutorials"), "launch", rviz_base]
    )

    # Launch RViz
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "base_link"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
    )

    # ros2_control using mock hardware for trajectory execution
    ros2_controllers_path = os.path.join(
        get_package_share_directory("xarm_controller"),
        "config",
        "xarm7_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path],
        remappings=[
            ("/controller_manager/robot_description", "/robot_description"),
        ],
        output="both",
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["xarm7_traj_controller", "-c", "/controller_manager"],
    )

    return LaunchDescription(
        [
            rviz_config_arg,
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            arm_controller_spawner,
        ]
    )

gaspatxo commented 6 months ago

Thank you very much @vimior, it now works in simulation, I slighltly modified the launch file, my guess is that it would not make a diference but just for completion I will add it here.

However, this cannot currently connect to the real robot. I suspect that to do that I must pass the robot_ip parameter to the MoveitConfigsBuilder in order to then generate and pass the robot_description to a couple of your executables. But I am unsure if that is the case; or if a more complicated approach is needed.

Thank you again for your help.

The current launch file I am succesfully using (only in simulation)

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.launch_context import LaunchContext
from launch.logging import get_logger
from ament_index_python.packages import get_package_share_directory
from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder

def generate_launch_description():

    # Load the robot configuration
    moveit_config = (
        MoveItConfigsBuilder(
            context=LaunchContext(),
            controllers_name="fake_controllers",
            robot_type="xarm",
            dof=7,
            ros2_control_plugin="uf_robot_hardware/UFRobotFakeSystemHardware",
            add_other_geometry=True,
            geometry_type="mesh",
            geometry_mesh_filename="../../../../../../my_stuff/meshes/extruder_assembly.stl",
            geometry_mesh_origin_rpy='"0 0 3.655"',
            geometry_mesh_origin_xyz='"0.0203 0.0886 0"',
            geometry_mesh_tcp_xyz='"0 0 0.1124"',
            geometry_mesh_tcp_rpy='"0 0 0"',
            geometry_mass=0.8,
        )
        .robot_description()
        .trajectory_execution(file_path="config/xarm7/fake_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
        .to_moveit_configs()
    )

    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict()],
        arguments=["--log-level", "debug"],
    )

    # RViz
    rviz_config_file = (
        get_package_share_directory("xarm_moveit_config") + "/rviz/moveit.rviz"
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "base_link"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
    )

    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("xarm_controller"),
        "config",
        "xarm7_controllers.yaml",
    )

    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path, moveit_config.robot_description],
        remappings=[
            ("/controller_manager/robot_description", "/robot_description"),
        ],
        output="both",
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["xarm7_traj_controller", "-c", "/controller_manager"],
    )

    return LaunchDescription(
        [
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            arm_controller_spawner,
        ]
    )

vimior commented 6 months ago

@gaspatxo If used on a real machine, some parameters need to be modified.

Add __robot_ip__ parameter
The __controllers_name__ parameter is "controllers"
The ros2_control_plugin parameter is "uf_robot_hardware/UFRobotSystemHardware"

moveit_config = (
      MoveItConfigsBuilder(
          context=LaunchContext(),
          controllers_name="controllers",
          robot_type="xarm",
          robot_ip="xxxxxxxxxxxx",
          dof=7,
          ros2_control_plugin="uf_robot_hardware/UFRobotSystemHardware",
          add_other_geometry=True,
          geometry_type="mesh",
          geometry_mesh_filename="../../../../../../my_stuff/meshes/extruder_assembly.stl",
          geometry_mesh_origin_rpy='"0 0 3.655"',
          geometry_mesh_origin_xyz='"0.0203 0.0886 0"',
          geometry_mesh_tcp_xyz='"0 0 0.1124"',
          geometry_mesh_tcp_rpy='"0 0 0"',
          geometry_mass=0.8,
      )
      .robot_description()
      .trajectory_execution(file_path="config/xarm7/controllers.yaml")
      .planning_scene_monitor(
          publish_robot_description=True, publish_robot_description_semantic=True
      )
      .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
      .to_moveit_configs()
  )

xArm-Developer / xarm_ros2

Using Pilz planner #72