robot customization in one script!

README.md

@@ -50,9 +50,18 @@ By default, it allows to hold a M6 threaded rod aligned with the wrist rotation
 according to your needs.
 
 ### Software modifications
-Once the physical modification of the robotic arm has been completed, several files need to be updated to use ROS 2 with the new arm.
-The default arm name is `wx250s` and one will create a new arm configuration `wx250s_custom`. To do so:
-1. Open the file `~/interbotix_ws/install/interbotix_xs_modules/lib/python3.10/site-packages/interbotix_xs_modules/xs_common/xs_common.py` and add `wx250_custom` to the `_XSARM_MODELS` tuple (near line 32).
-2. Copy the [/wx250s_custom](http://sekisushai.net/git/sekisushai/small/src/branch/master/wx250s_custom/wx250s_custom.urdf.xacro) to folder `~/interbotix_ws/install/interbotix_xsarm_descriptions/share/interbotix_xsarm_descriptions/urdf/`.
-3. Open the file `wx250s_custom.urdf.xacro`
+Once the Hardware modification has been completed, several files need to be updated in interbotix packages to use the new arm, named `ẁx250s_custom`. To do so, run the `install.sh` script as follows:
+```
+./install.sh
+```
+The script ask several infos to update the interbotix packages files. Also, the kinematics matrices `M` and `Slist` are computed. These matrices are needed by the Python API which will drive the robotic arm.
 
+
+```
+ros2 launch interbotix_xsarm_descriptions xsarm_description.launch.py robot_model:=wx250s_custom use_joint_pub_gui:=true
+```
+You should see this:
+
+![Alt text](wx250s_custom/rviz.png)
+
+### 

wx250s_custom/install.sh

@@ -0,0 +1,50 @@
+#!/bin/bash 
+
+# Tweak the default interbotix_ws install to add a customized robot wx250s_custom
+
+read -p "Enter the interbotix packages installation path [default: ~/interbotix_ws]:" DIR
+DIR=${DIR:-~/interbotix_ws}
+
+cp xs_common.py "$DIR/src/interbotix_ros_toolboxes/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_common/"
+cp wx250s_custom.yaml "$DIR/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_control/config/"
+cp -r wx250s_custom_meshes "$DIR/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/meshes/"
+
+VAR=`awk 'NR==451{ print; exit }' wx250s_custom.urdf.xacro`
+VAR=`awk -F'"' '{print $2}' <<< $VAR`
+
+read -p "Enter the coordinate between wrist_rotate joint and SMA center in m [default: ${VAR}]:" COORD
+COORD=${COORD:-${VAR}}
+
+cp wx250s_custom.urdf.xacro wx250s_custom.urdf.xacro~
+
+sed -i "451s/$VAR/$COORD/" wx250s_custom.urdf.xacro~
+
+# convert to urdf file needed for kinematic_from_description package
+ros2 run xacro xacro wx250s_custom.urdf.xacro~ > out.urdf
+
+# move the urdf.xacro file
+mv wx250s_custom.urdf.xacro~ "$DIR/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/wx250s_custom.urdf.xacro"
+
+#
+git clone https://github.com/Interbotix/kinematics_from_description.git
+cd kinematics_from_description
+python setup.py install --user
+cd ..
+rm -rf kinematics_from_description
+
+cp mr_descriptions.py mr_descriptions.py~
+
+python kinematics.py
+
+mv mr_descriptions.py "$DIR/src/interbotix_ros_toolboxes/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/"
+
+mv mr_descriptions.py~ mr_descriptions.py
+
+rm out.urdf
+
+cd $DIR
+
+rm -rf build
+rm -rf install
+
+colcon build

wx250s_custom/kinematics.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Fri Apr  7 15:38:46 2023
+
+@author: Pierre Lecomte
+"""
+import numpy as np
+from kinematics_from_description.kfd import KinematicsFromDescriptionTool as KFD
+
+tool = KFD(
+            {
+                "body_frame": "sma_link",
+                "space_frame": "base_link",
+                "namespace": "wx250s_custom",
+            }            
+            )
+
+tool.load_desc_from_file('out.urdf')
+tool.run()
+
+print("Slist:")
+print(tool.Slist.T)
+
+print("M:")
+print(tool.M)
+
+x = f"""class wx250s_custom(ModernRoboticsDescription):
+    Slist = np.array(
+        {np.array2string(tool.Slist.T, separator=',')}
+        ).T
+
+    M = np.array({np.array2string(tool.M, separator=',')})
+    """
+
+with open("mr_descriptions.py","a") as f:
+    f.writelines(x)

wx250s_custom/matrices.txt

@@ -0,0 +1,12 @@
+Slist:
+[[ 0.       0.       1.       0.       0.       0.     ]
+ [ 0.       1.       0.      -0.11065  0.       0.     ]
+ [ 0.       1.       0.      -0.36065  0.       0.04975]
+ [ 1.       0.       0.       0.       0.36065  0.     ]
+ [ 0.       1.       0.      -0.36065  0.       0.29975]
+ [ 1.       0.       0.       0.       0.36065  0.     ]]
+M:
+[[1.      0.      0.      0.49275]
+ [0.      1.      0.      0.     ]
+ [0.      0.      1.      0.36065]
+ [0.      0.      0.      1.     ]]

wx250s_custom/mr_descriptions.py

@@ -0,0 +1,233 @@
+# 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.
+
+"""
+Modern Robotics Descriptions for all Interbotix X-Series Arms.
+
+This module contains properties required by the Modern Robotics library to perform
+kinematic calculations such as the Joint Screw Axes and Home Configuration for each
+X-Series robot.
+
+Note that the end-effector is positioned at '<robot_name>/ee_gripper_link' and that the
+Space frame is positioned at '<robot_name>/base_link'.
+
+To calculate your own MR Description, check out the kinematics_from_description package:
+    https://github.com/Interbotix/kinematics_from_description
+"""
+
+from abc import ABC
+
+import numpy as np
+
+
+class ModernRoboticsDescription(ABC):
+    """
+    Abstract base class for other MR Description classes.
+
+    Derived classes should override the Slist and M member variables.
+    """
+
+    Slist: np.ndarray = None
+    """
+    Joint screw axes in the space frame when the manipulator is at the home position, in the format
+    of a matrix with axes as the columns
+    """
+
+    M: np.ndarray = None
+    """The home configuration (position and orientation) of the end-effector"""
+
+
+class px100(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,    0.0, 0.0],
+                      [0.0, 1.0, 0.0, -0.0931, 0.0, 0.0],
+                      [0.0, 1.0, 0.0, -0.1931, 0.0, 0.035],
+                      [0.0, 1.0, 0.0, -0.1931, 0.0, 0.135]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.248575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.1931],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class px150(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.10457, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.25457, 0.0,     0.05],
+                      [0.0, 1.0, 0.0, -0.25457, 0.0,     0.2],
+                      [1.0, 0.0, 0.0,  0.0,     0.25457, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.358575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.25457],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class rx150(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.10457, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.25457, 0.0,     0.05],
+                      [0.0, 1.0, 0.0, -0.25457, 0.0,     0.2],
+                      [1.0, 0.0, 0.0,  0.0,     0.25457, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.358575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.25457],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class rx200(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.10457, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.30457, 0.0,     0.05],
+                      [0.0, 1.0, 0.0, -0.30457, 0.0,     0.25],
+                      [1.0, 0.0, 0.0,  0.0,     0.30457, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.408575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.30457],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class vx250(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.12705, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.37705, 0.0,     0.06],
+                      [0.0, 1.0, 0.0, -0.37705, 0.0,     0.31],
+                      [1.0, 0.0, 0.0,  0.0,     0.37705, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.468575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.37705],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class vx300(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.12705, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.42705, 0.0,     0.05955],
+                      [0.0, 1.0, 0.0, -0.42705, 0.0,     0.35955],
+                      [1.0, 0.0, 0.0,  0.0,     0.42705, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.536494],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.42705],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class vx300s(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.12705, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.42705, 0.0,     0.05955],
+                      [1.0, 0.0, 0.0,  0.0,     0.42705, 0.0],
+                      [0.0, 1.0, 0.0, -0.42705, 0.0,     0.35955],
+                      [1.0, 0.0, 0.0,  0.0,     0.42705, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.536494],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.42705],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class wx200(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.11065, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.31065, 0.0,     0.05],
+                      [0.0, 1.0, 0.0, -0.31065, 0.0,     0.25],
+                      [1.0, 0.0, 0.0,  0.0,     0.31065, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.408575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.31065],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class wx250(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.11065, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.36065, 0.0,     0.04975],
+                      [0.0, 1.0, 0.0, -0.36065, 0.0,     0.29975],
+                      [1.0, 0.0, 0.0,  0.0,     0.36065, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.458325],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.36065],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class wx250s(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.11065, 0.0,     0.0],
+                      [0.0, 1.0, 0.0, -0.36065, 0.0,     0.04975],
+                      [1.0, 0.0, 0.0,  0.0,     0.36065, 0.0],
+                      [0.0, 1.0, 0.0, -0.36065, 0.0,     0.29975],
+                      [1.0, 0.0, 0.0,  0.0,     0.36065, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.458325],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.36065],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class mobile_px100(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,     0.0, 0.0],
+                      [0.0, 1.0, 0.0, -0.08518, 0.0, 0.0],
+                      [0.0, 1.0, 0.0, -0.18518, 0.0, 0.035],
+                      [0.0, 1.0, 0.0, -0.18518, 0.0, 0.135]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.248575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.18518],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class mobile_wx200(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,      0.0,      0.0],
+                      [0.0, 1.0, 0.0, -0.104825, 0.0,      0.0],
+                      [0.0, 1.0, 0.0, -0.304825, 0.0,      0.05],
+                      [0.0, 1.0, 0.0, -0.304825, 0.0,      0.25],
+                      [1.0, 0.0, 0.0,  0.0,      0.304825, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.408575],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.304825],
+                  [0.0, 0.0, 0.0, 1.0]])
+
+
+class mobile_wx250s(ModernRoboticsDescription):
+    Slist = np.array([[0.0, 0.0, 1.0,  0.0,      0.0,      0.0],
+                      [0.0, 1.0, 0.0, -0.104825, 0.0,      0.0],
+                      [0.0, 1.0, 0.0, -0.354825, 0.0,      0.04975],
+                      [1.0, 0.0, 0.0,  0.0,      0.354825, 0.0],
+                      [0.0, 1.0, 0.0, -0.354825, 0.0,      0.29975],
+                      [1.0, 0.0, 0.0,  0.0,      0.354825, 0.0]]).T
+
+    M = np.array([[1.0, 0.0, 0.0, 0.458325],
+                  [0.0, 1.0, 0.0, 0.0],
+                  [0.0, 0.0, 1.0, 0.354825],
+                  [0.0, 0.0, 0.0, 1.0]])
+

wx250s_custom/wx250s_custom.urdf.xacro

@@ -0,0 +1,499 @@
+<?xml version="1.0"?>
+<robot name="wx250s_custom" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:property name="robot_model"
+    value="wx250s_custom"
+    scope="global"/>
+  <xacro:property name="dof"
+    value="6"
+    scope="global"/>
+
+  <xacro:arg name="robot_name"                      default="${robot_model}"/>
+  <xacro:arg name="base_link_frame"                 default="base_link"/>
+  <xacro:arg name="use_world_frame"                 default="true"/>
+  <xacro:arg name="use_gripper"                     default="false"/>
+  <xacro:arg name="external_urdf_loc"               default=""/>
+  <xacro:arg name="hardware_type"                   default="actual"/>
+
+  <xacro:property name="mesh_directory"             value="package://interbotix_xsarm_descriptions/meshes/${robot_model}_meshes"/>
+  <xacro:property name="urdf_loc"                   value="$(arg external_urdf_loc)"/>
+  <xacro:property name="hardware_type"              value="$(arg hardware_type)"/>
+  <xacro:property name="pi_offset"                  value="0.00001"/>
+
+  <xacro:property name="waist_limit_effort"         value="10"/> 
+  <xacro:property name="waist_limit_lower"          value="${-pi + pi_offset}"/>
+  <xacro:property name="waist_limit_upper"          value="${pi - pi_offset}"/>
+  <xacro:property name="waist_limit_vel"            value="${pi}"/>
+
+  <xacro:property name="shoulder_limit_effort"      value="100"/><!-- 20 -->
+  <xacro:property name="shoulder_limit_lower"       value="${radians(-108)}"/>
+  <xacro:property name="shoulder_limit_upper"       value="${radians(114)}"/>
+  <xacro:property name="shoulder_limit_vel"         value="${pi}"/>
+
+  <xacro:property name="elbow_limit_effort"         value="15"/>
+  <xacro:property name="elbow_limit_lower"          value="${radians(-123)}"/>
+  <xacro:property name="elbow_limit_upper"          value="${radians(92)}"/>
+  <xacro:property name="elbow_limit_vel"            value="${pi}"/>
+
+  <xacro:property name="forearm_roll_limit_effort"   value="2"/>
+  <xacro:property name="forearm_roll_limit_lower"    value="${-pi + pi_offset}"/>
+  <xacro:property name="forearm_roll_limit_upper"    value="${pi - pi_offset}"/>
+  <xacro:property name="forearm_roll_limit_vel"      value="${pi}"/>
+
+  <xacro:property name="wrist_angle_limit_effort"   value="5"/>
+  <xacro:property name="wrist_angle_limit_lower"    value="${radians(-100)}"/>
+  <xacro:property name="wrist_angle_limit_upper"    value="${radians(123)}"/>
+  <xacro:property name="wrist_angle_limit_vel"      value="${pi}"/>
+
+  <xacro:property name="wrist_rotate_limit_effort"  value="1"/>
+  <xacro:property name="wrist_rotate_limit_lower"   value="${-pi + pi_offset}"/>
+  <xacro:property name="wrist_rotate_limit_upper"   value="${pi - pi_offset}"/>
+  <xacro:property name="wrist_rotate_limit_vel"     value="${pi}"/>
+
+  <xacro:property name="gripper_limit_vel"          value="${pi}"/>
+
+  <xacro:property name="finger_limit_effort"        value="5"/>
+  <xacro:property name="finger_limit_lower"         value="0.015"/>
+  <xacro:property name="finger_limit_upper"         value="0.037"/>
+  <xacro:property name="finger_limit_vel"           value="1"/>
+
+  <material name="interbotix_black">
+    <texture filename="package://interbotix_xsarm_descriptions/meshes/interbotix_black.png"/>
+  </material>
+
+  <xacro:if value="$(arg use_world_frame)">
+    <link name="world"/>
+    <joint name="fixed" type="fixed">
+      <parent
+        link="world"/>
+      <child
+        link="$(arg robot_name)/$(arg base_link_frame)"/>
+    </joint>
+  </xacro:if> <!-- use_world_frame -->
+
+  <link name="$(arg robot_name)/$(arg base_link_frame)">
+    <visual>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/base.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="interbotix_black"/>
+    </visual>
+    <collision>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/base.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="-0.0380446000 0.0006138920 0.0193354000"/>
+      <mass value="0.538736"/>
+      <inertia
+        ixx="0.0021150000"
+        iyy="0.0006921000"
+        izz="0.0025240000"
+        ixy="-0.0000163500"
+        ixz="0.0000006998"
+        iyz="0.0000464200"/>
+    </inertial>
+  </link>
+
+  <joint name="waist" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit
+      effort="${waist_limit_effort}"
+      lower="${waist_limit_lower}"
+      upper="${waist_limit_upper}"
+      velocity="${waist_limit_vel}"/>
+    <origin
+      rpy="0 0 0"
+      xyz="0 0 0.072"/>
+    <parent
+      link="$(arg robot_name)/$(arg base_link_frame)"/>
+    <child
+      link="$(arg robot_name)/shoulder_link"/>
+    <dynamics
+      friction="0.1"
+      damping="0.1"/>
+  </joint>
+
+  <link name="$(arg robot_name)/shoulder_link">
+    <visual>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 -0.003"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/shoulder.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="interbotix_black"/>
+    </visual>
+    <collision>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 -0.003"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/shoulder.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0.0000223482 0.0000414609 0.0066287000"/>
+      <mass value="0.480879"/>
+      <inertia
+        ixx="0.0003790000"
+        iyy="0.0005557000"
+        izz="0.0005889000"
+        ixy="0.0000000022"
+        ixz="-0.0000003561"
+        iyz="0.0000012245"/>
+    </inertial>
+  </link>
+
+  <joint name="shoulder" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit
+      effort="${shoulder_limit_effort}"
+      lower="${shoulder_limit_lower}"
+      upper="${shoulder_limit_upper}"
+      velocity="${shoulder_limit_vel}"/>
+    <origin
+      rpy="0 0 0"
+      xyz="0 0 0.03865"/>
+    <parent
+      link="$(arg robot_name)/shoulder_link"/>
+    <child
+      link="$(arg robot_name)/upper_arm_link"/>
+    <dynamics
+      friction="0.1"
+      damping="0.1"/>
+  </joint>
+
+  <link name="$(arg robot_name)/upper_arm_link">
+    <visual>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/upper_arm.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="interbotix_black"/>
+    </visual>
+    <collision>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/upper_arm.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0.0171605000 0.0000002725 0.1913230000"/>
+      <mass value="0.430811"/>
+      <inertia
+        ixx="0.0034630000"
+        iyy="0.0035870000"
+        izz="0.0004566000"
+        ixy="-0.0000000001"
+        ixz="-0.0000000002"
+        iyz="0.0004272000"/>
+    </inertial>
+  </link>
+
+  <joint name="elbow" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit
+      effort="${elbow_limit_effort}"
+      lower="${elbow_limit_lower}"
+      upper="${elbow_limit_upper}"
+      velocity="${elbow_limit_vel}"/>
+    <origin
+      rpy="0 0 0"
+      xyz="0.04975 0 0.25"/>
+    <parent
+      link="$(arg robot_name)/upper_arm_link"/>
+    <child
+      link="$(arg robot_name)/upper_forearm_link"/>
+    <dynamics
+      friction="0.1"
+      damping="0.1"/>
+  </joint>
+
+  <link name="$(arg robot_name)/upper_forearm_link">
+    <visual>
+      <origin
+        rpy="0 0 0"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/upper_forearm.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="interbotix_black"/>
+    </visual>
+    <collision>
+      <origin
+        rpy="0 0 0"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/upper_forearm.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin
+        rpy="0 0 0"
+        xyz="0.1079630000 0.0001158760 0"/>
+      <mass value="0.234589"/>
+      <inertia
+        ixx="0.0000397100"
+        iyy="0.0008878000"
+        izz="0.0008880000"
+        ixy="0.0000023528"
+        ixz="0"
+        iyz="0"/>
+    </inertial>
+  </link>
+
+  <joint name="forearm_roll" type="revolute">
+    <axis xyz="1 0 0"/>
+    <limit
+      effort="${forearm_roll_limit_effort}"
+      lower="${forearm_roll_limit_lower}"
+      upper="${forearm_roll_limit_upper}"
+      velocity="${forearm_roll_limit_vel}"/>
+    <origin
+      rpy="0 0 0"
+      xyz="0.175 0 0"/>
+    <parent
+      link="$(arg robot_name)/upper_forearm_link"/>
+    <child
+      link="$(arg robot_name)/lower_forearm_link"/>
+    <dynamics
+      friction="0.1"
+      damping="0.1"/>
+  </joint>
+
+  <link name="$(arg robot_name)/lower_forearm_link">
+    <visual>
+      <origin
+        rpy="${pi} 0 0"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/lower_forearm.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="interbotix_black"/>
+    </visual>
+    <collision>
+      <origin
+        rpy="${pi} 0 0"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/lower_forearm.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin
+        rpy="${pi} 0 0"
+        xyz="0.0374395000 0.0052225200 0"/>
+      <mass value="0.220991"/>
+      <inertia
+        ixx="0.0000636900"
+        iyy="0.0001677000"
+        izz="0.0001834000"
+        ixy="-0.0000229200"
+        ixz="0"
+        iyz="0"/>
+    </inertial>
+  </link>
+
+  <joint name="wrist_angle" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit
+      effort="${wrist_angle_limit_effort}"
+      lower="${wrist_angle_limit_lower}"
+      upper="${wrist_angle_limit_upper}"
+      velocity="${wrist_angle_limit_vel}"/>
+    <origin
+      rpy="0 0 0"
+      xyz="0.075 0 0"/>
+    <parent
+      link="$(arg robot_name)/lower_forearm_link"/>
+    <child
+      link="$(arg robot_name)/wrist_link"/>
+    <dynamics
+      friction="0.1"
+      damping="0.1"/>
+  </joint>
+
+  <link name="$(arg robot_name)/wrist_link">
+    <visual>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/wrist.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="interbotix_black"/>
+    </visual>
+    <collision>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0 0 0"/>
+      <geometry>
+        <mesh
+          filename="${mesh_directory}/wrist.stl"
+          scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin
+        rpy="0 0 ${pi/2}"
+        xyz="0.0423600000 -0.0000106630 0.0105770000"/>
+      <mass value="0.084957"/>
+      <inertia
+        ixx="0.0000308200"
+        iyy="0.0000282200"
+        izz="0.0000315200"
+        ixy="0.0000000191"
+        ixz="0.0000000023"
+        iyz="0.0000025481"/>
+    </inertial>
+  </link>
+
+    <joint name="wrist_rotate" type="revolute">
+      <axis xyz="1 0 0"/>
+      <limit
+        effort="${wrist_rotate_limit_effort}"
+        lower="${wrist_rotate_limit_lower}"
+        upper="${wrist_rotate_limit_upper}"
+        velocity="${wrist_rotate_limit_vel}"/>
+      <origin
+        rpy="0 0 0"
+        xyz="0.065 0 0"/>
+      <parent
+        link="$(arg robot_name)/wrist_link"/>
+      <child
+        link="$(arg robot_name)/sma_attach_link"/>
+      <dynamics
+        friction="0.1"
+        damping="0.1"/>
+    </joint>
+
+    <link name="$(arg robot_name)/sma_attach_link">
+      <visual>
+        <origin
+          rpy="0 0 ${pi/2}"
+          xyz="0.007 0 0"/>
+        <geometry>
+          <mesh
+            filename="${mesh_directory}/attach.stl"
+            scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="interbotix_black"/>
+      </visual>
+      <collision>
+        <origin
+          rpy="0 0 ${pi/2}"
+          xyz="0.007 0 0"/>
+        <geometry>
+          <mesh
+            filename="${mesh_directory}/attach.stl"
+            scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin
+          rpy="0 0 ${pi/2}"
+          xyz="0.0216310000 0.0000002516 0.0114100000"/>
+        <mass value="0.072885"/>
+        <inertia
+          ixx="0.0000253700"
+          iyy="0.0000183600"
+          izz="0.0000167400"
+          ixy="0.0000000000"
+          ixz="0.0000000000"
+          iyz="0.0000004340"/>
+      </inertial>
+    </link>
+    
+    <joint name="sma" type="fixed">
+      <axis xyz="1 0 0"/>
+      <origin
+        rpy="0 0 0"
+        xyz="0.128 0 0"/>
+      <parent
+        link="$(arg robot_name)/sma_attach_link"/>
+      <child
+        link="$(arg robot_name)/sma_link"/>
+    </joint>
+
+    <link name="$(arg robot_name)/sma_link">
+      <visual>
+        <origin
+          rpy="0 0 ${pi/2}"
+          xyz="0 0 0"/>
+        <geometry>
+          <mesh
+            filename="${mesh_directory}/sma.stl"
+            scale="0.001 0.001 0.001"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin
+          rpy="0 0 ${pi/2}"
+          xyz="0 0 0"/>
+        <geometry>
+          <mesh
+            filename="${mesh_directory}/sma.stl"
+            scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="0.1"/>
+        <inertia
+          ixx="0.001"
+          iyy="0.001"
+          izz="0.001"
+          ixy="0"
+          ixz="0"
+          iyz="0"/>
+      </inertial>
+    </link>
+
+  <xacro:if value="${hardware_type == 'gz_classic'}">
+    <xacro:gazebo_configs/>
+  </xacro:if>
+
+  <xacro:if value="${urdf_loc != ''}">
+    <xacro:include filename="${urdf_loc}"/>
+  </xacro:if>
+
+</robot>

wx250s_custom/wx250s_custom.yaml

@@ -0,0 +1,104 @@
+port: /dev/ttyDXL
+
+joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate]
+sleep_positions: [0, -1.80, 1.55, 0, 0.8, 0]
+
+joint_state_publisher:
+  update_rate: 100
+  publish_states: true
+  topic_name: joint_states
+
+groups:
+  arm: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate]
+
+shadows:
+  shoulder:
+    shadow_list: [shoulder_shadow]
+    calibrate: true
+  elbow:
+    shadow_list: [elbow_shadow]
+    calibrate: true
+
+sisters:
+
+motors:
+  waist:
+    ID: 1
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 0
+    Velocity_Limit: 131
+    Min_Position_Limit: 0
+    Max_Position_Limit: 4095
+    Secondary_ID: 255
+
+  shoulder:
+    ID: 2
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 0
+    Velocity_Limit: 131
+    Min_Position_Limit: 819
+    Max_Position_Limit: 3345
+    Secondary_ID: 255
+
+  shoulder_shadow:
+    ID: 3
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 1
+    Velocity_Limit: 131
+    Min_Position_Limit: 819
+    Max_Position_Limit: 3345
+    Secondary_ID: 2
+
+  elbow:
+    ID: 4
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 0
+    Velocity_Limit: 131
+    Min_Position_Limit: 648
+    Max_Position_Limit: 3094
+    Secondary_ID: 255
+
+  elbow_shadow:
+    ID: 5
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 1
+    Velocity_Limit: 131
+    Min_Position_Limit: 648
+    Max_Position_Limit: 3094
+    Secondary_ID: 4
+
+  forearm_roll:
+    ID: 6
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 0
+    Velocity_Limit: 131
+    Min_Position_Limit: 0
+    Max_Position_Limit: 4095
+    Secondary_ID: 255
+
+  wrist_angle:
+    ID: 7
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 1
+    Velocity_Limit: 131
+    Min_Position_Limit: 910
+    Max_Position_Limit: 3447
+    Secondary_ID: 255
+
+  wrist_rotate:
+    ID: 8
+    Baud_Rate: 3
+    Return_Delay_Time: 0
+    Drive_Mode: 0
+    Velocity_Limit: 131
+    Min_Position_Limit: 0
+    Max_Position_Limit: 4095
+    Secondary_ID: 255
+ 

wx250s_custom/wx250s_custom_attach.scad

@@ -2,6 +2,8 @@ include <../cad/threads.scad>;
 
 $fn=128;
 
+rotate([90,0,0])
+translate([-28.5/2,-46.5/2+11,0])
 difference(){
 union(){
 linear_extrude(4)

wx250s_custom/xs_common.py

@@ -0,0 +1,88 @@
+# 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, Tuple
+
+# Tuple of valid Interbotix X-Series arm models
+_XSARM_MODELS = (
+    'px100',
+    'px150',
+    'rx150',
+    'rx200',
+    'wx200',
+    'wx250',
+    'wx250s',
+    'wx250s_custom',
+    'vx250',
+    'vx300',
+    'vx300s',
+    'mobile_px100',
+    'mobile_wx200',
+    'mobile_wx250s',
+)
+
+# Tuple of valid Interbotix LoCoBot models
+_XSLOCOBOT_MODELS = (
+    'locobot_base',
+    'locobot_px100',
+    'locobot_wx200',
+    'locobot_wx250s',
+)
+
+
+def get_interbotix_xsarm_models() -> Tuple[str]:
+    """Get the tuple of valid Interbotix X-Series arm models."""
+    return _XSARM_MODELS
+
+
+def get_interbotix_xslocobot_models() -> Tuple[str]:
+    """Get the tuple of valid Interbotix LoCoBot models."""
+    return _XSLOCOBOT_MODELS
+
+
+def get_interbotix_xsarm_joints(robot_model: str) -> List[str]:
+    """
+    Return a list of joints in the robot_model.
+
+    :param robot_model: The robot model to get the joints of
+    :return: A list of joint names of the given robot model
+    :raises: KeyError if the robot model is not valid
+    """
+    if robot_model in ('mobile_px100', 'px100'):
+        return ['waist', 'shoulder', 'elbow', 'wrist_angle', 'left_finger']
+    elif robot_model in ('px150', 'rx150', 'rx200', 'wx200', 'wx250', 'vx250', 'vx300'):
+        return ['waist', 'shoulder', 'elbow', 'wrist_angle', 'wrist_rotate', 'left_finger']
+    elif robot_model in ('wx250s_custom'):
+        return ['waist', 'shoulder', 'elbow', 'forearm_roll', 'wrist_angle', 'wrist_rotate']
+    elif robot_model in ('mobile_wx250s', 'wx250s', 'vx300s'):
+        return [
+            'waist', 'shoulder', 'elbow', 'forearm_roll',
+            'wrist_angle', 'wrist_rotate', 'left_finger'
+        ]
+    else:
+        raise KeyError(f'{robot_model} is not a valid robot model.')