Added a gravity compensation package, modified sleep positions (#225)

* added demo of gravity compensation on xsarm

* added config and launch files for gravity compensation demo on xsarm

* updated doc for gravity compensation

* properly formatted

* move the config doc to interbotix_gravity_compensation

* use namespace in the launch file

* fixed typo in the grav comp yaml files, pointed to the closer example config

* two arms can run at the same time, added parameters for friction modelling

* corrected some entries of the URDF for  the aloha wx250s

* 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.

* fixed an incorrect doc link

* changed the sleep positions so the arm doesn't drop as much

The arms involved: aloha wx250s, aloha vx300s

* updated README for the gravity compensation package

- matched the node name with the one used in the code

* Updated README.md for the interbotix_xsarm_gravity_compensation package

added link to the doc page with a example motor specs configuration

* 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

* Minor fixes on the gravity compensation package

- change file path concatenation style for better readability

* 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

interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+project(interbotix_xsarm_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()

interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/README.md

@@ -0,0 +1,33 @@
+# 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, and `motor_specs` defaults to `<path_to_this_package>/config/motor_specs_<robot_model>.yaml`:
+```
+ros2 launch interbotix_xsarm_gravity_compensation interbotix_gravity_compensation.launch.py robot_model:=xxx [robot_name:=xxx] [motor_specs:=xxx]
+```
+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:
+```
+ros2 service call /<robot_name>/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]
+> 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.

interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_aloha_wx250s.yaml

@@ -0,0 +1,78 @@
+# 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
+  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
+
+# 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
+    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
+    # should be as large as possible without the joint accelerating by itself
+    no_load_current: 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
+    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
+    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.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.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.2
+    kinetic_friction_coefficient: 0.0
+    static_friction_coefficient: 0.0
+    dither_speed: 0.0

interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/config/motor_specs_wx250s.yaml

@@ -0,0 +1,52 @@
+# Please refer to the motor_specs_aloha_wx250s.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
+
+dither: false
+
+motor_specs:
+  waist:
+    torque_constant: 1.793
+    current_unit: 0.00269
+    no_load_current: 0.1
+    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
+    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
+    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
+    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
+    kinetic_friction_coefficient: 0.0
+    static_friction_coefficient: 0.0
+    dither_speed: 0.0

interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/launch/interbotix_gravity_compensation.launch.py

@@ -0,0 +1,113 @@
+#!/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.
+
+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
+
+
+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='gravity_compensation',
+        namespace=robot_name_launch_arg,
+        output='screen',
+        emulate_tty=True,
+        parameters=[{'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=('wx250s', 'aloha_wx250s'),
+            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_xsarm_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)])

interbotix_ros_xsarms/examples/interbotix_xsarm_gravity_compensation/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>interbotix_xsarm_gravity_compensation</name>
+  <version>0.0.0</version>
+  <description>The demo of the interbotix_gravity_compensation package on xsarm</description>
+  <maintainer email="trsupport@trossenrobotics.com">Luke Schmitt</maintainer>
+  <license>BSD-3-Clause</license>
+  <author email="trsupport@trossenrobotics.com">Shiming Liang</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>interbotix_gravity_compensation</depend>
+  <depend>interbotix_xsarm_control</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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

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