From f55147b7f3632d17375f5e1f3475c935a8433e96 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Wed, 11 Sep 2024 17:03:20 -0500 Subject: [PATCH 01/17] added demo of gravity compensation on xsarm --- .../CMakeLists.txt | 27 ++++ .../interbotix_gravity_compensation/README.md | 9 ++ .../config/motor_specs_aloha_wx250s.yaml | 44 +++++++ .../config/motor_specs_wx250s.yaml | 38 ++++++ .../interbotix_gravity_compensation.launch.py | 118 ++++++++++++++++++ .../package.xml | 20 +++ 6 files changed, 256 insertions(+) create mode 100644 interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt create mode 100644 interbotix_ros_xsarms/examples/interbotix_gravity_compensation/README.md create mode 100644 interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml create mode 100644 interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_wx250s.yaml create mode 100644 interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py create mode 100644 interbotix_ros_xsarms/examples/interbotix_gravity_compensation/package.xml diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt new file mode 100644 index 0000000..694f979 --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5) +project(interbotix_gravity_compensation) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + config + launch + 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_gravity_compensation/README.md b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/README.md new file mode 100644 index 0000000..8ef29c7 --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/README.md @@ -0,0 +1,9 @@ +# interbotix_gravity_compensation + +## Overview +This package demos the gravity compensation feature of the Interbotix arms. + +### Gravity Compensation +[![docs](https://docs.trossenrobotics.com/docs_button.svg)](https://docs.trossenrobotics.com/interbotix_xsarms_docs/ros2_packages/gravity_compensation.html) + +As its name suggests, gravity compensation cancels out the effect of the gravity on a system. A toy example is the elevator. The counterweight of the elevator counteracts the gravity applied on the elevator, allowing it to move up and down effortlessly. In our case, we compensate the gravity on a X-Series robotic arm with its joint motors. This feature is useful when we use the arm as a teleoperation remote or when we teach the arm a specific trajectory and don't want to keep holding the arm to avoid it from collapsing. When enabled, this package sets the arm to the current/torque control mode. When a new joint state is received, it solves for the torques required to counteract the gravity. This requires solving a special case of the [inverse dynamics](https://en.wikipedia.org/wiki/Inverse_dynamics) problem where the joint velocities, accelerations, and external forces are 0s. Then, it converts the resulting torques into motor current commands and publishes it to the `//commands/joint_group` topic for the **xs_sdk** node. The inverse dynamics solver is ported from the [Orocos Kinematics and Dynamics Library (KDL)](https://www.orocos.org/kdl.html). Please refer to the solver [doc](https://docs.ros.org/en/indigo/api/orocos_kdl/html/classKDL_1_1ChainIdSolver__RNE.html) page for its derivations and implementation details. \ No newline at end of file diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml new file mode 100644 index 0000000..fec6cd9 --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml @@ -0,0 +1,44 @@ +# Please refer to the leader_motor_specs.yaml file +# for a detailed explanation of the parameters +motor_assist: + all: -1 + waist: 0.5 + shoulder: 0.5 + elbow: 0.5 + forearm_roll: 0.5 + wrist_angle: 0.5 + wrist_rotate: 0.5 + +motor_specs: + waist: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.1 + + shoulder: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.0 + + elbow: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.0 + + forearm_roll: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.1 + + wrist_angle: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.0 + + wrist_rotate: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.1 + +joint_names: + [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_wx250s.yaml new file mode 100644 index 0000000..869e257 --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_wx250s.yaml @@ -0,0 +1,38 @@ +# Please refer to the leader_motor_specs.yaml file +# for a detailed explanation of the parameters +motor_assist: + all: -1 + waist: 0.5 + shoulder: 0.5 + elbow: 0.5 + forearm_roll: 0.5 + wrist_angle: 0.5 + +motor_specs: + waist: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.1 + + shoulder: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.0 + + elbow: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.0 + + forearm_roll: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.1 + + wrist_angle: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.0 + +joint_names: + [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py new file mode 100644 index 0000000..0aea42d --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 typing import List + +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, + OpaqueFunction +) +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 + +from interbotix_xs_modules.xs_common import get_interbotix_xsarm_models + + +def launch_setup(context, *args, **kwargs): + # Define the launch arguments + robot_model_launch_arg = LaunchConfiguration('robot_model') + robot_name_launch_arg = LaunchConfiguration('robot_name') + motor_specs_launch_arg = LaunchConfiguration('motor_specs') + + # Create the gravity compensation node + gravity_compensation_node = Node( + package='interbotix_gravity_compensation', + executable='interbotix_gravity_compensation', + name='interbotix_gravity_compensation', + output='screen', + emulate_tty=True, + parameters=[{'robot_name': robot_name_launch_arg, + 'motor_specs': motor_specs_launch_arg}] + ) + + # include the xsarm_control_launch with arguments + xsarm_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_control'), + 'launch', + 'xsarm_control.launch.py' + ]) + ]), + launch_arguments={ + 'robot_model': robot_model_launch_arg, + 'robot_name': robot_name_launch_arg, + }.items() + ) + + return [gravity_compensation_node, xsarm_control_launch] + + +def generate_launch_description(): + # Create a list of all the launch arguments + declared_arguments: List[DeclareLaunchArgument] = [] + declared_arguments.append( + DeclareLaunchArgument( + 'robot_model', + choices=get_interbotix_xsarm_models(), + description='model type of the Interbotix Arm such as `wx200` or `rx150`.' + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'robot_name', + default_value=LaunchConfiguration('robot_model'), + description=( + 'name of the robot (typically equal to `robot_model`, but could be anything).' + ), + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'motor_specs', + default_value=[ + PathJoinSubstitution([ + FindPackageShare('interbotix_gravity_compensation'), + 'config']), + "/motor_specs_", + LaunchConfiguration('robot_model'), + ".yaml" + ], + description="the file path to the 'motor specs' YAML file.", + ) + ) + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/package.xml b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/package.xml new file mode 100644 index 0000000..4c3b451 --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/package.xml @@ -0,0 +1,20 @@ + + + + interbotix_xsarm_gravity_compensation + 0.0.0 + The demo of the interbotix_gravity_compensation package on xsarm + Luke Schmitt + BSD-3-Clause + Shiming Liang + + ament_cmake + interbotix_gravity_compensation + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From b28807e6a580c51eaec91bc3b4998bf3484994ba Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Thu, 12 Sep 2024 11:43:30 -0500 Subject: [PATCH 02/17] added config and launch files for gravity compensation demo on xsarm --- .../CMakeLists.txt | 2 +- .../README.md | 0 .../config/motor_specs_aloha_wx250s.yaml | 11 +- .../config/motor_specs_wx250s.yaml | 0 .../interbotix_gravity_compensation.launch.py | 2 +- .../package.xml | 0 .../urdf/aloha_wx250s.urdf.xacro | 174 +++++++++++------- 7 files changed, 115 insertions(+), 74 deletions(-) rename interbotix_ros_xsarms/examples/{interbotix_gravity_compensation => interbotix_xsarm_gravity_compensation}/CMakeLists.txt (91%) rename interbotix_ros_xsarms/examples/{interbotix_gravity_compensation => interbotix_xsarm_gravity_compensation}/README.md (100%) rename interbotix_ros_xsarms/examples/{interbotix_gravity_compensation => interbotix_xsarm_gravity_compensation}/config/motor_specs_aloha_wx250s.yaml (54%) rename interbotix_ros_xsarms/examples/{interbotix_gravity_compensation => interbotix_xsarm_gravity_compensation}/config/motor_specs_wx250s.yaml (100%) rename interbotix_ros_xsarms/examples/{interbotix_gravity_compensation => interbotix_xsarm_gravity_compensation}/launch/interbotix_gravity_compensation.launch.py (98%) rename interbotix_ros_xsarms/examples/{interbotix_gravity_compensation => interbotix_xsarm_gravity_compensation}/package.xml (100%) diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/CMakeLists.txt similarity index 91% rename from interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/CMakeLists.txt index 694f979..d459917 100644 --- a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/CMakeLists.txt +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(interbotix_gravity_compensation) +project(interbotix_xsarm_gravity_compensation) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/README.md b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md similarity index 100% rename from interbotix_ros_xsarms/examples/interbotix_gravity_compensation/README.md rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml similarity index 54% rename from interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml index fec6cd9..84acc04 100644 --- a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml @@ -1,7 +1,10 @@ -# Please refer to the leader_motor_specs.yaml file -# for a detailed explanation of the parameters +# Motor Assist: scale the no-load currents which alleviate the effects of friction +# If the values are invalid, they defaults to 0 motor_assist: + # Set 'all' to [0, 1] to scale the no load currents of all joints uniformly + # Or to -1 and use joint specific values all: -1 + # Set the joint specific values to [0, 1] to scale differently for each joint waist: 0.5 shoulder: 0.5 elbow: 0.5 @@ -11,8 +14,12 @@ motor_assist: motor_specs: waist: + # torque constant (Nm/A): how much torque is produced per Amp of current torque_constant: 1.793 + # current unit (A): how much current command is needed to produce 1 Amp of current current_unit: 0.00269 + # no load current (A): the maximum no load current applied when motor_assist == 1 + # It should be as large as possible without the joint accelerating by itself no_load_current: 0.1 shoulder: diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml similarity index 100% rename from interbotix_ros_xsarms/examples/interbotix_gravity_compensation/config/motor_specs_wx250s.yaml rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py similarity index 98% rename from interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py index 0aea42d..e30a5a5 100644 --- a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/launch/interbotix_gravity_compensation.launch.py +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py @@ -106,7 +106,7 @@ def generate_launch_description(): 'motor_specs', default_value=[ PathJoinSubstitution([ - FindPackageShare('interbotix_gravity_compensation'), + FindPackageShare('interbotix_xsarm_gravity_compensation'), 'config']), "/motor_specs_", LaunchConfiguration('robot_model'), diff --git a/interbotix_ros_xsarms/examples/interbotix_gravity_compensation/package.xml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml similarity index 100% rename from interbotix_ros_xsarms/examples/interbotix_gravity_compensation/package.xml rename to interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml diff --git a/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro b/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro index edf87e6..17a681c 100644 --- a/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro +++ b/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro @@ -12,6 +12,7 @@ + @@ -210,16 +211,16 @@ - + rpy="0 0 0" + xyz="0.0153860000 0.0000000000 0.1851100000"/> + + ixx="0.0043745412" + iyy="0.0042527386" + izz="0.0004827537" + ixy="0.0000000002" + ixz="-0.0004799594" + iyz="-0.0000000015"/> @@ -267,15 +268,15 @@ - + xyz="0.0835970000 0.0000000000 0.0000000000"/> + + ixx="0.0000279716" + iyy="0.0007756088" + izz="0.0007799547" + ixy="-0.0000000002" + ixz="0.0000000002" + iyz="-0.0000000008"/> @@ -322,16 +323,16 @@ - + rpy="0 0 0" + xyz="0.0335820000 0.0047260000 0.0000000000"/> + + ixx="0.0000741889" + iyy="0.0002100083" + izz="0.0002256883" + ixy="0.0000194025" + ixz="0.0000000000" + iyz="0.0000000004"/> @@ -378,16 +379,16 @@ - + rpy="0 0 0" + xyz="0.0484000000 0.0000020000 0.0081230000"/> + + ixx="0.0000551559" + iyy="0.0000691991" + izz="0.0000656414" + ixy="0.0000000002" + ixz="0.0000037652" + iyz="-0.0000000042"/> @@ -438,19 +439,52 @@ - + rpy="0 0 0" + xyz="0.0194110000 0.0000000000 0.0050100000"/> + + ixx="0.0000324528" + iyy="0.0000357525" + izz="0.0000270800" + ixy="-0.0000000001" + ixz="-0.0000023449" + iyz="0.0000000000"/> + + + + + + + + + + + + + + + + + + + + + - + rpy="0 0 0" + xyz="0.0117450000 0.0000000000 0.0018310000"/> + + ixx="0.0002705379" + iyy="0.0000371885" + izz="0.0002579847" + ixy="0.0000000015" + ixz="0.0000026430" + iyz="0.0000000000"/> @@ -658,16 +692,16 @@ - + rpy="0 0 0" + xyz="0.0042060000 0.0100000000 -0.0080300000"/> + + ixx="0.0000164667" + iyy="0.0000169266" + izz="0.0000038108" + ixy="-0.0000006813" + ixz="0.0000001193" + iyz="-0.0000001691"/> @@ -720,16 +754,16 @@ - + rpy="0 0 0" + xyz="0.0039690000 -0.0100000000 -0.0065280000"/> + + izz="0.0000181304" + ixy="0.0000039562" + ixz="0.0000005777" + iyz="-0.0000006879"/> From 1442da54d45d180b3cff97527b4f529b6d231a3b Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Thu, 12 Sep 2024 17:46:44 -0500 Subject: [PATCH 03/17] updated doc for gravity compensation --- .../README.md | 85 +++++++++++++++++-- .../config/motor_specs_aloha_wx250s.yaml | 2 + 2 files changed, 82 insertions(+), 5 deletions(-) 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 8ef29c7..0fb8a32 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -1,9 +1,84 @@ -# interbotix_gravity_compensation +# interbotix_xsarm_gravity_compensation ## Overview -This package demos the gravity compensation feature of the Interbotix arms. +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. -### Gravity Compensation -[![docs](https://docs.trossenrobotics.com/docs_button.svg)](https://docs.trossenrobotics.com/interbotix_xsarms_docs/ros2_packages/gravity_compensation.html) +## 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, and `motor_specs` defaults to `/config/motor_specs_.yaml`: +``` +ros2 launch interbotix_xsarm_gravity_compensation interbotix_gravity_compensation.launch.py robot_model:=xxx [robot_name:=xxx] [motor_specs:=xxx] +``` +It runs the `interbotix_gravity_compensation` node and launches the xsarm_control script to bring up the arm. -As its name suggests, gravity compensation cancels out the effect of the gravity on a system. A toy example is the elevator. The counterweight of the elevator counteracts the gravity applied on the elevator, allowing it to move up and down effortlessly. In our case, we compensate the gravity on a X-Series robotic arm with its joint motors. This feature is useful when we use the arm as a teleoperation remote or when we teach the arm a specific trajectory and don't want to keep holding the arm to avoid it from collapsing. When enabled, this package sets the arm to the current/torque control mode. When a new joint state is received, it solves for the torques required to counteract the gravity. This requires solving a special case of the [inverse dynamics](https://en.wikipedia.org/wiki/Inverse_dynamics) problem where the joint velocities, accelerations, and external forces are 0s. Then, it converts the resulting torques into motor current commands and publishes it to the `//commands/joint_group` topic for the **xs_sdk** node. The inverse dynamics solver is ported from the [Orocos Kinematics and Dynamics Library (KDL)](https://www.orocos.org/kdl.html). Please refer to the solver [doc](https://docs.ros.org/en/indigo/api/orocos_kdl/html/classKDL_1_1ChainIdSolver__RNE.html) page for its derivations and implementation details. \ No newline at end of file +Then, enable/disable the gravity compensation with the following service call: +``` +ros2 service call /wx250s/gravity_compensation_enable std_srvs/srv/SetBool 'data: [true/false]' +``` + +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.** + +## Configuration +The `motor_specs.yaml` hosts the motor specifications used in the node and provides knobs for motor assistance against joint frictions. A template file is given below: +``` +# Motor Assist: scale the no-load currents which alleviate the effects of friction +# If the values are invalid, they defaults to 0 +motor_assist: + # Set 'all' to [0, 1] to scale the no load currents of all joints uniformly + # Or to -1 and use joint specific values + all: -1 + # Set the joint specific values to [0, 1] to scale differently for each joint + waist: 0.5 + shoulder: 0.5 + elbow: 0.5 + forearm_roll: 0.5 + wrist_angle: 0.5 + wrist_rotate: 0.5 + +motor_specs: + waist: + # torque constant (Nm/A): how much torque is produced per Amp of current + torque_constant: 1.793 + # current unit (A): how much current command is needed to produce 1 Amp of current + current_unit: 0.00269 + # no load current (A): the maximum no load current applied when motor_assist == 1 + # It should be as large as possible without the joint accelerating by itself + no_load_current: 0.1 + + shoulder: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.0 + + elbow: + torque_constant: 1.793 + current_unit: 0.00269 + no_load_current: 0.0 + + forearm_roll: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.1 + + wrist_angle: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.0 + + wrist_rotate: + torque_constant: 0.897 + current_unit: 0.00269 + no_load_current: 0.1 + +# Joints specified here but not in motor_assist or motor_specs +# do not support the current control mode +joint_names: + [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] + +``` + +## Running on Other Arms +As of now, only the WidowX-250 6DOF and ALOHA WidowX-250 6DOF arms are tested and verified to work with the gravity compensation package. However, this package should be compatible with all RX, WX, VX series arms. To run the demo on those arms, one need to put together the corresponding `motor_specs_xxx.yaml`. The torque constants and current units can be looked up from the ROBOTIS e-manual ([example](https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/)) according to the arm specification ([example](https://docs.trossenrobotics.com/interbotix_xsarms_docs/specifications/awx250s.html)). One may also need to adjust the URDF inertia entries in the [interbotix_xsarm_description](https://github.com/Interbotix/interbotix_ros_manipulators/tree/main/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf) package to provide the node with an accurate mass distribution information. \ No newline at end of file diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml index 84acc04..d02733f 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml @@ -47,5 +47,7 @@ motor_specs: current_unit: 0.00269 no_load_current: 0.1 +# Joints specified here but not in motor_assist or motor_specs +# do not support the current control mode joint_names: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] From 5eba315e0ad819caa564b8a17f0d385ae196c0d8 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Fri, 13 Sep 2024 13:51:41 -0500 Subject: [PATCH 04/17] properly formatted --- .../README.md | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) 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 0fb8a32..bbead9e 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -1,7 +1,8 @@ # 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. +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. ## 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, and `motor_specs` defaults to `/config/motor_specs_.yaml`: @@ -15,14 +16,16 @@ Then, enable/disable the gravity compensation with the following service call: ros2 service call /wx250s/gravity_compensation_enable std_srvs/srv/SetBool 'data: [true/false]' ``` -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. +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.** ## Configuration -The `motor_specs.yaml` hosts the motor specifications used in the node and provides knobs for motor assistance against joint frictions. A template file is given below: +The `motor_specs.yaml` hosts the motor specifications used in the node and provides knobs for motor assistance against joint frictions. +A template file is given below: ``` # Motor Assist: scale the no-load currents which alleviate the effects of friction # If the values are invalid, they defaults to 0 @@ -81,4 +84,8 @@ joint_names: ``` ## Running on Other Arms -As of now, only the WidowX-250 6DOF and ALOHA WidowX-250 6DOF arms are tested and verified to work with the gravity compensation package. However, this package should be compatible with all RX, WX, VX series arms. To run the demo on those arms, one need to put together the corresponding `motor_specs_xxx.yaml`. The torque constants and current units can be looked up from the ROBOTIS e-manual ([example](https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/)) according to the arm specification ([example](https://docs.trossenrobotics.com/interbotix_xsarms_docs/specifications/awx250s.html)). One may also need to adjust the URDF inertia entries in the [interbotix_xsarm_description](https://github.com/Interbotix/interbotix_ros_manipulators/tree/main/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf) package to provide the node with an accurate mass distribution information. \ No newline at end of file +As of now, only the WidowX-250 6DOF and ALOHA WidowX-250 6DOF arms are tested and verified to work with the gravity compensation package. +However, this package should be compatible with all RX, WX, VX series arms. +To run the demo on those arms, one need to put together the corresponding `motor_specs_xxx.yaml`. +The torque constants and current units can be looked up from the ROBOTIS e-manual ([example](https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/)) according to the arm specification ([example](https://docs.trossenrobotics.com/interbotix_xsarms_docs/specifications/awx250s.html)). +One may also need to adjust the URDF inertia entries in the [interbotix_xsarm_description](https://github.com/Interbotix/interbotix_ros_manipulators/tree/main/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf) package to provide the node with an accurate mass distribution information. \ No newline at end of file From 652fa75535c39e72aca4b5770bfa44aa80e0c1e7 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Fri, 13 Sep 2024 14:32:06 -0500 Subject: [PATCH 05/17] move the config doc to interbotix_gravity_compensation --- .../README.md | 67 ------------------- 1 file changed, 67 deletions(-) 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 bbead9e..48951d7 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -22,70 +22,3 @@ 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.** - -## Configuration -The `motor_specs.yaml` hosts the motor specifications used in the node and provides knobs for motor assistance against joint frictions. -A template file is given below: -``` -# Motor Assist: scale the no-load currents which alleviate the effects of friction -# If the values are invalid, they defaults to 0 -motor_assist: - # Set 'all' to [0, 1] to scale the no load currents of all joints uniformly - # Or to -1 and use joint specific values - all: -1 - # Set the joint specific values to [0, 1] to scale differently for each joint - waist: 0.5 - shoulder: 0.5 - elbow: 0.5 - forearm_roll: 0.5 - wrist_angle: 0.5 - wrist_rotate: 0.5 - -motor_specs: - waist: - # torque constant (Nm/A): how much torque is produced per Amp of current - torque_constant: 1.793 - # current unit (A): how much current command is needed to produce 1 Amp of current - current_unit: 0.00269 - # no load current (A): the maximum no load current applied when motor_assist == 1 - # It should be as large as possible without the joint accelerating by itself - no_load_current: 0.1 - - shoulder: - torque_constant: 1.793 - current_unit: 0.00269 - no_load_current: 0.0 - - elbow: - torque_constant: 1.793 - current_unit: 0.00269 - no_load_current: 0.0 - - forearm_roll: - torque_constant: 0.897 - current_unit: 0.00269 - no_load_current: 0.1 - - wrist_angle: - torque_constant: 0.897 - current_unit: 0.00269 - no_load_current: 0.0 - - wrist_rotate: - torque_constant: 0.897 - current_unit: 0.00269 - no_load_current: 0.1 - -# Joints specified here but not in motor_assist or motor_specs -# do not support the current control mode -joint_names: - [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] - -``` - -## Running on Other Arms -As of now, only the WidowX-250 6DOF and ALOHA WidowX-250 6DOF arms are tested and verified to work with the gravity compensation package. -However, this package should be compatible with all RX, WX, VX series arms. -To run the demo on those arms, one need to put together the corresponding `motor_specs_xxx.yaml`. -The torque constants and current units can be looked up from the ROBOTIS e-manual ([example](https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/)) according to the arm specification ([example](https://docs.trossenrobotics.com/interbotix_xsarms_docs/specifications/awx250s.html)). -One may also need to adjust the URDF inertia entries in the [interbotix_xsarm_description](https://github.com/Interbotix/interbotix_ros_manipulators/tree/main/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf) package to provide the node with an accurate mass distribution information. \ No newline at end of file From 9e631f5199a76d7e0422677947504b910eca04de Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Fri, 13 Sep 2024 17:44:10 -0500 Subject: [PATCH 06/17] use namespace in the launch file --- .../launch/interbotix_gravity_compensation.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 e30a5a5..7284449 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 @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Trossen Robotics +# 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: @@ -57,11 +57,11 @@ def launch_setup(context, *args, **kwargs): gravity_compensation_node = Node( package='interbotix_gravity_compensation', executable='interbotix_gravity_compensation', - name='interbotix_gravity_compensation', + name='gravity_compensation', + namespace=robot_name_launch_arg, output='screen', emulate_tty=True, - parameters=[{'robot_name': robot_name_launch_arg, - 'motor_specs': motor_specs_launch_arg}] + parameters=[{'motor_specs': motor_specs_launch_arg}] ) # include the xsarm_control_launch with arguments From 033288b4f9b58c70b9b4eb96af46262270ad10f9 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Mon, 16 Sep 2024 14:08:42 -0500 Subject: [PATCH 07/17] fixed typo in the grav comp yaml files, pointed to the closer example config --- .../config/motor_specs_aloha_wx250s.yaml | 2 +- .../config/motor_specs_wx250s.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml index d02733f..8468f22 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml @@ -1,5 +1,5 @@ # Motor Assist: scale the no-load currents which alleviate the effects of friction -# If the values are invalid, they defaults to 0 +# If the values are invalid, they default to 0 motor_assist: # Set 'all' to [0, 1] to scale the no load currents of all joints uniformly # Or to -1 and use joint specific values diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml index 869e257..d4f9440 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml @@ -1,4 +1,4 @@ -# Please refer to the leader_motor_specs.yaml file +# Please refer to the motor_specs_aloha_wx250s.yaml file # for a detailed explanation of the parameters motor_assist: all: -1 From 999f8bf238c6ad39ce3a526190c95fb0bc89ee1b Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Wed, 18 Sep 2024 14:59:22 -0500 Subject: [PATCH 08/17] two arms can run at the same time, added parameters for friction modelling --- .../config/mode_configs_aloha_wx250s.yaml | 21 +++++++++++++++++++ .../config/mode_configs_wx250s.yaml | 21 +++++++++++++++++++ .../config/motor_specs_aloha_wx250s.yaml | 10 ++++++++- .../config/motor_specs_wx250s.yaml | 5 +++++ .../interbotix_gravity_compensation.launch.py | 14 +++++++++++++ 5 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_aloha_wx250s.yaml create mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml 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_aloha_wx250s.yaml new file mode 100644 index 0000000..1dfb3c6 --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_aloha_wx250s.yaml @@ -0,0 +1,21 @@ +# 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_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' + +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/config/mode_configs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml new file mode 100644 index 0000000..9cd53ae --- /dev/null +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml @@ -0,0 +1,21 @@ +# 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/config/motor_specs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml index 8468f22..d2fe606 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml @@ -20,32 +20,40 @@ motor_specs: current_unit: 0.00269 # no load current (A): the maximum no load current applied when motor_assist == 1 # It should be as large as possible without the joint accelerating by itself - no_load_current: 0.1 + no_load_current: 0.0 + # coefficient of friction (Nm/Nm): the torque needed to overcome Coulomb friction + # It should be as large as possible without the joint accelerating by itself + friction_coefficient: 0.0 shoulder: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 + friction_coefficient: 0.2 elbow: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 + friction_coefficient: 0.2 forearm_roll: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.1 + friction_coefficient: 0.0 wrist_angle: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.0 + friction_coefficient: 0.0 wrist_rotate: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.1 + friction_coefficient: 0.0 # Joints specified here but not in motor_assist or motor_specs # do not support the current control mode diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml index d4f9440..3fec6b3 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml @@ -13,26 +13,31 @@ motor_specs: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.1 + friction_coefficient: 0.0 shoulder: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 + friction_coefficient: 0.0 elbow: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 + friction_coefficient: 0.0 forearm_roll: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.1 + friction_coefficient: 0.0 wrist_angle: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.0 + friction_coefficient: 0.0 joint_names: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] 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 7284449..adb5a52 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 @@ -115,4 +115,18 @@ def generate_launch_description(): description="the file path to the 'motor specs' YAML file.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + 'mode_configs', + default_value=[ + PathJoinSubstitution([ + FindPackageShare('interbotix_xsarm_gravity_compensation'), + 'config']), + "/mode_configs_", + LaunchConfiguration('robot_model'), + ".yaml" + ], + description="the file path to the 'mode configs' YAML file.", + ) + ) return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) From 2bf7ccc2f887b74c57724a2cd3fe20bf8bdf840f Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Wed, 18 Sep 2024 16:35:55 -0500 Subject: [PATCH 09/17] corrected some entries of the URDF for the aloha wx250s --- .../urdf/aloha_wx250s.urdf.xacro | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro b/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro index 17a681c..901bff8 100644 --- a/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro +++ b/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro @@ -693,7 +693,7 @@ + xyz="0.0042060000 0.0000000000 -0.0080300000"/> + xyz="0.0039690000 0.0000000000 -0.0065280000"/> From 6f8102512361a8d66dddbfb090d582674a506439 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Mon, 23 Sep 2024 14:30:02 -0500 Subject: [PATCH 10/17] updated the config file for more sophisticated friction compensation, updated the corresponding contents at the readme file, updated the URDF file for the aloha wx250s. TO DO: update the link to the documentation of the motor specs configuration. --- .../README.md | 7 ++- .../config/motor_specs_aloha_wx250s.yaml | 51 ++++++++++++------- .../config/motor_specs_wx250s.yaml | 25 ++++++--- .../urdf/aloha_wx250s.urdf.xacro | 18 +++---- 4 files changed, 65 insertions(+), 36 deletions(-) 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 48951d7..6ac164c 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -4,8 +4,11 @@ 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#motor-configs) and [`motor_specs`](TO DO: Add link to the doc) 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, and `motor_specs` defaults to `/config/motor_specs_.yaml`: +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] ``` @@ -13,7 +16,7 @@ It runs the `interbotix_gravity_compensation` node and launches the xsarm_contro Then, enable/disable the gravity compensation with the following service call: ``` -ros2 service call /wx250s/gravity_compensation_enable std_srvs/srv/SetBool 'data: [true/false]' +ros2 service call //gravity_compensation_enable std_srvs/srv/SetBool 'data: [true/false]' ``` The arm will hold itself against gravity and can be moved freely when the gravity compensation is enabled. diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml index d2fe606..a8fa32a 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml @@ -1,5 +1,7 @@ # Motor Assist: scale the no-load currents which alleviate the effects of friction # If the values are invalid, they default to 0 +# Joints not specified in the motor_assist or motor_specs sections +# do not support the current control mode motor_assist: # Set 'all' to [0, 1] to scale the no load currents of all joints uniformly # Or to -1 and use joint specific values @@ -12,6 +14,11 @@ motor_assist: wrist_angle: 0.5 wrist_rotate: 0.5 +# Dither: add a oscillatory motion proportional to the load to break static friction +# It is helpful when slow and smooth movements are needed +# WARNING: excessive dithering WILL cause heat and wear on the joints +dither: false + motor_specs: waist: # torque constant (Nm/A): how much torque is produced per Amp of current @@ -19,43 +26,53 @@ motor_specs: # current unit (A): how much current command is needed to produce 1 Amp of current current_unit: 0.00269 # no load current (A): the maximum no load current applied when motor_assist == 1 - # It should be as large as possible without the joint accelerating by itself + # should be as large as possible without the joint accelerating by itself no_load_current: 0.0 - # coefficient of friction (Nm/Nm): the torque needed to overcome Coulomb friction - # It should be as large as possible without the joint accelerating by itself - friction_coefficient: 0.0 + # kinetic friction (Nm/Nm): the kinetic friction coefficient + # should be tuned so that the friction is uniform over the entire joint range + kinetic_friction_coefficient: 0.0 + # static friction coefficient (Nm/Nm): the static friction coefficient + # affects the amplitude of the dithering motion + static_friction_coefficient: 0.0 + # dither speed (rad/s): the speed under which the joint dithers + dither_speed: 0.0 shoulder: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 - friction_coefficient: 0.2 + kinetic_friction_coefficient: 0.1 + static_friction_coefficient: 0.4 + dither_speed: 0.5 elbow: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 - friction_coefficient: 0.2 + kinetic_friction_coefficient: 0.1 + static_friction_coefficient: 0.6 + dither_speed: 0.5 forearm_roll: torque_constant: 0.897 current_unit: 0.00269 - no_load_current: 0.1 - friction_coefficient: 0.0 + no_load_current: 0.2 + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 wrist_angle: torque_constant: 0.897 current_unit: 0.00269 - no_load_current: 0.0 - friction_coefficient: 0.0 + no_load_current: 0.1 + kinetic_friction_coefficient: 0.1 + static_friction_coefficient: 0.4 + dither_speed: 0.5 wrist_rotate: torque_constant: 0.897 current_unit: 0.00269 - no_load_current: 0.1 - friction_coefficient: 0.0 - -# Joints specified here but not in motor_assist or motor_specs -# do not support the current control mode -joint_names: - [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] + no_load_current: 0.2 + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml index 3fec6b3..164d509 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml @@ -8,36 +8,45 @@ motor_assist: forearm_roll: 0.5 wrist_angle: 0.5 +dither: false + motor_specs: waist: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.1 - friction_coefficient: 0.0 + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 shoulder: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 - friction_coefficient: 0.0 + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 elbow: torque_constant: 1.793 current_unit: 0.00269 no_load_current: 0.0 - friction_coefficient: 0.0 + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 forearm_roll: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.1 - friction_coefficient: 0.0 + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 wrist_angle: torque_constant: 0.897 current_unit: 0.00269 no_load_current: 0.0 - friction_coefficient: 0.0 - -joint_names: - [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] + kinetic_friction_coefficient: 0.0 + static_friction_coefficient: 0.0 + dither_speed: 0.0 diff --git a/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro b/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro index 901bff8..b65eb5a 100644 --- a/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro +++ b/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/aloha_wx250s.urdf.xacro @@ -268,15 +268,15 @@ - + xyz="0.1079630000 0.0001158760 0"/> + + ixx="0.0000397100" + iyy="0.0008878000" + izz="0.0008880000" + ixy="0.0000023528" + ixz="0" + iyz="0"/> @@ -552,7 +552,7 @@ - + Date: Mon, 23 Sep 2024 14:32:44 -0500 Subject: [PATCH 11/17] fixed an incorrect doc link --- .../examples/interbotix_xsarm_gravity_compensation/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6ac164c..82a671a 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -5,7 +5,7 @@ This package demos the interbotix_gravity_compensation package on an Interbotix 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#motor-configs) and [`motor_specs`](TO DO: Add link to the doc) for details. +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`](TO DO: Add link to the doc) 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`: From 5b90ea49cdfadf13b9e238e1b088147d98fd671d Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Mon, 30 Sep 2024 11:20:27 -0500 Subject: [PATCH 12/17] changed the sleep positions so the arm doesn't drop as much The arms involved: aloha wx250s, aloha vx300s --- .../interbotix_xsarm_control/config/aloha_vx300s.yaml | 2 +- .../interbotix_xsarm_control/config/aloha_wx250s.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_vx300s.yaml b/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_vx300s.yaml index bd964ff..3f655d1 100644 --- a/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_vx300s.yaml +++ b/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_vx300s.yaml @@ -1,5 +1,5 @@ joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] -sleep_positions: [0, -1.80, 1.55, 0, -1.57, 0, 0] +sleep_positions: [0, -2.05, 1.7, 0, -2.0, 0, 0] joint_state_publisher: update_rate: 100 diff --git a/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_wx250s.yaml b/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_wx250s.yaml index 58bf242..c4eb8e5 100644 --- a/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_wx250s.yaml +++ b/interbotix_ros_xsarms/interbotix_xsarm_control/config/aloha_wx250s.yaml @@ -1,5 +1,5 @@ joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] -sleep_positions: [0, -1.80, 1.55, 0, -1.57, 0, 0] +sleep_positions: [0, -1.88, 1.6, 0, -1.6, 0, 0] joint_state_publisher: update_rate: 100 From 0679501c746694d3d9b65c22bc0a92be99d4947c Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Mon, 30 Sep 2024 13:55:02 -0500 Subject: [PATCH 13/17] updated README for the gravity compensation package - matched the node name with the one used in the code --- .../examples/interbotix_xsarm_gravity_compensation/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 82a671a..4c3e8a4 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -12,7 +12,7 @@ Run the following launch command where `robot_model` is a mandatory choice betwe ``` ros2 launch interbotix_xsarm_gravity_compensation interbotix_gravity_compensation.launch.py robot_model:=xxx [robot_name:=xxx] [motor_specs:=xxx] ``` -It runs the `interbotix_gravity_compensation` node and launches the xsarm_control script to bring up the arm. +It runs the `gravity_compensation` node and launches the xsarm_control script to bring up the arm. Then, enable/disable the gravity compensation with the following service call: ``` From e69f2b4fd6c60c7bc017a409f0dbf6e5f59e1ca3 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Mon, 30 Sep 2024 14:09:39 -0500 Subject: [PATCH 14/17] Updated README.md for the interbotix_xsarm_gravity_compensation package added link to the doc page with a example motor specs configuration --- .../examples/interbotix_xsarm_gravity_compensation/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4c3e8a4..c01256b 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -5,7 +5,7 @@ This package demos the interbotix_gravity_compensation package on an Interbotix 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`](TO DO: Add link to the doc) for details. +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`: From 763239175522fa3fb81fbabe8fcd587fdb2ed838 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Tue, 1 Oct 2024 14:02:52 -0500 Subject: [PATCH 15/17] Minor fixes on gravity compensation readme - used the Github provided warning block - added empty lines after headers mode configs - changed the port setting to the default /dev/ttyDXL - merged the two mode configs file launch - limited choices to wx250s and aloha_wx250s - improved readability for path concatenations package.xml - added dependency for interbotix_xsarm_control --- .../README.md | 10 +++++++-- ...gs_aloha_wx250s.yaml => mode_configs.yaml} | 2 +- .../config/mode_configs_wx250s.yaml | 21 ------------------- .../interbotix_gravity_compensation.launch.py | 19 +++++++---------- .../package.xml | 1 + 5 files changed, 18 insertions(+), 35 deletions(-) rename interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/{mode_configs_aloha_wx250s.yaml => mode_configs.yaml} (94%) delete mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs_wx250s.yaml 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 From 4916fd4dcd54a1daa8193d5e23268c18c2633583 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Tue, 1 Oct 2024 14:56:45 -0500 Subject: [PATCH 16/17] Minor fixes on the gravity compensation package - change file path concatenation style for better readability --- .../launch/interbotix_gravity_compensation.launch.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 a2aecb9..e8baa3b 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 @@ -107,9 +107,7 @@ def generate_launch_description(): default_value=[PathJoinSubstitution([ FindPackageShare('interbotix_xsarm_gravity_compensation'), 'config', - "motor_specs_"]), - LaunchConfiguration('robot_model'), - '.yaml' + "motor_specs_"]), LaunchConfiguration('robot_model'), '.yaml' ], description="the file path to the 'motor specs' YAML file.", ) From 7915e1dde44c300482a4979f1aac5e7056c8a1a8 Mon Sep 17 00:00:00 2001 From: Shiming-Liang <25278939+Shiming-Liang@users.noreply.github.com> Date: Tue, 1 Oct 2024 17:09:33 -0500 Subject: [PATCH 17/17] Minor fixes on the gravity compensation demo package mode_configs.yaml - removed launch script - removed unused mode configs argument - removed unused get_interbotix_xsarm_models import readme - updated the description about the launch script arguments - updated the dummy namespace to match the launch script logic: we used the robot name as the namespace --- .../README.md | 4 ++-- .../config/mode_configs.yaml | 21 ------------------- .../interbotix_gravity_compensation.launch.py | 14 ------------- 3 files changed, 2 insertions(+), 37 deletions(-) delete mode 100644 interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.yaml 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 228597d..1299f53 100644 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md +++ b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md @@ -11,7 +11,7 @@ Please refer to the documentations for [`mode_configs`](https://docs.trossenrobo ## 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`: +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, and `motor_specs` defaults to `/config/motor_specs_.yaml`: ``` ros2 launch interbotix_xsarm_gravity_compensation interbotix_gravity_compensation.launch.py robot_model:=xxx [robot_name:=xxx] [motor_specs:=xxx] ``` @@ -19,7 +19,7 @@ It runs the `gravity_compensation` node and launches the xsarm_control script to Then, enable/disable the gravity compensation with the following service call: ``` -ros2 service call //gravity_compensation_enable std_srvs/srv/SetBool 'data: [true/false]' +ros2 service call //gravity_compensation_enable std_srvs/srv/SetBool 'data: [true/false]' ``` The arm will hold itself against gravity and can be moved freely when the gravity compensation is enabled. diff --git a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.yaml b/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.yaml deleted file mode 100644 index 66f6312..0000000 --- a/interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/mode_configs.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_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' - -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 e8baa3b..255f921 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 @@ -44,8 +44,6 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from interbotix_xs_modules.xs_common import get_interbotix_xsarm_models - def launch_setup(context, *args, **kwargs): # Define the launch arguments @@ -112,16 +110,4 @@ def generate_launch_description(): description="the file path to the 'motor specs' YAML file.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - 'mode_configs', - default_value=[ - PathJoinSubstitution([ - FindPackageShare('interbotix_xsarm_gravity_compensation'), - 'config', - 'mode_configs.yaml']) - ], - description="the file path to the 'mode configs' YAML file.", - ) - ) return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])