From 653b74e1ee3d687121a21a9c02e730e5cd9c2b6d Mon Sep 17 00:00:00 2001 From: Kenichi Maeda Date: Mon, 15 Jun 2026 21:52:10 -0400 Subject: [PATCH 1/4] Add dual-arm LBR demo support on rolling --- CHANGELOG.rst | 4 + lbr_demos/doc/lbr_demos.rst | 8 + .../lbr_dual_arm_bringup/CMakeLists.txt | 22 +++ .../lbr_dual_arm_bringup/doc/lbr_dual_arm.rst | 39 +++++ .../launch/hardware.launch.py | 95 +++++++++++ .../launch/mock.launch.py | 95 +++++++++++ .../launch/move_group.launch.py | 117 +++++++++++++ .../lbr_dual_arm_bringup/package.xml | 28 +++ .../lbr_dual_arm_description/CMakeLists.txt | 23 +++ .../doc/lbr_dual_arm_description.rst | 29 ++++ .../lbr_dual_arm_description/package.xml | 21 +++ .../ros2_control/dual_arm_controllers.yaml | 50 ++++++ .../ros2_control/lbr_one_system_config.yaml | 38 +++++ .../ros2_control/lbr_two_system_config.yaml | 38 +++++ .../urdf/lbr_dual_arm.xacro | 38 +++++ .../.setup_assistant | 25 +++ .../lbr_dual_arm_moveit_config/CMakeLists.txt | 11 ++ .../config/joint_limits.yaml | 80 +++++++++ .../config/kinematics.yaml | 8 + .../config/lbr_dual_arm.srdf | 134 +++++++++++++++ .../config/moveit.rviz | 159 ++++++++++++++++++ .../config/moveit_controllers.yaml | 27 +++ .../config/pilz_cartesian_limits.yaml | 6 + .../launch/move_group.launch.py | 31 ++++ .../launch/moveit_rviz.launch.py | 31 ++++ .../launch/setup_assistant.launch.py | 5 + .../lbr_dual_arm_moveit_config/package.xml | 41 +++++ 27 files changed, 1203 insertions(+) create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/CMakeLists.txt create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/joint_limits.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/kinematics.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/lbr_dual_arm.srdf create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit_controllers.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/pilz_cartesian_limits.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/setup_assistant.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f286706e..ff972f80 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +11,9 @@ Jazzy v2.x (TBD) * Replace ``ament_target_dependencies`` with ``target_link_libraries``. * ``lbr_moveit``: Add missing ``pynput`` dependency and fix dataclasses. + * ``lbr_dual_arm_bringup``: Dual arm bringup package. + * ``lbr_dual_arm_description``: Dual arm description package. + * ``lbr_dual_arm_moveit_config``: Dual arm moveit configuration package. * ``lbr_description``: * Removes ``lbr_description`` from this folder in favor of external descriptions: @@ -38,6 +41,7 @@ Jazzy v2.x (TBD) * Related pull requests: * MoveIt fix: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/379 + * Dual arm: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/386 (kenichi-maeda) * Namespace argument: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/344, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/353 * Effort limits: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/357 * URDF de-coupling: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/359, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/389 diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 8a8b431f..245de490 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -32,6 +32,14 @@ MoveIt lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst> lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> +Dual Arm +-------- +.. toctree:: + :titlesonly: + + lbr_dual_arm <../lbr_dual_arm/doc/lbr_dual_arm.rst> + lbr_dual_arm_description <../lbr_dual_arm_description/doc/lbr_dual_arm_description.rst> + Integration ----------- TODO system integration demos diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt new file mode 100644 index 00000000..ffedbdf8 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.22) +project(lbr_dual_arm_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY doc launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst new file mode 100644 index 00000000..b78f7822 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst @@ -0,0 +1,39 @@ +lbr_dual_arm_bringup +==================== +Launch package for the dual-arm ``iiwa7`` demo. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +Mock Bringup +------------ +Terminal 1: + +.. code-block:: bash + + ros2 launch lbr_dual_arm_bringup mock.launch.py + +Terminal 2: + +.. code-block:: bash + + ros2 launch lbr_dual_arm_bringup move_group.launch.py \ + rviz:=true + +Hardware Bringup +---------------- +Terminal 1: + +.. code-block:: bash + + ros2 launch lbr_dual_arm_bringup hardware.launch.py + +Terminal 2: + +.. code-block:: bash + + ros2 launch lbr_dual_arm_bringup move_group.launch.py \ + mode:=hardware \ + rviz:=true diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py new file mode 100644 index 00000000..56c9a360 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py @@ -0,0 +1,95 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from lbr_bringup.ros2_control import LBRROS2ControlMixin + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + ld.add_action( + DeclareLaunchArgument( + name="ctrl", + default_value="joint_trajectory_controller", + description="Desired default controller.", + choices=["joint_trajectory_controller"], + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="robot_name", + default_value="lbr_dual_arm", + description="Namespace for the dual-arm bringup nodes.", + ) + ) + + robot_description = { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathJoinSubstitution( + [ + FindPackageShare("lbr_dual_arm_description"), + "urdf", + "lbr_dual_arm.xacro", + ] + ), + " mode:=hardware", + ] + ) + } + + ld.add_action( + LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, + robot_name=LaunchConfiguration("robot_name"), + use_sim_time=False, + ) + ) + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathJoinSubstitution( + [ + FindPackageShare("lbr_dual_arm_description"), + "ros2_control", + "dual_arm_controllers.yaml", + ] + ), + robot_description, + ], + namespace=LaunchConfiguration("robot_name"), + remappings=[("~/robot_description", "robot_description")], + ) + ld.add_action(ros2_control_node) + + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + robot_name=LaunchConfiguration("robot_name"), + controller="joint_state_broadcaster", + ) + controller = LBRROS2ControlMixin.node_controller_spawner( + robot_name=LaunchConfiguration("robot_name"), + controller=LaunchConfiguration("ctrl"), + ) + + ld.add_action( + RegisterEventHandler( + OnProcessStart( + target_action=ros2_control_node, + on_start=[joint_state_broadcaster, controller], + ) + ) + ) + return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py new file mode 100644 index 00000000..76b1735f --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py @@ -0,0 +1,95 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from lbr_bringup.ros2_control import LBRROS2ControlMixin + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + ld.add_action( + DeclareLaunchArgument( + name="ctrl", + default_value="joint_trajectory_controller", + description="Desired default controller.", + choices=["joint_trajectory_controller"], + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="robot_name", + default_value="lbr_dual_arm", + description="Namespace for the dual-arm bringup nodes.", + ) + ) + + robot_description = { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathJoinSubstitution( + [ + FindPackageShare("lbr_dual_arm_description"), + "urdf", + "lbr_dual_arm.xacro", + ] + ), + " mode:=mock", + ] + ) + } + + ld.add_action( + LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, + robot_name=LaunchConfiguration("robot_name"), + use_sim_time=False, + ) + ) + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathJoinSubstitution( + [ + FindPackageShare("lbr_dual_arm_description"), + "ros2_control", + "dual_arm_controllers.yaml", + ] + ), + robot_description, + ], + namespace=LaunchConfiguration("robot_name"), + remappings=[("~/robot_description", "robot_description")], + ) + ld.add_action(ros2_control_node) + + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + robot_name=LaunchConfiguration("robot_name"), + controller="joint_state_broadcaster", + ) + controller = LBRROS2ControlMixin.node_controller_spawner( + robot_name=LaunchConfiguration("robot_name"), + controller=LaunchConfiguration("ctrl"), + ) + + ld.add_action( + RegisterEventHandler( + OnProcessStart( + target_action=ros2_control_node, + on_start=[joint_state_broadcaster, controller], + ) + ) + ) + return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py new file mode 100644 index 00000000..bb67b14e --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py @@ -0,0 +1,117 @@ +import os +from typing import List + +from ament_index_python import get_package_share_directory +from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from lbr_bringup.moveit import LBRMoveGroupMixin +from lbr_bringup.rviz import RVizMixin +from moveit_configs_utils import MoveItConfigsBuilder + + +def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: + ld = LaunchDescription() + + moveit_configs_builder = ( + MoveItConfigsBuilder( + robot_name="lbr_dual_arm", + package_name="lbr_dual_arm_moveit_config", + ) + .robot_description( + os.path.join( + get_package_share_directory("lbr_dual_arm_description"), + "urdf/lbr_dual_arm.xacro", + ), + mappings={"mode": LaunchConfiguration("mode")}, + ) + .planning_pipelines( + default_planning_pipeline="ompl", + pipelines=["ompl"], + ) + ) + move_group_params = LBRMoveGroupMixin.params_move_group() + moveit_configs = moveit_configs_builder.to_moveit_configs() + robot_name = LaunchConfiguration("robot_name") + + ld.add_action( + LBRMoveGroupMixin.node_move_group( + parameters=[ + moveit_configs_builder.to_dict(), + move_group_params, + {"use_sim_time": False}, + ], + namespace=robot_name, + ) + ) + + ld.add_action( + RVizMixin.node_rviz( + rviz_cfg_pkg="lbr_dual_arm_moveit_config", + rviz_cfg="config/moveit.rviz", + parameters=LBRMoveGroupMixin.params_rviz(moveit_configs=moveit_configs) + + [{"use_sim_time": False}], + remappings=[ + ( + "display_planned_path", + PathJoinSubstitution([robot_name, "display_planned_path"]), + ), + ("joint_states", PathJoinSubstitution([robot_name, "joint_states"])), + ( + "monitored_planning_scene", + PathJoinSubstitution([robot_name, "monitored_planning_scene"]), + ), + ( + "planning_scene", + PathJoinSubstitution([robot_name, "planning_scene"]), + ), + ( + "planning_scene_world", + PathJoinSubstitution([robot_name, "planning_scene_world"]), + ), + ( + "robot_description", + PathJoinSubstitution([robot_name, "robot_description"]), + ), + ( + "robot_description_semantic", + PathJoinSubstitution([robot_name, "robot_description_semantic"]), + ), + ( + "recognized_object_array", + PathJoinSubstitution([robot_name, "recognized_object_array"]), + ), + ], + condition=IfCondition(LaunchConfiguration("rviz")), + ) + ) + return ld.entities + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + ld.add_action( + DeclareLaunchArgument( + name="mode", + default_value="mock", + description="Dual-arm description mode.", + choices=["mock", "hardware"], + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="robot_name", + default_value="lbr_dual_arm", + description="Namespace for MoveIt and RViz remappings.", + ) + ) + ld.add_action(RVizMixin.arg_rviz(default_value="true")) + ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) + ld.add_action(LBRMoveGroupMixin.arg_capabilities()) + ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities()) + ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics()) + ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene()) + ld.add_action(OpaqueFunction(function=hidden_setup)) + return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml new file mode 100644 index 00000000..41290499 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml @@ -0,0 +1,28 @@ + + + + lbr_dual_arm_bringup + 2.4.3 + Dual-arm demo bringup and MoveIt launch package. + kenichi-maeda + mhubii + Apache-2.0 + + ament_cmake + + controller_manager + joint_state_broadcaster + joint_trajectory_controller + lbr_bringup + lbr_dual_arm_description + lbr_dual_arm_moveit_config + robot_state_publisher + xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt new file mode 100644 index 00000000..cf762080 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.22) +project(lbr_dual_arm_description) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(lbr_description REQUIRED) + +install( + DIRECTORY doc ros2_control urdf + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst new file mode 100644 index 00000000..27b10fd3 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst @@ -0,0 +1,29 @@ +lbr_dual_arm_description +======================== + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +Customize Robot Placement +------------------------- +#. Open ``urdf/lbr_dual_arm.xacro``. +#. Adjust the fixed joint origins: + + - ``lbr_one_base_joint`` + - ``lbr_two_base_joint`` + +#. Typical changes are: + + - Increase/decrease spacing between both arms via ``xyz``. + - Rotate one arm around the base frame via ``rpy``. + +Customize Hardware Network Settings +----------------------------------- +#. Open ``ros2_control/lbr_one_system_config.yaml`` and ``ros2_control/lbr_two_system_config.yaml``. +#. Set unique ``port_id`` values for each arm. +#. Set ``remote_host`` per arm according to your network setup. + +.. note:: + ``port_id`` must be different for both arms. diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml new file mode 100644 index 00000000..ec8165a3 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml @@ -0,0 +1,21 @@ + + + + lbr_dual_arm_description + 2.4.3 + Dual-arm LBR description demo package. + kenichi-maeda + mhubii + Apache-2.0 + + ament_cmake + + lbr_description + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml new file mode 100644 index 00000000..6a199a0a --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml @@ -0,0 +1,50 @@ +# Use of /** so that the configurations hold for controller +# managers regardless of their namespace. Usefull in multi-robot setups. +/**/controller_manager: + ros__parameters: + update_rate: 100 + + # ROS 2 control broadcasters + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # # LBR ROS 2 control broadcasters + # lbr_state_broadcaster: + # type: lbr_ros2_control/LBRStateBroadcaster + + # ROS 2 control controllers + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +# # LBR ROS 2 control broadcasters +# /**/lbr_state_broadcaster: +# ros__parameters: +# robot_name: lbr + +# ROS 2 control controllers +/**/joint_trajectory_controller: + ros__parameters: + joints: + # robot one + - lbr_one_A1 + - lbr_one_A2 + - lbr_one_A3 + - lbr_one_A4 + - lbr_one_A5 + - lbr_one_A6 + - lbr_one_A7 + # robot two + - lbr_two_A1 + - lbr_two_A2 + - lbr_two_A3 + - lbr_two_A4 + - lbr_two_A5 + - lbr_two_A6 + - lbr_two_A7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 50.0 + action_monitor_rate: 20.0 \ No newline at end of file diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml new file mode 100644 index 00000000..a3488d5d --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml @@ -0,0 +1,38 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 + client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups + remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection + rt_prio: 80 # real-time priority for the control loop + # exponential moving average time constant for joint position commands [s]: + # set tau > robot sample time for more smoothing on the commands + # set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.04 + command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] + # exponential moving average time constant for external joint torque measurements [s]: + # set tau > robot sample time for more smoothing on the external torque measurements + # set tau << robot sample time for no smoothing on the external torque measurements + external_torque_tau: 0.04 + # exponential moving average time constant for joint torque measurements [s]: + # set tau > robot sample time for more smoothing on the raw torque measurements + # set tau << robot sample time for no smoothing on the raw torque measurements + measured_torque_tau: 0.04 + # for position control, LBR works best in open_loop control mode + # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode + open_loop: true +estimated_ft_sensor: # estimates the external force-torque from the external joint torque values + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation + chain_root: lbr_one_link_0 + chain_tip: lbr_one_link_ee + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered + force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered + force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered + torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml new file mode 100644 index 00000000..b2039852 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml @@ -0,0 +1,38 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 + client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + port_id: 30201 # port id for the UDP communication. Useful in multi-robot setups + remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection + rt_prio: 80 # real-time priority for the control loop + # exponential moving average time constant for joint position commands [s]: + # set tau > robot sample time for more smoothing on the commands + # set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.04 + command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] + # exponential moving average time constant for external joint torque measurements [s]: + # set tau > robot sample time for more smoothing on the external torque measurements + # set tau << robot sample time for no smoothing on the external torque measurements + external_torque_tau: 0.04 + # exponential moving average time constant for joint torque measurements [s]: + # set tau > robot sample time for more smoothing on the raw torque measurements + # set tau << robot sample time for no smoothing on the raw torque measurements + measured_torque_tau: 0.04 + # for position control, LBR works best in open_loop control mode + # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode + open_loop: true +estimated_ft_sensor: # estimates the external force-torque from the external joint torque values + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation + chain_root: lbr_two_link_0 + chain_tip: lbr_two_link_ee + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered + force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered + force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered + torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro new file mode 100644 index 00000000..7ff5b76b --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant new file mode 100644 index 00000000..125ca860 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: lbr_dual_arm_description + relative_path: urdf/lbr_dual_arm.xacro + srdf: + relative_path: config/lbr_dual_arm.srdf + package_settings: + author_name: mhubii + author_email: m.huber_1994@hotmail.de + generated_timestamp: 1774816000 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/CMakeLists.txt b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/CMakeLists.txt new file mode 100644 index 00000000..077c8789 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.22) +project(lbr_dual_arm_moveit_config) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/joint_limits.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/joint_limits.yaml new file mode 100644 index 00000000..399ab3e8 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/joint_limits.yaml @@ -0,0 +1,80 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + lbr_one_A1: + has_velocity_limits: true + max_velocity: 1.7104226669544429 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_one_A2: + has_velocity_limits: true + max_velocity: 1.7104226669544429 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_one_A3: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_one_A4: + has_velocity_limits: true + max_velocity: 2.2689280275926285 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_one_A5: + has_velocity_limits: true + max_velocity: 2.4434609527920612 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_one_A6: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_one_A7: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A1: + has_velocity_limits: true + max_velocity: 1.7104226669544429 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A2: + has_velocity_limits: true + max_velocity: 1.7104226669544429 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A3: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A4: + has_velocity_limits: true + max_velocity: 2.2689280275926285 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A5: + has_velocity_limits: true + max_velocity: 2.4434609527920612 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A6: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: true + max_acceleration: 10.0 + lbr_two_A7: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: true + max_acceleration: 10.0 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/kinematics.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/kinematics.yaml new file mode 100644 index 00000000..80e3a0e0 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/kinematics.yaml @@ -0,0 +1,8 @@ +arm_one: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 +arm_two: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/lbr_dual_arm.srdf b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/lbr_dual_arm.srdf new file mode 100644 index 00000000..c899e87b --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/lbr_dual_arm.srdf @@ -0,0 +1,134 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz new file mode 100644 index 00000000..cedc543b --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz @@ -0,0 +1,159 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 396 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 34; 141; 255 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: lbr_dual_arm + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm_one + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 21; 21; 26 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0.5 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Target Frame: base_link + Value: Orbit (rviz_default_plugins) + Yaw: 5.699999809265137 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + Views: + collapsed: false diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit_controllers.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 00000000..eff400c9 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,27 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller + + joint_trajectory_controller: + type: FollowJointTrajectory + joints: + - lbr_one_A1 + - lbr_one_A2 + - lbr_one_A3 + - lbr_one_A4 + - lbr_one_A5 + - lbr_one_A6 + - lbr_one_A7 + - lbr_two_A1 + - lbr_two_A2 + - lbr_two_A3 + - lbr_two_A4 + - lbr_two_A5 + - lbr_two_A6 + - lbr_two_A7 + action_ns: follow_joint_trajectory + default: true diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/pilz_cartesian_limits.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py new file mode 100644 index 00000000..7db63441 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py @@ -0,0 +1,31 @@ +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + ld = LaunchDescription() + ld.add_action(DeclareLaunchArgument("mode", default_value="mock")) + + moveit_config = ( + MoveItConfigsBuilder( + "lbr_dual_arm", + package_name="lbr_dual_arm_moveit_config", + ) + .robot_description( + os.path.join( + get_package_share_directory("lbr_dual_arm_description"), + "urdf/lbr_dual_arm.xacro", + ), + mappings={"mode": LaunchConfiguration("mode")}, + ) + .to_moveit_configs() + ) + for entity in generate_move_group_launch(moveit_config).entities: + ld.add_action(entity) + return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 00000000..4ed306ae --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,31 @@ +import os + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + ld = LaunchDescription() + ld.add_action(DeclareLaunchArgument("mode", default_value="mock")) + + moveit_config = ( + MoveItConfigsBuilder( + "lbr_dual_arm", + package_name="lbr_dual_arm_moveit_config", + ) + .robot_description( + os.path.join( + get_package_share_directory("lbr_dual_arm_description"), + "urdf/lbr_dual_arm.xacro", + ), + mappings={"mode": LaunchConfiguration("mode")}, + ) + .to_moveit_configs() + ) + for entity in generate_moveit_rviz_launch(moveit_config).entities: + ld.add_action(entity) + return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/setup_assistant.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 00000000..e5d96795 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,5 @@ +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + return generate_setup_assistant_launch() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml new file mode 100644 index 00000000..d3021935 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml @@ -0,0 +1,41 @@ + + + + lbr_dual_arm_moveit_config + 0.3.0 + + MoveIt configuration for the dual-arm LBR demo + + kenichi-maeda + mhubii + + BSD-3-Clause + + http://moveit.ros.org/ + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 + + kenichi-maeda + + ament_cmake + + joint_state_publisher + joint_state_publisher_gui + lbr_dual_arm_description + moveit_configs_utils + moveit_kinematics + moveit_planners + moveit_ros_move_group + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + xacro + + + ament_cmake + + From 2d73a40ee1f2cf06c90a456d72f25a9da388f651 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 20 Jun 2026 13:27:11 +0100 Subject: [PATCH 2/4] update doc --- lbr_ros2_control/system_integration/iiwa14/iiwa14.xacro | 2 +- lbr_ros2_control/system_integration/med14/med14.xacro | 2 +- lbr_ros2_control/system_integration/med7/med7.xacro | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_ros2_control/system_integration/iiwa14/iiwa14.xacro b/lbr_ros2_control/system_integration/iiwa14/iiwa14.xacro index 9e74f763..36928c40 100644 --- a/lbr_ros2_control/system_integration/iiwa14/iiwa14.xacro +++ b/lbr_ros2_control/system_integration/iiwa14/iiwa14.xacro @@ -3,7 +3,7 @@ - + diff --git a/lbr_ros2_control/system_integration/med14/med14.xacro b/lbr_ros2_control/system_integration/med14/med14.xacro index bfccb23c..f393111d 100644 --- a/lbr_ros2_control/system_integration/med14/med14.xacro +++ b/lbr_ros2_control/system_integration/med14/med14.xacro @@ -3,7 +3,7 @@ - + diff --git a/lbr_ros2_control/system_integration/med7/med7.xacro b/lbr_ros2_control/system_integration/med7/med7.xacro index ec19d3e7..1b35f0dd 100644 --- a/lbr_ros2_control/system_integration/med7/med7.xacro +++ b/lbr_ros2_control/system_integration/med7/med7.xacro @@ -3,7 +3,7 @@ - + From 6bde65a437faf6092d50ca675bcd510d069e32dc Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 21 Jun 2026 15:30:50 +0100 Subject: [PATCH 3/4] update dual arm demo --- CHANGELOG.rst | 7 +- lbr_demos/doc/lbr_demos.rst | 12 +- .../CMakeLists.txt | 11 +- .../config}/dual_arm_controllers.yaml | 11 +- .../config/initial_joint_positions.yaml | 8 + .../lbr_dual_arm/config/joint_limits.yaml | 49 ++++ .../config/lbr_one_system_config.yaml | 32 +++ .../config/lbr_two_system_config.yaml | 32 +++ .../lbr_dual_arm/doc/lbr_dual_arm.rst | 63 +++++ .../lbr_dual_arm/launch/hardware.launch.py | 150 ++++++++++++ .../lbr_dual_arm/launch/mock.launch.py | 150 ++++++++++++ .../package.xml | 10 +- .../lbr_dual_arm/urdf/lbr_dual_arm.xacro | 61 +++++ .../lbr_dual_arm_bringup/doc/lbr_dual_arm.rst | 39 --- .../launch/hardware.launch.py | 95 -------- .../launch/mock.launch.py | 95 -------- .../launch/move_group.launch.py | 117 --------- .../lbr_dual_arm_description/CMakeLists.txt | 23 -- .../doc/lbr_dual_arm_description.rst | 29 --- .../lbr_dual_arm_description/package.xml | 21 -- .../ros2_control/lbr_one_system_config.yaml | 38 --- .../ros2_control/lbr_two_system_config.yaml | 38 --- .../urdf/lbr_dual_arm.xacro | 38 --- .../.setup_assistant | 6 +- .../config/moveit.rviz | 222 +++++++++++++++++- .../launch/move_group.launch.py | 9 +- .../launch/moveit_rviz.launch.py | 9 +- .../lbr_dual_arm_moveit_config/package.xml | 10 +- 28 files changed, 787 insertions(+), 598 deletions(-) rename lbr_demos/lbr_dual_arm/{lbr_dual_arm_bringup => lbr_dual_arm}/CMakeLists.txt (52%) rename lbr_demos/lbr_dual_arm/{lbr_dual_arm_description/ros2_control => lbr_dual_arm/config}/dual_arm_controllers.yaml (79%) create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/config/initial_joint_positions.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/config/joint_limits.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_one_system_config.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_two_system_config.yaml create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py rename lbr_demos/lbr_dual_arm/{lbr_dual_arm_bringup => lbr_dual_arm}/package.xml (79%) create mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm/urdf/lbr_dual_arm.xacro delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml delete mode 100644 lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ff972f80..8b26cb8f 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,9 +11,8 @@ Jazzy v2.x (TBD) * Replace ``ament_target_dependencies`` with ``target_link_libraries``. * ``lbr_moveit``: Add missing ``pynput`` dependency and fix dataclasses. - * ``lbr_dual_arm_bringup``: Dual arm bringup package. - * ``lbr_dual_arm_description``: Dual arm description package. - * ``lbr_dual_arm_moveit_config``: Dual arm moveit configuration package. + * ``lbr_dual_arm``: Adds dual-arm integration demo package. + * ``lbr_dual_arm_moveit_config``: Adds dual-arm moveit configuration package. * ``lbr_description``: * Removes ``lbr_description`` from this folder in favor of external descriptions: @@ -41,7 +40,7 @@ Jazzy v2.x (TBD) * Related pull requests: * MoveIt fix: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/379 - * Dual arm: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/386 (kenichi-maeda) + * Dual-arm: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/393 (kenichi-maeda) * Namespace argument: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/344, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/353 * Effort limits: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/357 * URDF de-coupling: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/359, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/389 diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 245de490..59ca6b57 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -32,14 +32,10 @@ MoveIt lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst> lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> -Dual Arm --------- -.. toctree:: - :titlesonly: - - lbr_dual_arm <../lbr_dual_arm/doc/lbr_dual_arm.rst> - lbr_dual_arm_description <../lbr_dual_arm_description/doc/lbr_dual_arm_description.rst> Integration ----------- -TODO system integration demos +.. toctree:: + :titlesonly: + + lbr_dual_arm <../lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst> diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt b/lbr_demos/lbr_dual_arm/lbr_dual_arm/CMakeLists.txt similarity index 52% rename from lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt rename to lbr_demos/lbr_dual_arm/lbr_dual_arm/CMakeLists.txt index ffedbdf8..41ae0a97 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/CMakeLists.txt +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(lbr_dual_arm_bringup) +project(lbr_dual_arm) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -8,15 +8,8 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY doc launch + DIRECTORY config launch urdf DESTINATION share/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml similarity index 79% rename from lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml rename to lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml index 6a199a0a..e1a949e9 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/dual_arm_controllers.yaml +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml @@ -8,19 +8,10 @@ joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - # # LBR ROS 2 control broadcasters - # lbr_state_broadcaster: - # type: lbr_ros2_control/LBRStateBroadcaster - # ROS 2 control controllers joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController -# # LBR ROS 2 control broadcasters -# /**/lbr_state_broadcaster: -# ros__parameters: -# robot_name: lbr - # ROS 2 control controllers /**/joint_trajectory_controller: ros__parameters: @@ -47,4 +38,4 @@ - position - velocity state_publish_rate: 50.0 - action_monitor_rate: 20.0 \ No newline at end of file + action_monitor_rate: 20.0 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/initial_joint_positions.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/initial_joint_positions.yaml new file mode 100644 index 00000000..a4846b46 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/initial_joint_positions.yaml @@ -0,0 +1,8 @@ +# initial joint positions [degrees] +A1: 0.0 +A2: 25.0 +A3: 0.0 +A4: 90.0 +A5: 0.0 +A6: 0.0 +A7: 0.0 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/joint_limits.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/joint_limits.yaml new file mode 100644 index 00000000..2f942bc5 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/joint_limits.yaml @@ -0,0 +1,49 @@ +A1: + lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits + upper: 169 + velocity: 98 + effort: 200 + safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin + k_velocity: 10 +A2: + lower: -119 + upper: 119 + velocity: 98 + effort: 200 + safety_position_margin: 5 + k_velocity: 10 +A3: + lower: -169 + upper: 169 + velocity: 100 + effort: 200 + safety_position_margin: 5 + k_velocity: 10 +A4: + lower: -119 + upper: 119 + velocity: 130 + effort: 200 + safety_position_margin: 5 + k_velocity: 10 +A5: + lower: -169 + upper: 169 + velocity: 140 + effort: 200 + safety_position_margin: 5 + k_velocity: 10 +A6: + lower: -119 + upper: 119 + velocity: 180 + effort: 200 + safety_position_margin: 5 + k_velocity: 10 +A7: + lower: -174 + upper: 174 + velocity: 180 + effort: 200 + safety_position_margin: 5 + k_velocity: 10 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_one_system_config.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_one_system_config.yaml new file mode 100644 index 00000000..522829eb --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_one_system_config.yaml @@ -0,0 +1,32 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 +client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] +port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups +remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection +rt_prio: 80 # real-time priority for the control loop +# exponential moving average time constant for joint position commands [s]: +# set tau > robot sample time for more smoothing on the commands +# set tau << robot sample time for no smoothing on the commands +joint_position_tau: 0.04 +command_guard: + # if requested position / velocities beyond limits, CommandGuard will be triggered and the command will be overriden with the current state instead + # available: [default, safe_stop] + variant: default +state_guard: + # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE] + # in POSITION control mode, safety check is not performed + external_torque_safety_check: true + external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm] +# exponential moving average time constant for external joint torque measurements [s]: +# set tau > robot sample time for more smoothing on the external torque measurements +# set tau << robot sample time for no smoothing on the external torque measurements +external_torque_tau: 0.01 +# exponential moving average time constant for joint torque measurements [s]: +# set tau > robot sample time for more smoothing on the raw torque measurements +# set tau << robot sample time for no smoothing on the raw torque measurements +measured_torque_tau: 0.01 +# for position control, LBR works best in open_loop control mode +# note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode +open_loop: true diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_two_system_config.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_two_system_config.yaml new file mode 100644 index 00000000..4dc5200e --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/lbr_two_system_config.yaml @@ -0,0 +1,32 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 +client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] +port_id: 30201 # port id for the UDP communication. Useful in multi-robot setups +remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection +rt_prio: 80 # real-time priority for the control loop +# exponential moving average time constant for joint position commands [s]: +# set tau > robot sample time for more smoothing on the commands +# set tau << robot sample time for no smoothing on the commands +joint_position_tau: 0.04 +command_guard: + # if requested position / velocities beyond limits, CommandGuard will be triggered and the command will be overriden with the current state instead + # available: [default, safe_stop] + variant: default +state_guard: + # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE] + # in POSITION control mode, safety check is not performed + external_torque_safety_check: true + external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm] +# exponential moving average time constant for external joint torque measurements [s]: +# set tau > robot sample time for more smoothing on the external torque measurements +# set tau << robot sample time for no smoothing on the external torque measurements +external_torque_tau: 0.01 +# exponential moving average time constant for joint torque measurements [s]: +# set tau > robot sample time for more smoothing on the raw torque measurements +# set tau << robot sample time for no smoothing on the raw torque measurements +measured_torque_tau: 0.01 +# for position control, LBR works best in open_loop control mode +# note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode +open_loop: true diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst b/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst new file mode 100644 index 00000000..f5e16601 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst @@ -0,0 +1,63 @@ +lbr_dual_arm +============ +Dual-arm ``iiwa7`` integration demo, including: launch files, description file, and ``ros2_control`` configurations. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +Quick Start +----------- +Terminal 1: + +.. code-block:: bash + + ros2 launch lbr_dual_arm mock.launch.py + +.. note:: + For hardware, run ``hardware.launch.py`` (follow :doc:`Hardware Setup <../../../../lbr_fri_ros2_stack/doc/hardware_setup>` first). + +.. note:: + To configure position / orientation, launch with ``lbr_one_x:=`` etc. (list all arguments via ``ros2 launch lbr_dual_arm mock.launch.py -s``) + +Terminal 2: + +.. code-block:: bash + + ros2 launch lbr_dual_arm_moveit_config move_group.launch.py + +Terminal 3 (optional): + +.. code-block:: bash + + ros2 launch lbr_dual_arm_moveit_config moveit_rviz.launch.py + +Description File +---------------- +Custom description files can be generated via ``xacro``, see `lbr_dual_arm.xacro `_:octicon:`link-external`. + +#. Include macros: + + .. code-block:: xml + + + + +#. Instantiate macros (``ros2_control`` plugin and robot description) + + .. code-block:: xml + + + + +Done! The controller manager will load appropriate plugins when reading the robot description from the robot state publisher. + +Customize Hardware Network Settings +----------------------------------- +#. Open `lbr_one_system_config.yaml `_:octicon:`link-external` and `lbr_two_system_config.yaml `_:octicon:`link-external`. +#. Set unique ``port_id`` values for each arm. +#. Set ``remote_host`` per arm according to your network setup. + +.. note:: + ``port_id`` must be different for both arms. diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py new file mode 100644 index 00000000..3b9515c5 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py @@ -0,0 +1,150 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription( + [ + DeclareLaunchArgument( + name="namespace", + default_value="", + description="Nodes in this launch file will be spawned with this namespace.", + ), + DeclareLaunchArgument( + name="lbr_one_x", + default_value="0", + description="X position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_y", + default_value="0.5", + description="Y position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_z", + default_value="0", + description="Z position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_roll", + default_value="0", + description="Roll orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_pitch", + default_value="0.0", + description="Pitch orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_yaw", + default_value="0", + description="Yaw orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_two_x", + default_value="0", + description="X position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_y", + default_value="-0.5", + description="Y position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_z", + default_value="0", + description="Z position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_roll", + default_value="0", + description="Roll orientation of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_pitch", + default_value="0.0", + description="Pitch orientation of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_yaw", + default_value="0", + description="Yaw orientation of the second robot.", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathSubstitution(FindPackageShare("lbr_dual_arm")) + / "urdf" + / "lbr_dual_arm.xacro", + " mode:=hardware", + " lbr_one_x:=", + LaunchConfiguration("lbr_one_x"), + " lbr_one_y:=", + LaunchConfiguration("lbr_one_y"), + " lbr_one_z:=", + LaunchConfiguration("lbr_one_z"), + " lbr_one_roll:=", + LaunchConfiguration("lbr_one_roll"), + " lbr_one_pitch:=", + LaunchConfiguration("lbr_one_pitch"), + " lbr_one_yaw:=", + LaunchConfiguration("lbr_one_yaw"), + " lbr_two_x:=", + LaunchConfiguration("lbr_two_x"), + " lbr_two_y:=", + LaunchConfiguration("lbr_two_y"), + " lbr_two_z:=", + LaunchConfiguration("lbr_two_z"), + " lbr_two_roll:=", + LaunchConfiguration("lbr_two_roll"), + " lbr_two_pitch:=", + LaunchConfiguration("lbr_two_pitch"), + " lbr_two_yaw:=", + LaunchConfiguration("lbr_two_yaw"), + ] + ) + }, + {"use_sim_time": False}, + ], + namespace=LaunchConfiguration("namespace"), + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathSubstitution(FindPackageShare("lbr_dual_arm")) + / "config" + / "dual_arm_controllers.yaml", + ], + namespace=LaunchConfiguration("namespace"), + ), + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + "joint_trajectory_controller", + ], + namespace=LaunchConfiguration("namespace"), + ), + ] + ) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py new file mode 100644 index 00000000..059950e8 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py @@ -0,0 +1,150 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription( + [ + DeclareLaunchArgument( + name="namespace", + default_value="", + description="Nodes in this launch file will be spawned with this namespace.", + ), + DeclareLaunchArgument( + name="lbr_one_x", + default_value="0", + description="X position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_y", + default_value="0.5", + description="Y position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_z", + default_value="0", + description="Z position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_roll", + default_value="0", + description="Roll orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_pitch", + default_value="0.0", + description="Pitch orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_yaw", + default_value="0", + description="Yaw orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_two_x", + default_value="0", + description="X position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_y", + default_value="-0.5", + description="Y position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_z", + default_value="0", + description="Z position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_roll", + default_value="0", + description="Roll orientation of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_pitch", + default_value="0.0", + description="Pitch orientation of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_yaw", + default_value="0", + description="Yaw orientation of the second robot.", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathSubstitution(FindPackageShare("lbr_dual_arm")) + / "urdf" + / "lbr_dual_arm.xacro", + " mode:=mock", + " lbr_one_x:=", + LaunchConfiguration("lbr_one_x"), + " lbr_one_y:=", + LaunchConfiguration("lbr_one_y"), + " lbr_one_z:=", + LaunchConfiguration("lbr_one_z"), + " lbr_one_roll:=", + LaunchConfiguration("lbr_one_roll"), + " lbr_one_pitch:=", + LaunchConfiguration("lbr_one_pitch"), + " lbr_one_yaw:=", + LaunchConfiguration("lbr_one_yaw"), + " lbr_two_x:=", + LaunchConfiguration("lbr_two_x"), + " lbr_two_y:=", + LaunchConfiguration("lbr_two_y"), + " lbr_two_z:=", + LaunchConfiguration("lbr_two_z"), + " lbr_two_roll:=", + LaunchConfiguration("lbr_two_roll"), + " lbr_two_pitch:=", + LaunchConfiguration("lbr_two_pitch"), + " lbr_two_yaw:=", + LaunchConfiguration("lbr_two_yaw"), + ] + ) + }, + {"use_sim_time": False}, + ], + namespace=LaunchConfiguration("namespace"), + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathSubstitution(FindPackageShare("lbr_dual_arm")) + / "config" + / "dual_arm_controllers.yaml", + ], + namespace=LaunchConfiguration("namespace"), + ), + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + "joint_trajectory_controller", + ], + namespace=LaunchConfiguration("namespace"), + ), + ] + ) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/package.xml similarity index 79% rename from lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml rename to lbr_demos/lbr_dual_arm/lbr_dual_arm/package.xml index 41290499..101a72f0 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/package.xml +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/package.xml @@ -1,9 +1,9 @@ - lbr_dual_arm_bringup + lbr_dual_arm 2.4.3 - Dual-arm demo bringup and MoveIt launch package. + Dual-arm integration demo. kenichi-maeda mhubii Apache-2.0 @@ -13,9 +13,9 @@ controller_manager joint_state_broadcaster joint_trajectory_controller - lbr_bringup - lbr_dual_arm_description + lbr_ros2_control lbr_dual_arm_moveit_config + lbr_iiwa7_r800_description robot_state_publisher xacro @@ -25,4 +25,4 @@ ament_cmake - + \ No newline at end of file diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/urdf/lbr_dual_arm.xacro b/lbr_demos/lbr_dual_arm/lbr_dual_arm/urdf/lbr_dual_arm.xacro new file mode 100644 index 00000000..45948e08 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/urdf/lbr_dual_arm.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst deleted file mode 100644 index b78f7822..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/doc/lbr_dual_arm.rst +++ /dev/null @@ -1,39 +0,0 @@ -lbr_dual_arm_bringup -==================== -Launch package for the dual-arm ``iiwa7`` demo. - -.. contents:: Table of Contents - :depth: 2 - :local: - :backlinks: none - -Mock Bringup ------------- -Terminal 1: - -.. code-block:: bash - - ros2 launch lbr_dual_arm_bringup mock.launch.py - -Terminal 2: - -.. code-block:: bash - - ros2 launch lbr_dual_arm_bringup move_group.launch.py \ - rviz:=true - -Hardware Bringup ----------------- -Terminal 1: - -.. code-block:: bash - - ros2 launch lbr_dual_arm_bringup hardware.launch.py - -Terminal 2: - -.. code-block:: bash - - ros2 launch lbr_dual_arm_bringup move_group.launch.py \ - mode:=hardware \ - rviz:=true diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py deleted file mode 100644 index 56c9a360..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/hardware.launch.py +++ /dev/null @@ -1,95 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from lbr_bringup.ros2_control import LBRROS2ControlMixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - ld.add_action( - DeclareLaunchArgument( - name="ctrl", - default_value="joint_trajectory_controller", - description="Desired default controller.", - choices=["joint_trajectory_controller"], - ) - ) - ld.add_action( - DeclareLaunchArgument( - name="robot_name", - default_value="lbr_dual_arm", - description="Namespace for the dual-arm bringup nodes.", - ) - ) - - robot_description = { - "robot_description": Command( - [ - FindExecutable(name="xacro"), - " ", - PathJoinSubstitution( - [ - FindPackageShare("lbr_dual_arm_description"), - "urdf", - "lbr_dual_arm.xacro", - ] - ), - " mode:=hardware", - ] - ) - } - - ld.add_action( - LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, - robot_name=LaunchConfiguration("robot_name"), - use_sim_time=False, - ) - ) - - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[ - {"use_sim_time": False}, - PathJoinSubstitution( - [ - FindPackageShare("lbr_dual_arm_description"), - "ros2_control", - "dual_arm_controllers.yaml", - ] - ), - robot_description, - ], - namespace=LaunchConfiguration("robot_name"), - remappings=[("~/robot_description", "robot_description")], - ) - ld.add_action(ros2_control_node) - - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - robot_name=LaunchConfiguration("robot_name"), - controller="joint_state_broadcaster", - ) - controller = LBRROS2ControlMixin.node_controller_spawner( - robot_name=LaunchConfiguration("robot_name"), - controller=LaunchConfiguration("ctrl"), - ) - - ld.add_action( - RegisterEventHandler( - OnProcessStart( - target_action=ros2_control_node, - on_start=[joint_state_broadcaster, controller], - ) - ) - ) - return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py deleted file mode 100644 index 76b1735f..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/mock.launch.py +++ /dev/null @@ -1,95 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from lbr_bringup.ros2_control import LBRROS2ControlMixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - ld.add_action( - DeclareLaunchArgument( - name="ctrl", - default_value="joint_trajectory_controller", - description="Desired default controller.", - choices=["joint_trajectory_controller"], - ) - ) - ld.add_action( - DeclareLaunchArgument( - name="robot_name", - default_value="lbr_dual_arm", - description="Namespace for the dual-arm bringup nodes.", - ) - ) - - robot_description = { - "robot_description": Command( - [ - FindExecutable(name="xacro"), - " ", - PathJoinSubstitution( - [ - FindPackageShare("lbr_dual_arm_description"), - "urdf", - "lbr_dual_arm.xacro", - ] - ), - " mode:=mock", - ] - ) - } - - ld.add_action( - LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, - robot_name=LaunchConfiguration("robot_name"), - use_sim_time=False, - ) - ) - - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[ - {"use_sim_time": False}, - PathJoinSubstitution( - [ - FindPackageShare("lbr_dual_arm_description"), - "ros2_control", - "dual_arm_controllers.yaml", - ] - ), - robot_description, - ], - namespace=LaunchConfiguration("robot_name"), - remappings=[("~/robot_description", "robot_description")], - ) - ld.add_action(ros2_control_node) - - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - robot_name=LaunchConfiguration("robot_name"), - controller="joint_state_broadcaster", - ) - controller = LBRROS2ControlMixin.node_controller_spawner( - robot_name=LaunchConfiguration("robot_name"), - controller=LaunchConfiguration("ctrl"), - ) - - ld.add_action( - RegisterEventHandler( - OnProcessStart( - target_action=ros2_control_node, - on_start=[joint_state_broadcaster, controller], - ) - ) - ) - return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py deleted file mode 100644 index bb67b14e..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_bringup/launch/move_group.launch.py +++ /dev/null @@ -1,117 +0,0 @@ -import os -from typing import List - -from ament_index_python import get_package_share_directory -from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from lbr_bringup.moveit import LBRMoveGroupMixin -from lbr_bringup.rviz import RVizMixin -from moveit_configs_utils import MoveItConfigsBuilder - - -def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: - ld = LaunchDescription() - - moveit_configs_builder = ( - MoveItConfigsBuilder( - robot_name="lbr_dual_arm", - package_name="lbr_dual_arm_moveit_config", - ) - .robot_description( - os.path.join( - get_package_share_directory("lbr_dual_arm_description"), - "urdf/lbr_dual_arm.xacro", - ), - mappings={"mode": LaunchConfiguration("mode")}, - ) - .planning_pipelines( - default_planning_pipeline="ompl", - pipelines=["ompl"], - ) - ) - move_group_params = LBRMoveGroupMixin.params_move_group() - moveit_configs = moveit_configs_builder.to_moveit_configs() - robot_name = LaunchConfiguration("robot_name") - - ld.add_action( - LBRMoveGroupMixin.node_move_group( - parameters=[ - moveit_configs_builder.to_dict(), - move_group_params, - {"use_sim_time": False}, - ], - namespace=robot_name, - ) - ) - - ld.add_action( - RVizMixin.node_rviz( - rviz_cfg_pkg="lbr_dual_arm_moveit_config", - rviz_cfg="config/moveit.rviz", - parameters=LBRMoveGroupMixin.params_rviz(moveit_configs=moveit_configs) - + [{"use_sim_time": False}], - remappings=[ - ( - "display_planned_path", - PathJoinSubstitution([robot_name, "display_planned_path"]), - ), - ("joint_states", PathJoinSubstitution([robot_name, "joint_states"])), - ( - "monitored_planning_scene", - PathJoinSubstitution([robot_name, "monitored_planning_scene"]), - ), - ( - "planning_scene", - PathJoinSubstitution([robot_name, "planning_scene"]), - ), - ( - "planning_scene_world", - PathJoinSubstitution([robot_name, "planning_scene_world"]), - ), - ( - "robot_description", - PathJoinSubstitution([robot_name, "robot_description"]), - ), - ( - "robot_description_semantic", - PathJoinSubstitution([robot_name, "robot_description_semantic"]), - ), - ( - "recognized_object_array", - PathJoinSubstitution([robot_name, "recognized_object_array"]), - ), - ], - condition=IfCondition(LaunchConfiguration("rviz")), - ) - ) - return ld.entities - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - ld.add_action( - DeclareLaunchArgument( - name="mode", - default_value="mock", - description="Dual-arm description mode.", - choices=["mock", "hardware"], - ) - ) - ld.add_action( - DeclareLaunchArgument( - name="robot_name", - default_value="lbr_dual_arm", - description="Namespace for MoveIt and RViz remappings.", - ) - ) - ld.add_action(RVizMixin.arg_rviz(default_value="true")) - ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) - ld.add_action(LBRMoveGroupMixin.arg_capabilities()) - ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities()) - ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics()) - ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene()) - ld.add_action(OpaqueFunction(function=hidden_setup)) - return ld diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt deleted file mode 100644 index cf762080..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(lbr_dual_arm_description) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(lbr_description REQUIRED) - -install( - DIRECTORY doc ros2_control urdf - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst deleted file mode 100644 index 27b10fd3..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/doc/lbr_dual_arm_description.rst +++ /dev/null @@ -1,29 +0,0 @@ -lbr_dual_arm_description -======================== - -.. contents:: Table of Contents - :depth: 2 - :local: - :backlinks: none - -Customize Robot Placement -------------------------- -#. Open ``urdf/lbr_dual_arm.xacro``. -#. Adjust the fixed joint origins: - - - ``lbr_one_base_joint`` - - ``lbr_two_base_joint`` - -#. Typical changes are: - - - Increase/decrease spacing between both arms via ``xyz``. - - Rotate one arm around the base frame via ``rpy``. - -Customize Hardware Network Settings ------------------------------------ -#. Open ``ros2_control/lbr_one_system_config.yaml`` and ``ros2_control/lbr_two_system_config.yaml``. -#. Set unique ``port_id`` values for each arm. -#. Set ``remote_host`` per arm according to your network setup. - -.. note:: - ``port_id`` must be different for both arms. diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml deleted file mode 100644 index ec8165a3..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - lbr_dual_arm_description - 2.4.3 - Dual-arm LBR description demo package. - kenichi-maeda - mhubii - Apache-2.0 - - ament_cmake - - lbr_description - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml deleted file mode 100644 index a3488d5d..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_one_system_config.yaml +++ /dev/null @@ -1,38 +0,0 @@ -# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface -hardware: - fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro - major_version: 1 - minor_version: 15 - client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] - port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups - remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection - rt_prio: 80 # real-time priority for the control loop - # exponential moving average time constant for joint position commands [s]: - # set tau > robot sample time for more smoothing on the commands - # set tau << robot sample time for no smoothing on the commands - joint_position_tau: 0.04 - command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - # exponential moving average time constant for external joint torque measurements [s]: - # set tau > robot sample time for more smoothing on the external torque measurements - # set tau << robot sample time for no smoothing on the external torque measurements - external_torque_tau: 0.04 - # exponential moving average time constant for joint torque measurements [s]: - # set tau > robot sample time for more smoothing on the raw torque measurements - # set tau << robot sample time for no smoothing on the raw torque measurements - measured_torque_tau: 0.04 - # for position control, LBR works best in open_loop control mode - # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode - open_loop: true -estimated_ft_sensor: # estimates the external force-torque from the external joint torque values - enabled: true # whether to enable the force-torque estimation - update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) - rt_prio: 30 # real-time priority for the force-torque estimation - chain_root: lbr_one_link_0 - chain_tip: lbr_one_link_ee - damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian - force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered - force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered - force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered - torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml deleted file mode 100644 index b2039852..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/ros2_control/lbr_two_system_config.yaml +++ /dev/null @@ -1,38 +0,0 @@ -# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface -hardware: - fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro - major_version: 1 - minor_version: 15 - client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] - port_id: 30201 # port id for the UDP communication. Useful in multi-robot setups - remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection - rt_prio: 80 # real-time priority for the control loop - # exponential moving average time constant for joint position commands [s]: - # set tau > robot sample time for more smoothing on the commands - # set tau << robot sample time for no smoothing on the commands - joint_position_tau: 0.04 - command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - # exponential moving average time constant for external joint torque measurements [s]: - # set tau > robot sample time for more smoothing on the external torque measurements - # set tau << robot sample time for no smoothing on the external torque measurements - external_torque_tau: 0.04 - # exponential moving average time constant for joint torque measurements [s]: - # set tau > robot sample time for more smoothing on the raw torque measurements - # set tau << robot sample time for no smoothing on the raw torque measurements - measured_torque_tau: 0.04 - # for position control, LBR works best in open_loop control mode - # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode - open_loop: true -estimated_ft_sensor: # estimates the external force-torque from the external joint torque values - enabled: true # whether to enable the force-torque estimation - update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) - rt_prio: 30 # real-time priority for the force-torque estimation - chain_root: lbr_two_link_0 - chain_tip: lbr_two_link_ee - damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian - force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered - force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered - force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered - torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro b/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro deleted file mode 100644 index 7ff5b76b..00000000 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_description/urdf/lbr_dual_arm.xacro +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant index 125ca860..a49dc9ed 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant @@ -1,12 +1,12 @@ moveit_setup_assistant_config: urdf: - package: lbr_dual_arm_description + package: lbr_dual_arm relative_path: urdf/lbr_dual_arm.xacro srdf: relative_path: config/lbr_dual_arm.srdf package_settings: - author_name: mhubii - author_email: m.huber_1994@hotmail.de + author_name: kenichi-maeda + author_email: kenichi.maeda121@gmail.com generated_timestamp: 1774816000 control_xacro: command: diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz index cedc543b..95a7c7c5 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz @@ -4,9 +4,11 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 + - /Status1 - /MotionPlanning1 Splitter Ratio: 0.5 - Tree Height: 396 + Tree Height: 329 - Class: rviz_common/Help Name: Help - Class: rviz_common/Views @@ -38,7 +40,7 @@ Visualization Manager: - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: lbr_dual_arm + Move Group Namespace: "" MoveIt_Allow_Approximate_IK: false MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false @@ -60,6 +62,104 @@ Visualization Manager: Planned Path: Color Enabled: false Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + lbr_one_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + lbr_two_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -83,7 +183,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: arm_one + Planning Group: both_arms Query Goal State: true Query Start State: false Show Workspace: false @@ -100,6 +200,104 @@ Visualization Manager: Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + lbr_one_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_one_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + lbr_two_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_two_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true @@ -108,7 +306,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 21; 21; 26 - Fixed Frame: base_link + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -123,7 +321,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3 + Distance: 3.449727773666382 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -132,21 +330,21 @@ Visualization Manager: Focal Point: X: 0 Y: 0 - Z: 0.5 - Focal Shape Fixed Size: true + Z: 0 + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5 + Pitch: 0.4753984808921814 Target Frame: base_link Value: Orbit (rviz_default_plugins) - Yaw: 5.699999809265137 + Yaw: 3.1254067420959473 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1011 Help: collapsed: false Hide Left Dock: false @@ -155,5 +353,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd00000002000000000000022e00000395fc0200000004fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004400fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000003f000001c00000019600fffffffb000000100044006900730070006c0061007900730100000205000001cf000000cc00fffffffb0000000800480065006c007000000002b80000006f0000006f00ffffff00000001000001c900000395fc0200000001fb0000000a00560069006500770073010000003f00000395000000a900ffffff0000033b0000039500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false + Width: 1854 + X: 66 + Y: 32 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py index 7db63441..e07f5e64 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py @@ -1,4 +1,4 @@ -import os +from pathlib import Path from ament_index_python import get_package_share_directory from launch import LaunchDescription @@ -18,10 +18,9 @@ def generate_launch_description(): package_name="lbr_dual_arm_moveit_config", ) .robot_description( - os.path.join( - get_package_share_directory("lbr_dual_arm_description"), - "urdf/lbr_dual_arm.xacro", - ), + Path(get_package_share_directory("lbr_dual_arm")) + / "urdf" + / "lbr_dual_arm.xacro", mappings={"mode": LaunchConfiguration("mode")}, ) .to_moveit_configs() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py index 4ed306ae..efddb040 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py @@ -1,4 +1,4 @@ -import os +from pathlib import Path from ament_index_python import get_package_share_directory from launch import LaunchDescription @@ -18,10 +18,9 @@ def generate_launch_description(): package_name="lbr_dual_arm_moveit_config", ) .robot_description( - os.path.join( - get_package_share_directory("lbr_dual_arm_description"), - "urdf/lbr_dual_arm.xacro", - ), + Path(get_package_share_directory("lbr_dual_arm")) + / "urdf" + / "lbr_dual_arm.xacro", mappings={"mode": LaunchConfiguration("mode")}, ) .to_moveit_configs() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml index d3021935..645dffd4 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml @@ -3,9 +3,7 @@ lbr_dual_arm_moveit_config 0.3.0 - - MoveIt configuration for the dual-arm LBR demo - + MoveIt configuration for the dual-arm LBR demo kenichi-maeda mhubii @@ -21,7 +19,7 @@ joint_state_publisher joint_state_publisher_gui - lbr_dual_arm_description + lbr_iiwa7_r800_description moveit_configs_utils moveit_kinematics moveit_planners @@ -36,6 +34,6 @@ xacro - ament_cmake + ament_cmake - + \ No newline at end of file From f1551c9750de7b667c6fc26af0db48e3acef58b0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 22 Jun 2026 17:52:56 +0100 Subject: [PATCH 4/4] address dynamic pose for moveit (https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/393#pullrequestreview-4546215457) --- .../lbr_dual_arm/doc/lbr_dual_arm.rst | 17 +--- .../lbr_dual_arm/launch/hardware.launch.py | 73 ++++++++++++--- .../lbr_dual_arm/launch/mock.launch.py | 73 ++++++++++++--- .../config/moveit.rviz | 6 +- .../launch/move_group.launch.py | 93 ++++++++++++++++++- .../launch/moveit_rviz.launch.py | 93 ++++++++++++++++++- 6 files changed, 300 insertions(+), 55 deletions(-) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst b/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst index f5e16601..f60c284c 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst @@ -9,11 +9,12 @@ Dual-arm ``iiwa7`` integration demo, including: launch files, description file, Quick Start ----------- -Terminal 1: +Open a terminal, then run: .. code-block:: bash - ros2 launch lbr_dual_arm mock.launch.py + ros2 launch lbr_dual_arm mock.launch.py \ + moveit:=true .. note:: For hardware, run ``hardware.launch.py`` (follow :doc:`Hardware Setup <../../../../lbr_fri_ros2_stack/doc/hardware_setup>` first). @@ -21,18 +22,6 @@ Terminal 1: .. note:: To configure position / orientation, launch with ``lbr_one_x:=`` etc. (list all arguments via ``ros2 launch lbr_dual_arm mock.launch.py -s``) -Terminal 2: - -.. code-block:: bash - - ros2 launch lbr_dual_arm_moveit_config move_group.launch.py - -Terminal 3 (optional): - -.. code-block:: bash - - ros2 launch lbr_dual_arm_moveit_config moveit_rviz.launch.py - Description File ---------------- Custom description files can be generated via ``xacro``, see `lbr_dual_arm.xacro `_:octicon:`link-external`. diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py index 3b9515c5..91883a91 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py @@ -1,5 +1,6 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.substitutions import ( Command, FindExecutable, @@ -14,13 +15,13 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ DeclareLaunchArgument( - name="namespace", - default_value="", - description="Nodes in this launch file will be spawned with this namespace.", + name="moveit", + default_value="false", + description="Launch with MoveIt and RViz.", ), DeclareLaunchArgument( name="lbr_one_x", - default_value="0", + default_value="0.0", description="X position of the first robot.", ), DeclareLaunchArgument( @@ -30,12 +31,12 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_one_z", - default_value="0", + default_value="0.0", description="Z position of the first robot.", ), DeclareLaunchArgument( name="lbr_one_roll", - default_value="0", + default_value="0.0", description="Roll orientation of the first robot.", ), DeclareLaunchArgument( @@ -45,12 +46,12 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_one_yaw", - default_value="0", + default_value="0.0", description="Yaw orientation of the first robot.", ), DeclareLaunchArgument( name="lbr_two_x", - default_value="0", + default_value="0.0", description="X position of the second robot.", ), DeclareLaunchArgument( @@ -60,12 +61,12 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_two_z", - default_value="0", + default_value="0.0", description="Z position of the second robot.", ), DeclareLaunchArgument( name="lbr_two_roll", - default_value="0", + default_value="0.0", description="Roll orientation of the second robot.", ), DeclareLaunchArgument( @@ -75,7 +76,7 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_two_yaw", - default_value="0", + default_value="0.0", description="Yaw orientation of the second robot.", ), Node( @@ -121,7 +122,6 @@ def generate_launch_description() -> LaunchDescription: }, {"use_sim_time": False}, ], - namespace=LaunchConfiguration("namespace"), ), Node( package="controller_manager", @@ -132,7 +132,6 @@ def generate_launch_description() -> LaunchDescription: / "config" / "dual_arm_controllers.yaml", ], - namespace=LaunchConfiguration("namespace"), ), Node( package="controller_manager", @@ -144,7 +143,51 @@ def generate_launch_description() -> LaunchDescription: "joint_state_broadcaster", "joint_trajectory_controller", ], - namespace=LaunchConfiguration("namespace"), + ), + IncludeLaunchDescription( + PathSubstitution( + FindPackageShare("lbr_dual_arm_moveit_config") + / "launch" + / "move_group.launch.py" + ), + launch_arguments=[ + ("lbr_one_x", LaunchConfiguration("lbr_one_x")), + ("lbr_one_y", LaunchConfiguration("lbr_one_y")), + ("lbr_one_z", LaunchConfiguration("lbr_one_z")), + ("lbr_one_roll", LaunchConfiguration("lbr_one_roll")), + ("lbr_one_pitch", LaunchConfiguration("lbr_one_pitch")), + ("lbr_one_yaw", LaunchConfiguration("lbr_one_yaw")), + ("lbr_two_x", LaunchConfiguration("lbr_two_x")), + ("lbr_two_y", LaunchConfiguration("lbr_two_y")), + ("lbr_two_z", LaunchConfiguration("lbr_two_z")), + ("lbr_two_roll", LaunchConfiguration("lbr_two_roll")), + ("lbr_two_pitch", LaunchConfiguration("lbr_two_pitch")), + ("lbr_two_yaw", LaunchConfiguration("lbr_two_yaw")), + ], + condition=IfCondition(LaunchConfiguration("moveit")), + ), + IncludeLaunchDescription( + PathSubstitution( + FindPackageShare("lbr_dual_arm_moveit_config") + / "launch" + / "moveit_rviz.launch.py" + ), + launch_arguments=[ + ("mode", "hardware"), + ("lbr_one_x", LaunchConfiguration("lbr_one_x")), + ("lbr_one_y", LaunchConfiguration("lbr_one_y")), + ("lbr_one_z", LaunchConfiguration("lbr_one_z")), + ("lbr_one_roll", LaunchConfiguration("lbr_one_roll")), + ("lbr_one_pitch", LaunchConfiguration("lbr_one_pitch")), + ("lbr_one_yaw", LaunchConfiguration("lbr_one_yaw")), + ("lbr_two_x", LaunchConfiguration("lbr_two_x")), + ("lbr_two_y", LaunchConfiguration("lbr_two_y")), + ("lbr_two_z", LaunchConfiguration("lbr_two_z")), + ("lbr_two_roll", LaunchConfiguration("lbr_two_roll")), + ("lbr_two_pitch", LaunchConfiguration("lbr_two_pitch")), + ("lbr_two_yaw", LaunchConfiguration("lbr_two_yaw")), + ], + condition=IfCondition(LaunchConfiguration("moveit")), ), ] ) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py index 059950e8..21a0373c 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py @@ -1,5 +1,6 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.substitutions import ( Command, FindExecutable, @@ -14,13 +15,13 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ DeclareLaunchArgument( - name="namespace", - default_value="", - description="Nodes in this launch file will be spawned with this namespace.", + name="moveit", + default_value="false", + description="Launch with MoveIt and RViz.", ), DeclareLaunchArgument( name="lbr_one_x", - default_value="0", + default_value="0.0", description="X position of the first robot.", ), DeclareLaunchArgument( @@ -30,12 +31,12 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_one_z", - default_value="0", + default_value="0.0", description="Z position of the first robot.", ), DeclareLaunchArgument( name="lbr_one_roll", - default_value="0", + default_value="0.0", description="Roll orientation of the first robot.", ), DeclareLaunchArgument( @@ -45,12 +46,12 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_one_yaw", - default_value="0", + default_value="0.0", description="Yaw orientation of the first robot.", ), DeclareLaunchArgument( name="lbr_two_x", - default_value="0", + default_value="0.0", description="X position of the second robot.", ), DeclareLaunchArgument( @@ -60,12 +61,12 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_two_z", - default_value="0", + default_value="0.0", description="Z position of the second robot.", ), DeclareLaunchArgument( name="lbr_two_roll", - default_value="0", + default_value="0.0", description="Roll orientation of the second robot.", ), DeclareLaunchArgument( @@ -75,7 +76,7 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( name="lbr_two_yaw", - default_value="0", + default_value="0.0", description="Yaw orientation of the second robot.", ), Node( @@ -121,7 +122,6 @@ def generate_launch_description() -> LaunchDescription: }, {"use_sim_time": False}, ], - namespace=LaunchConfiguration("namespace"), ), Node( package="controller_manager", @@ -132,7 +132,6 @@ def generate_launch_description() -> LaunchDescription: / "config" / "dual_arm_controllers.yaml", ], - namespace=LaunchConfiguration("namespace"), ), Node( package="controller_manager", @@ -144,7 +143,51 @@ def generate_launch_description() -> LaunchDescription: "joint_state_broadcaster", "joint_trajectory_controller", ], - namespace=LaunchConfiguration("namespace"), + ), + IncludeLaunchDescription( + PathSubstitution( + FindPackageShare("lbr_dual_arm_moveit_config") + / "launch" + / "move_group.launch.py" + ), + launch_arguments=[ + ("mode", "mock"), + ("lbr_one_x", LaunchConfiguration("lbr_one_x")), + ("lbr_one_y", LaunchConfiguration("lbr_one_y")), + ("lbr_one_z", LaunchConfiguration("lbr_one_z")), + ("lbr_one_roll", LaunchConfiguration("lbr_one_roll")), + ("lbr_one_pitch", LaunchConfiguration("lbr_one_pitch")), + ("lbr_one_yaw", LaunchConfiguration("lbr_one_yaw")), + ("lbr_two_x", LaunchConfiguration("lbr_two_x")), + ("lbr_two_y", LaunchConfiguration("lbr_two_y")), + ("lbr_two_z", LaunchConfiguration("lbr_two_z")), + ("lbr_two_roll", LaunchConfiguration("lbr_two_roll")), + ("lbr_two_pitch", LaunchConfiguration("lbr_two_pitch")), + ("lbr_two_yaw", LaunchConfiguration("lbr_two_yaw")), + ], + condition=IfCondition(LaunchConfiguration("moveit")), + ), + IncludeLaunchDescription( + PathSubstitution( + FindPackageShare("lbr_dual_arm_moveit_config") + / "launch" + / "moveit_rviz.launch.py" + ), + launch_arguments=[ + ("lbr_one_x", LaunchConfiguration("lbr_one_x")), + ("lbr_one_y", LaunchConfiguration("lbr_one_y")), + ("lbr_one_z", LaunchConfiguration("lbr_one_z")), + ("lbr_one_roll", LaunchConfiguration("lbr_one_roll")), + ("lbr_one_pitch", LaunchConfiguration("lbr_one_pitch")), + ("lbr_one_yaw", LaunchConfiguration("lbr_one_yaw")), + ("lbr_two_x", LaunchConfiguration("lbr_two_x")), + ("lbr_two_y", LaunchConfiguration("lbr_two_y")), + ("lbr_two_z", LaunchConfiguration("lbr_two_z")), + ("lbr_two_roll", LaunchConfiguration("lbr_two_roll")), + ("lbr_two_pitch", LaunchConfiguration("lbr_two_pitch")), + ("lbr_two_yaw", LaunchConfiguration("lbr_two_yaw")), + ], + condition=IfCondition(LaunchConfiguration("moveit")), ), ] ) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz index 95a7c7c5..51cf5047 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz @@ -163,8 +163,8 @@ Visualization Manager: Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true + Show Robot Collision: true + Show Robot Visual: false Show Trail: false State Display Time: 0.05 s Trail Step Size: 1 @@ -302,7 +302,7 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: true Value: true - Velocity_Scaling_Factor: 0.1 + Velocity_Scaling_Factor: 0.02 Enabled: true Global Options: Background Color: 21; 21; 26 diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py index e07f5e64..1ac2d3e8 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py @@ -11,6 +11,78 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(DeclareLaunchArgument("mode", default_value="mock")) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_x", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_y", + default_value="0.5", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_z", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_roll", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_pitch", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_yaw", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_x", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_y", + default_value="-0.5", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_z", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_roll", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_pitch", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_yaw", + default_value="0.0", + ) + ) moveit_config = ( MoveItConfigsBuilder( @@ -21,10 +93,23 @@ def generate_launch_description(): Path(get_package_share_directory("lbr_dual_arm")) / "urdf" / "lbr_dual_arm.xacro", - mappings={"mode": LaunchConfiguration("mode")}, + mappings={ + "mode": LaunchConfiguration("mode"), + # manual pose setting until load from robot description topic properly supported: https://github.com/moveit/moveit2/issues/2291#issuecomment-4769692822 + "lbr_one_x": LaunchConfiguration("lbr_one_x"), + "lbr_one_y": LaunchConfiguration("lbr_one_y"), + "lbr_one_z": LaunchConfiguration("lbr_one_z"), + "lbr_one_roll": LaunchConfiguration("lbr_one_roll"), + "lbr_one_pitch": LaunchConfiguration("lbr_one_pitch"), + "lbr_one_yaw": LaunchConfiguration("lbr_one_yaw"), + "lbr_two_x": LaunchConfiguration("lbr_two_x"), + "lbr_two_y": LaunchConfiguration("lbr_two_y"), + "lbr_two_z": LaunchConfiguration("lbr_two_z"), + "lbr_two_roll": LaunchConfiguration("lbr_two_roll"), + "lbr_two_pitch": LaunchConfiguration("lbr_two_pitch"), + "lbr_two_yaw": LaunchConfiguration("lbr_two_yaw"), + }, ) .to_moveit_configs() ) - for entity in generate_move_group_launch(moveit_config).entities: - ld.add_action(entity) - return ld + return generate_move_group_launch(moveit_config) diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py index efddb040..4da69fe0 100644 --- a/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py @@ -11,6 +11,78 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(DeclareLaunchArgument("mode", default_value="mock")) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_x", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_y", + default_value="0.5", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_z", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_roll", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_pitch", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_one_yaw", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_x", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_y", + default_value="-0.5", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_z", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_roll", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_pitch", + default_value="0.0", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="lbr_two_yaw", + default_value="0.0", + ) + ) moveit_config = ( MoveItConfigsBuilder( @@ -21,10 +93,23 @@ def generate_launch_description(): Path(get_package_share_directory("lbr_dual_arm")) / "urdf" / "lbr_dual_arm.xacro", - mappings={"mode": LaunchConfiguration("mode")}, + mappings={ + "mode": LaunchConfiguration("mode"), + # manual pose setting until load from robot description topic properly supported: https://github.com/moveit/moveit2/issues/2291#issuecomment-4769692822 + "lbr_one_x": LaunchConfiguration("lbr_one_x"), + "lbr_one_y": LaunchConfiguration("lbr_one_y"), + "lbr_one_z": LaunchConfiguration("lbr_one_z"), + "lbr_one_roll": LaunchConfiguration("lbr_one_roll"), + "lbr_one_pitch": LaunchConfiguration("lbr_one_pitch"), + "lbr_one_yaw": LaunchConfiguration("lbr_one_yaw"), + "lbr_two_x": LaunchConfiguration("lbr_two_x"), + "lbr_two_y": LaunchConfiguration("lbr_two_y"), + "lbr_two_z": LaunchConfiguration("lbr_two_z"), + "lbr_two_roll": LaunchConfiguration("lbr_two_roll"), + "lbr_two_pitch": LaunchConfiguration("lbr_two_pitch"), + "lbr_two_yaw": LaunchConfiguration("lbr_two_yaw"), + }, ) .to_moveit_configs() ) - for entity in generate_moveit_rviz_launch(moveit_config).entities: - ld.add_action(entity) - return ld + return generate_moveit_rviz_launch(moveit_config)