diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md index c01256b..228597d 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -1,13 +1,16 @@ # interbotix_xsarm_gravity_compensation ## Overview + This package demos the interbotix_gravity_compensation package on an Interbotix arm. As of now, the supported arms include: WidowX-250 6DOF and ALOHA WidowX-250 6DOF. ## Configuration + Please refer to the documentations for [`mode_configs`](https://docs.trossenrobotics.com/interbotix_xsarms_docs/ros_interface/ros2/config.html#mode-configs) and [`motor_specs`](https://docs.trossenrobotics.com/interbotix_xsarms_docs/ros2_packages/gravity_compensation.html#configuration) for details. ## Usage + Run the following launch command where `robot_model` is a mandatory choice between `aloha_wx250s` and `wx250s`, `robot_name` defaults to be the same as `robot_model` but can be anything, `motor_specs` defaults to `/config/motor_specs_.yaml`, and `mode_configs` defaults to `/config/mode_configs_.yaml`: ``` ros2 launch interbotix_xsarm_gravity_compensation interbotix_gravity_compensation.launch.py robot_model:=xxx [robot_name:=xxx] [motor_specs:=xxx] @@ -22,6 +25,9 @@ ros2 service call //gravity_compensation_enable std_srvs/srv/SetBoo The arm will hold itself against gravity and can be moved freely when the gravity compensation is enabled. It will lock in its current position when the gravity compensation is disabled. -**WARNING: the arm WILL torque off and drop for a short period of time while enabling/disabling. Please make sure it is in a resting position or manually held.** -**WARNING: the joints not supporting current control WILL torque off. Please make sure to use arm with at least the first three joints supporting current control, e.g., RX, WX, VX series.** +> [!WARNING] +> WARNING: the arm WILL torque off and drop for a short period of time while enabling/disabling. Please make sure it is in a resting position or manually held. + +> [!WARNING] +> WARNING: the joints not supporting current control WILL torque off. Please make sure to use arm with at least the first three joints supporting current control, e.g., RX, WX, VX series. diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.yaml similarity index 94% rename from interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_aloha_wx250s.yaml rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.yaml index 1dfb3c6..66f6312 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.yaml @@ -2,7 +2,7 @@ # It should be placed in the 'config' directory of any ROS package that builds ontop of the interbotix_xs_sdk. # Example values are shown as well. All parameters are mandatory unless stated otherwise -port: /dev/ttyDXL_awx250s # Optional; specifies the USB port that connects to the U2D2; +port: /dev/ttyDXL_awx250s # Optional; specifies the USB port that connects to the U2D2; # if specified, it overwrites the port in the 'motor_configs' yaml file. # if the 'port' parameter and its value are left out, the port in the 'motor_configs' yaml file is used. # if the 'port' parameter is specified but the value is not, it defaults to '/dev/ttyDXL' diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml deleted file mode 100644 index 9cd53ae..0000000 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml +++ /dev/null @@ -1,21 +0,0 @@ -# This file is used to customize the initial operating modes of all joint groups and individual joints in a robot. -# It should be placed in the 'config' directory of any ROS package that builds ontop of the interbotix_xs_sdk. -# Example values are shown as well. All parameters are mandatory unless stated otherwise - -port: /dev/ttyDXL_wx250s # Optional; specifies the USB port that connects to the U2D2; - # if specified, it overwrites the port in the 'motor_configs' yaml file. - # if the 'port' parameter and its value are left out, the port in the 'motor_configs' yaml file is used. - # if the 'port' parameter is specified but the value is not, it defaults to '/dev/ttyDXL' - -groups: # Optional; specify initial Operating Modes for all joint groups (as detailed in the 'motor_configs' yaml file under 'groups') - arm: # Example joint group name - operating_mode: position # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for the seven modes; defaults to 'position' - profile_type: time # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for the two profile types; defaults to 'velocity' - profile_velocity: 2000 # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for a description; defaults to '0' - profile_acceleration: 1000 # Optional; refer to the 'OperatingModes' Service description file in the 'srv' directory for a description; defaults to '0' - torque_enable: true # Optional; whether the motors in this group should be torqued on or off by default; defaults to 'true' - -singles: # Optional; specify the Operating Mode for individual joints (any joint specified in the 'motor_configs' yaml file); parameters follow the same structure as above - gripper: - operating_mode: pwm - torque_enable: true diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py index adb5a52..a2aecb9 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( 'robot_model', - choices=get_interbotix_xsarm_models(), + choices=('wx250s', 'aloha_wx250s'), description='model type of the Interbotix Arm such as `wx200` or `rx150`.' ) ) @@ -104,13 +104,12 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( 'motor_specs', - default_value=[ - PathJoinSubstitution([ - FindPackageShare('interbotix_xsarm_gravity_compensation'), - 'config']), - "/motor_specs_", + default_value=[PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_gravity_compensation'), + 'config', + "motor_specs_"]), LaunchConfiguration('robot_model'), - ".yaml" + '.yaml' ], description="the file path to the 'motor specs' YAML file.", ) @@ -121,10 +120,8 @@ def generate_launch_description(): default_value=[ PathJoinSubstitution([ FindPackageShare('interbotix_xsarm_gravity_compensation'), - 'config']), - "/mode_configs_", - LaunchConfiguration('robot_model'), - ".yaml" + 'config', + 'mode_configs.yaml']) ], description="the file path to the 'mode configs' YAML file.", ) diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml index 4c3b451..8f7a2ef 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml @@ -10,6 +10,7 @@ ament_cmake interbotix_gravity_compensation + interbotix_xsarm_control ament_lint_auto ament_lint_common