ROS 1 Configuration

Network Configuration

ROS_IP

export ROS_IP=$(echo `hostname -I | cut -d" " -f1`)
if [ -z "$ROS_IP" ]; then
    export ROS_IP=127.0.0.1
fi

X-Series SDK Configuration

Motor Configs

The motors can be configured on startup by providing the motor_configs parameter to the xs_sdk. The expected format and explanation of the configuration options is below:

Note

The xs_sdk load_configs parameter must be set to true for the configurations to take effect.

Note

The motor_configs only need to be set once per robot configuration - meaning that the load_configs parameter can be set to false after the first boot.

# This file 'organizes' the motors in any Dynamixel-based robot into meaningful units. Every robot must
# have at least one instance of this file located in a 'config' directory in their root 'control' ROS package.
# Note that all parameters are mandatory unless stated otherwise. This file demos the setup for the VX300S robot.
#
# Refer to http://emanual.robotis.com/#control-table to look up the control registers for the various motors.
#
# SETUP OF REGISTERS FOR THE INTERBOTIX PLATFORMS
#
# All registers were left to their default values except for the ones listed here:
#   1) ID.........................The ID of the Dynamixel servo. Valids IDs go from 1 - 251.
#   2) Baud_Rate..................The speed at which serial communication occurs. All motors have this set to 1M
#                                 baud (corresponds to a register value of '3')
#   3) Return_Delay_Time..........The amount of time the servo delays in sending a reply packet after receiving a
#                                 command packet. A value of '0' tells the servo to send a reply without any delay.
#   4) Drive_Mode:................Used to define what direction is positive rotation. A value of '0' means CCW is
#                                 positive while a value of '1' means CW is positive. It also defines if profiles are
#                                 built based on velocity or time. 'Velocity' is the default (value of '0') and 'Time'
#                                 is a value of '4'. Adding the 'direction' value to the 'profile' value gives the
#                                 appropiate register value
#   5) Velocity_Limit.............Defines the max speed of the motor. A value of '131' correpsonds to a max speed
#                                 of PI rad/s
#   6) Min_Position_Limit.........Defines the minimum limit of a joint. Values range from 0 to 4095 with 2048
#                                 being equivalent to '0' rad and 0 being '-PI' rad.
#   7) Max_Position_Limit.........Defines the maximum limit of a joint. Values range from 0 to 4095 with 2048
#                                 being equivalent to '0' rad and 4095 being 'PI' rad.
#   8) Secondary_ID...............If a joint is controlled by two motors (usually by the shoulder or elbow), one motor
#                                 can be set to follow the commands of another motor by setting this register to the ID
#                                 of the master. A value of '255' disables this.
#
# Each motor's configs are grouped under the name of the joint the motor is controlling. The names are defined at
# http://support.interbotix.com/ under the 'Specifications' section. (Click a robot and scroll down to the 'Default Joint Limits' section.)

port: /dev/ttyDXL                                                               # Optional; specifies the USB port that connects to the U2D2; defaults to '/dev/ttyDXL'

joint_order: [waist, shoulder, elbow, wrist_angle, wrist_rotate, gripper]       # Defines the order in which the joints will appear in a JointState message; joint names must match those in the URDF (if present)
sleep_positions: [0, -1.85, -1.55, 0, -0.8, 0, 0]                               # Optional; specified joint positions that define a 'rest' pose where it is safe to torque off the robot; defaults to 0 for each joint

joint_state_publisher:                                                          # Joint State Publisher Settings...
  update_rate: 100                                                              # Optional; rate at which the motor states are sampled (and potentially published) in Hz; defaults to '100'
  publish_states: true                                                          # Optional; boolean that either enables or disables the Publisher; defaults to 'true'
  topic_name: joint_states                                                      # Optional; desired JointState topic name; defaults to 'joint_states'

groups:                                                                         # Optional; defines an unlimited number of joint groups in the robot; custom 'group' names can be used.
  arm: [waist, shoulder, elbow, wrist_angle, wrist_rotate]                      # Optional; groups can contain overlapping joints but those joints will retain the operating mode of the latter group.
                                                                                # Note that 'all' is a special group name that contains all the joints in the 'joint_order' sequence.
grippers:                                                                       # Optional; defines an unlimited number of Interbotix grippers; custom 'gripper' names can be used.
  gripper:                                                                      # Example gripper name
    horn_radius: 0.022                                                          # Optional; length in meters from the middle of the gripper servo horn to its edge; defaults to '0.014'
    arm_length: 0.036                                                           # Optional; length in meters from the end of the servo horn to a finger; defaults to '0.024'
    left_finger: left_finger                                                    # Optional; name of the 'left_finger' joint; this must match the name in the URDF (if present); defaults to 'left_finger'
    right_finger: right_finger                                                  # Optional; name of the 'right_finger' joint; this must match the name in the URDF (if present); defaults to 'right_finger'

shadows:                                                                        # Optional; list of any 'master' motors and all of its shadows (usually 1, but could be unlimited)
  shoulder:
    shadow_list: [shoulder_shadow]
    calibrate: true                                                             # Optional; set to true if the master and shadow servos make up a single joint and the shadows should be calibrated to read the exact same value as the master
  elbow:
    shadow_list: [elbow_shadow]
    calibrate: true

sisters:                                                                        # Optional; list of any '2in1' motors and their corresponding 'sister'; order is irrelevant

motors:                                                                         # List of all motors and the default values that should be written to the EEPROM registers; Note that any EEPROM register can be added;
  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: 1
    Velocity_Limit: 131
    Min_Position_Limit: 841
    Max_Position_Limit: 3196
    Secondary_ID: 255

  shoulder_shadow:
    ID: 3
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 841
    Max_Position_Limit: 3196
    Secondary_ID: 2

  elbow:
    ID: 4
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 1002
    Max_Position_Limit: 3196
    Secondary_ID: 255

  elbow_shadow:
    ID: 5
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 1
    Velocity_Limit: 131
    Min_Position_Limit: 1002
    Max_Position_Limit: 3196
    Secondary_ID: 4

  wrist_angle:
    ID: 6
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 570
    Max_Position_Limit: 3264
    Secondary_ID: 255

  wrist_rotate:
    ID: 7
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 0
    Max_Position_Limit: 4095
    Secondary_ID: 255

  gripper:
    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

Mode Configs

The startup operating modes can be set on startup by providing the mode_configs parameter to the xs_sdk. The expected format and explanation of the configuration options is below:

# 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                             # 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