From 63562f071b7e58f8535c3cb8b4cac7bf377acca0 Mon Sep 17 00:00:00 2001 From: lukeschmitt-tr <85308904+lukeschmitt-tr@users.noreply.github.com> Date: Mon, 26 Feb 2024 16:06:15 -0600 Subject: [PATCH] Port interbotix_xsarm_dual package to ROS 2 (#170) * [xsarm_dual] Port xsarm_dual demo to ROS 2 * Fix DeclareLaunchArgument ordering * Fix some DeclareLaunchArgument descriptions * Fix description declaration kwargs, silence RViz * Port over xsarm_dual script * Fix robot names, delay type, add descriptive comments * Clarify comment --- .../interbotix_xsarm_dual/CMakeLists.txt | 50 +++- .../interbotix_xsarm_dual/COLCON_IGNORE | 0 .../interbotix_xsarm_dual/demos/xsarm_dual.py | 116 ++++++++ .../launch/xsarm_dual.launch | 55 ---- .../launch/xsarm_dual.launch.py | 261 ++++++++++++++++++ .../interbotix_xsarm_dual/package.xml | 27 +- .../rviz/xsarm_dual.rviz | 200 +++++--------- .../scripts/xsarm_dual.py | 37 --- 8 files changed, 493 insertions(+), 253 deletions(-) delete mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_dual/COLCON_IGNORE create mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_dual/demos/xsarm_dual.py delete mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch create mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch.py delete mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_dual/scripts/xsarm_dual.py diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/CMakeLists.txt b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/CMakeLists.txt index 5946ebe4..d78f3812 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/CMakeLists.txt +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/CMakeLists.txt @@ -1,20 +1,42 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(interbotix_xsarm_dual) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - interbotix_xsarm_control - interbotix_xs_modules +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +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 Python executables +install( + PROGRAMS + demos/xsarm_dual.py + DESTINATION + lib/${PROJECT_NAME} ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -catkin_package( - CATKIN_DEPENDS interbotix_xsarm_control interbotix_xs_modules +install( + DIRECTORY + config + launch + rviz + DESTINATION + share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/COLCON_IGNORE b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/demos/xsarm_dual.py b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/demos/xsarm_dual.py new file mode 100644 index 00000000..51c3fdad --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/demos/xsarm_dual.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Trossen Robotics +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +import math + +from interbotix_xs_modules.xs_robot.arm import InterbotixManipulatorXS +from rclpy.duration import Duration + +""" +This script is used to make two WidowX-200 arms work in tandem with one another + +To get started, open a terminal and type: + + ros2 launch interbotix_xsarm_dual xsarm_dual.launch.py + +Then change to this directory and type: + + python3 xsarm_dual.py + +Note that the 'robot_name' argument used when instantiating an InterbotixManipulatorXS instance is +the same name as the 'robot_name_X' launch file argument. + +To simultaneously control two arms, we don't wait for movements to finish by setting blocking to +False and delays to 0; opting instead for a function called after each movement command that uses +the node-owning robot's core clock's `sleep_for()` method. This allows us to send commands to both +robots at the same time and wait for their movements to complete after. +""" + +MOVING_TIME_S = 2 +SLEEP_DURATION = Duration(seconds=MOVING_TIME_S) + + +def main(): + # robot_1 is the "node owner", meaning that it controls the state of rclpy. + robot_1 = InterbotixManipulatorXS( + robot_model='wx200', + robot_name='arm_1', + moving_time=MOVING_TIME_S, + node_owner=True, + ) + + # Because robot_1 is the node owner, we set this one's node_owner arg to False. + robot_2 = InterbotixManipulatorXS( + robot_model='wx200', + robot_name='arm_2', + moving_time=MOVING_TIME_S, + node_owner=False, + ) + + # Helper function used to wait for the robots' pre-configured moving times. + def wait() -> None: + """Sleep for SLEEP_DURATION.""" + robot_1.core.get_clock().sleep_for(SLEEP_DURATION) + + robot_1.arm.go_to_home_pose(blocking=False) + robot_2.arm.go_to_home_pose(blocking=False) + wait() + + robot_1.arm.set_ee_pose_components(x=0.3, z=0.2, blocking=False) + robot_2.arm.set_ee_pose_components(x=0.3, z=0.2, blocking=False) + wait() + + robot_1.gripper.release(delay=0.0) + robot_2.gripper.release(delay=0.0) + wait() + + robot_1.arm.set_single_joint_position('waist', -math.pi/4.0, blocking=False) + robot_2.arm.set_single_joint_position('waist', math.pi/4.0, blocking=False) + wait() + + robot_1.gripper.grasp(delay=0.0) + robot_2.gripper.grasp(delay=0.0) + wait() + + robot_1.arm.set_single_joint_position('waist', 0.0, blocking=False) + robot_2.arm.set_single_joint_position('waist', 0.0, blocking=False) + wait() + + robot_1.arm.go_to_sleep_pose(blocking=False) + robot_2.arm.go_to_sleep_pose(blocking=False) + wait() + + # Because robot_1 is the node owner, use it to do the shutdown process + robot_1.shutdown() + + +if __name__ == '__main__': + main() diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch deleted file mode 100644 index c27188de..00000000 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch.py b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch.py new file mode 100644 index 00000000..57f954bd --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/launch/xsarm_dual.launch.py @@ -0,0 +1,261 @@ +# Copyright 2024 Trossen Robotics +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from interbotix_xs_modules.xs_common import ( + get_interbotix_xsarm_models, +) +from interbotix_xs_modules.xs_launch import ( + declare_interbotix_xsarm_robot_description_launch_arguments, +) +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + robot_model_1_launch_arg = LaunchConfiguration('robot_model_1') + robot_name_1_launch_arg = LaunchConfiguration('robot_name_1') + mode_configs_1_launch_arg = LaunchConfiguration('mode_configs_1') + robot_description_1_launch_arg = LaunchConfiguration('robot_description_1') + + robot_model_2_launch_arg = LaunchConfiguration('robot_model_2') + robot_name_2_launch_arg = LaunchConfiguration('robot_name_2') + mode_configs_2_launch_arg = LaunchConfiguration('mode_configs_2') + robot_description_2_launch_arg = LaunchConfiguration('robot_description_2') + + use_sim_launch_arg = LaunchConfiguration('use_sim') + + xsarm_control_1_launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_control'), + 'launch', + 'xsarm_control.launch.py' + ]) + ]), + launch_arguments={ + 'robot_model': robot_model_1_launch_arg, + 'robot_name': robot_name_1_launch_arg, + 'mode_configs': mode_configs_1_launch_arg, + 'use_rviz': 'false', + 'robot_description': robot_description_1_launch_arg, + 'use_sim': use_sim_launch_arg, + }.items(), + ) + + xsarm_control_2_launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_control'), + 'launch', + 'xsarm_control.launch.py' + ]) + ]), + launch_arguments={ + 'robot_model': robot_model_2_launch_arg, + 'robot_name': robot_name_2_launch_arg, + 'mode_configs': mode_configs_2_launch_arg, + 'use_rviz': 'false', + 'robot_description': robot_description_2_launch_arg, + 'use_sim': use_sim_launch_arg, + }.items(), + ) + + robot_1_transform_broadcaster_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='robot_1_transform_broadcaster', + arguments=[ + '0.0', + '-0.25', + '0.0', + '0.0', + '0.0', + '0.0', + '1.0', + '/world', + ( + '/', LaunchConfiguration('robot_name_1'), + '/', LaunchConfiguration('base_link_frame') + ), + ], + ) + + robot_2_transform_broadcaster_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='robot_2_transform_broadcaster', + arguments=[ + '0.0', + '0.25', + '0.0', + '0.0', + '0.0', + '0.0', + '1.0', + '/world', + ( + '/', LaunchConfiguration('robot_name_2'), + '/', LaunchConfiguration('base_link_frame') + ), + ], + output={'both': 'screen'} + ) + + dual_rviz_node = Node( + condition=IfCondition(LaunchConfiguration('use_dual_rviz')), + package='rviz2', + executable='rviz2', + arguments=['-d', LaunchConfiguration('dual_rviz_config')], + output={'both': 'log'}, + ) + + return [ + xsarm_control_1_launch_include, + xsarm_control_2_launch_include, + robot_1_transform_broadcaster_node, + robot_2_transform_broadcaster_node, + dual_rviz_node, + ] + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + 'robot_model_1', + default_value='wx200', + choices=get_interbotix_xsarm_models(), + description='model type of the first Interbotix Arm such as `wx200` or `rx150`.' + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'robot_name_1', + default_value='arm_1', + description='name of the first robot', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'mode_configs_1', + default_value=PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_dual'), + 'config', + 'modes_1.yaml', + ]), + description="the file path to the 'mode config' YAML file for the first arm.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'robot_model_2', + default_value='wx200', + choices=get_interbotix_xsarm_models(), + description='model type of the second Interbotix Arm such as `wx200` or `rx150`.' + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'robot_name_2', + default_value='arm_2', + description='name of the second robot', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'mode_configs_2', + default_value=PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_dual'), + 'config', + 'modes_2.yaml', + ]), + description="the file path to the 'mode config' YAML file for the second arm.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'use_dual_rviz', + default_value='true', + choices=('true', 'false'), + description='launches RViz for the dual-arm configuration if set to `true`.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'dual_rviz_config', + default_value=PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_dual'), + 'rviz', + 'xsarm_dual.rviz', + ]), + description='file path to the dual-arm config file RViz should load.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'use_sim', + default_value='false', + choices=('true', 'false'), + description=( + 'if `true`, the DYNAMIXEL simulator node is run; use RViz to visualize the' + " robot's motion; if `false`, the real DYNAMIXEL driver node is run." + ), + ) + ) + declared_arguments.extend( + declare_interbotix_xsarm_robot_description_launch_arguments( + robot_description_launch_config_name='robot_description_1', + robot_model_launch_config_name='robot_model_1', + robot_name_launch_config_name='robot_name_1', + base_link_frame='base_link', + use_world_frame='false', + ) + ) + declared_arguments.extend( + declare_interbotix_xsarm_robot_description_launch_arguments( + robot_description_launch_config_name='robot_description_2', + robot_model_launch_config_name='robot_model_2', + robot_name_launch_config_name='robot_name_2', + base_link_frame='base_link', + use_world_frame='false', + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/package.xml b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/package.xml index f76475f9..250c4a99 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/package.xml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/package.xml @@ -5,23 +5,22 @@ 0.0.0 The interbotix_xsarm_dual package Luke Schmitt - BSD - + BSD-3-Clause Solomon Wiznitzer - - - catkin - interbotix_xsarm_control - interbotix_xs_modules - interbotix_xsarm_control - interbotix_xs_modules - interbotix_xsarm_control - interbotix_xs_modules + ament_cmake - - - + interbotix_xs_modules + interbotix_xsarm_control + launch_ros + launch + rviz2 + tf2_ros + + ament_lint_auto + ament_lint_common + + ament_cmake diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/rviz/xsarm_dual.rviz b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/rviz/xsarm_dual.rviz index f649e8e8..6aa6d1c6 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/rviz/xsarm_dual.rviz +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/rviz/xsarm_dual.rviz @@ -1,38 +1,20 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.577777803 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Toolbars: - toolButtonStyle: 2 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: interbotix_xs_rviz/Interbotix Control Panel + Name: Interbotix Control Panel + RobotNameSpace: "" Visualization Manager: Class: "" Displays: - Alpha: 0.699999988 Cell Size: 0.25 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 255; 255; 255 Enabled: true Line Style: @@ -49,8 +31,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /arm_1/robot_description Enabled: true Links: All Links Enabled: true @@ -120,15 +110,25 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel 1 - Robot Description: arm_1/robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /arm_2/robot_description Enabled: true Links: All Links Enabled: true @@ -198,126 +198,60 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel 2 - Robot Description: arm_2/robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - arm_1/base_link: - Value: true - arm_1/ee_arm_link: - Value: true - arm_1/ee_gripper_link: - Value: true - arm_1/fingers_link: - Value: true - arm_1/forearm_link: - Value: true - arm_1/gripper_bar_link: - Value: true - arm_1/gripper_link: - Value: true - arm_1/gripper_prop_link: - Value: true - arm_1/left_finger_link: - Value: true - arm_1/right_finger_link: - Value: true - arm_1/shoulder_link: - Value: true - arm_1/upper_arm_link: - Value: true - arm_1/wrist_link: - Value: true - arm_2/base_link: - Value: true - arm_2/base_link_2: - Value: true - arm_2/ee_arm_link: - Value: true - arm_2/ee_gripper_link: - Value: true - arm_2/fingers_link: - Value: true - arm_2/forearm_link: - Value: true - arm_2/gripper_bar_link: - Value: true - arm_2/gripper_link: - Value: true - arm_2/gripper_prop_link: - Value: true - arm_2/left_finger_link: - Value: true - arm_2/right_finger_link: - Value: true - arm_2/shoulder_link: - Value: true - arm_2/upper_arm_link: - Value: true - arm_2/wrist_link: - Value: true - world: - Value: true - Marker Scale: 0.150000006 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - arm_1/base_link: - arm_1/shoulder_link: - arm_1/upper_arm_link: - arm_1/forearm_link: - arm_1/wrist_link: - arm_1/gripper_link: - arm_1/ee_arm_link: - arm_1/gripper_bar_link: - arm_1/fingers_link: - arm_1/ee_gripper_link: - {} - arm_1/left_finger_link: - {} - arm_1/right_finger_link: - {} - arm_1/gripper_prop_link: - {} - arm_2/base_link_2: - {} - Update Interval: 0 - Value: true Enabled: true Global Options: Background Color: 0; 176; 240 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 1.5 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 @@ -343,8 +277,8 @@ Window Geometry: collapsed: false Height: 1056 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/scripts/xsarm_dual.py b/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/scripts/xsarm_dual.py deleted file mode 100644 index 311d73f1..00000000 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/scripts/xsarm_dual.py +++ /dev/null @@ -1,37 +0,0 @@ -import math -import rospy -from threading import Thread -from interbotix_xs_modules.arm import InterbotixManipulatorXS - -# This script is used to make two WidowX200 arms work in tandem with one another -# -# To get started, open a terminal and type 'roslaunch interbotix_xsarm_dual xsarm_dual.launch' -# Then change to this directory and type 'python xsarm_dual.py' -# Note that the 'robot_name' argument used when instantiating an InterbotixManipulatorXS instance -# is the same name as the 'robot_name_X' launch file argument - -def robot_1(): - robot_1 = InterbotixManipulatorXS(robot_model="wx200", robot_name="arm_1", moving_time=1.0, gripper_pressure=1.0, init_node=False) - robot_1.arm.set_ee_pose_components(x=0.3, z=0.2) - robot_1.gripper.open(delay=0.05) - robot_1.arm.set_single_joint_position("waist", math.pi/4.0) - robot_1.gripper.close(delay=0.05) - robot_1.arm.set_single_joint_position("waist", 0) - robot_1.arm.go_to_sleep_pose() - -def robot_2(): - robot_2 = InterbotixManipulatorXS(robot_model="wx200", robot_name="arm_2", moving_time=1.0, gripper_pressure=1.0, init_node=False) - robot_2.arm.set_ee_pose_components(x=0.3, z=0.2) - robot_2.gripper.open(delay=0.05) - robot_2.arm.set_single_joint_position("waist", -math.pi/4.0) - robot_2.gripper.close(delay=0.05) - robot_2.arm.set_single_joint_position("waist", 0) - robot_2.arm.go_to_sleep_pose() - -def main(): - rospy.init_node("xsarm_dual") - Thread(target=robot_1).start() - Thread(target=robot_2).start() - -if __name__=='__main__': - main()