Added a gravity compensation package, modified sleep positions

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="[email protected]">Luke Schmitt</maintainer>
+  <license>BSD-3-Clause</license>
+  <author email="[email protected]">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