diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f286706e..8b26cb8f 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +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``: 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: @@ -38,6 +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/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 8a8b431f..59ca6b57 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -32,6 +32,10 @@ MoveIt lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst> lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.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/CMakeLists.txt b/lbr_demos/lbr_dual_arm/lbr_dual_arm/CMakeLists.txt new file mode 100644 index 00000000..41ae0a97 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.22) +project(lbr_dual_arm) + +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 config launch urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml new file mode 100644 index 00000000..e1a949e9 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/config/dual_arm_controllers.yaml @@ -0,0 +1,41 @@ +# 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 + + # ROS 2 control controllers + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +# 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 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..b072dc31 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/doc/lbr_dual_arm.rst @@ -0,0 +1,52 @@ +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 +----------- +Open a terminal, then run: + +.. code-block:: bash + + 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). + +.. note:: + To configure position / orientation, launch with ``lbr_one_x:=`` etc. (list all arguments via ``ros2 launch lbr_dual_arm mock.launch.py -s``) + +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..91883a91 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/hardware.launch.py @@ -0,0 +1,193 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +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="moveit", + default_value="false", + description="Launch with MoveIt and RViz.", + ), + DeclareLaunchArgument( + name="lbr_one_x", + default_value="0.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.0", + description="Z position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_roll", + default_value="0.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.0", + description="Yaw orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_two_x", + default_value="0.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.0", + description="Z position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_roll", + default_value="0.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.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}, + ], + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathSubstitution(FindPackageShare("lbr_dual_arm")) + / "config" + / "dual_arm_controllers.yaml", + ], + ), + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + "joint_trajectory_controller", + ], + ), + 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 new file mode 100644 index 00000000..21a0373c --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/launch/mock.launch.py @@ -0,0 +1,193 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +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="moveit", + default_value="false", + description="Launch with MoveIt and RViz.", + ), + DeclareLaunchArgument( + name="lbr_one_x", + default_value="0.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.0", + description="Z position of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_one_roll", + default_value="0.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.0", + description="Yaw orientation of the first robot.", + ), + DeclareLaunchArgument( + name="lbr_two_x", + default_value="0.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.0", + description="Z position of the second robot.", + ), + DeclareLaunchArgument( + name="lbr_two_roll", + default_value="0.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.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}, + ], + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathSubstitution(FindPackageShare("lbr_dual_arm")) + / "config" + / "dual_arm_controllers.yaml", + ], + ), + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + "joint_trajectory_controller", + ], + ), + 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/package.xml b/lbr_demos/lbr_dual_arm/lbr_dual_arm/package.xml new file mode 100644 index 00000000..101a72f0 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm/package.xml @@ -0,0 +1,28 @@ + + + + lbr_dual_arm + 2.4.3 + Dual-arm integration demo. + kenichi-maeda + mhubii + Apache-2.0 + + ament_cmake + + controller_manager + joint_state_broadcaster + joint_trajectory_controller + lbr_ros2_control + lbr_dual_arm_moveit_config + lbr_iiwa7_r800_description + robot_state_publisher + xacro + + ament_lint_auto + ament_lint_common + + + 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_moveit_config/.setup_assistant b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/.setup_assistant new file mode 100644 index 00000000..a49dc9ed --- /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 + relative_path: urdf/lbr_dual_arm.xacro + srdf: + relative_path: config/lbr_dual_arm.srdf + package_settings: + author_name: kenichi-maeda + author_email: kenichi.maeda121@gmail.com + 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..51cf5047 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/config/moveit.rviz @@ -0,0 +1,361 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 329 + - 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: "" + 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 + 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 + Show Robot Collision: true + Show Robot Visual: false + 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: both_arms + 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 + 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 + Value: true + Velocity_Scaling_Factor: 0.02 + Enabled: true + Global Options: + Background Color: 21; 21; 26 + Fixed Frame: world + 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.449727773666382 + 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 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4753984808921814 + Target Frame: base_link + Value: Orbit (rviz_default_plugins) + Yaw: 3.1254067420959473 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1011 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + 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/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..1ac2d3e8 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/move_group.launch.py @@ -0,0 +1,115 @@ +from pathlib import Path + +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")) + 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( + "lbr_dual_arm", + package_name="lbr_dual_arm_moveit_config", + ) + .robot_description( + Path(get_package_share_directory("lbr_dual_arm")) + / "urdf" + / "lbr_dual_arm.xacro", + 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() + ) + 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 new file mode 100644 index 00000000..4da69fe0 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,115 @@ +from pathlib import Path + +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")) + 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( + "lbr_dual_arm", + package_name="lbr_dual_arm_moveit_config", + ) + .robot_description( + Path(get_package_share_directory("lbr_dual_arm")) + / "urdf" + / "lbr_dual_arm.xacro", + 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() + ) + return generate_moveit_rviz_launch(moveit_config) 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..645dffd4 --- /dev/null +++ b/lbr_demos/lbr_dual_arm/lbr_dual_arm_moveit_config/package.xml @@ -0,0 +1,39 @@ + + + + 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_iiwa7_r800_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 + + \ No newline at end of file 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 @@ - +