ARCHIVE: Controlling the Seed Robotics units under ROS
IMPORTANT:
This page contains archived / obsolete information regarding our ROS packages. It applies to ROS distribution up to Kinetic.
For newer version, please check the most up to date information, from our WIKI index.
For operation under ROS, we recommend using the open-source ROS dynamixel_driver package.
This package is under active maintenance at the time of writing. While the package does not allow access to all of the Seed Robotics' device features (for example the current readings), it will let you fully operate in Position Control mode, at the time of writing.
Please note: The original creator of the Dynamixel protocol specification (Robotis Co) has released their own ROS package. However these instructions will focus on the original ROS dynamixel_driver
as it has shown better overall compatibility.
Pre Requisites
At the time of writing the package did not yet contain definitions for the “Model Numbers” reported by Seed Robotics units. Therefore you will need to apply a small patch to one of the files.
Begin by installing the Complete Dynamixel motor package (a superset of the Dynamixel driver) following the instructions on the package page.
Once it's installed, we need to edit 2 files to get the package ready:
1) Type roscd
to move to your ROS root
(this is usually /opt/ros/[ROS version]/…
but please adjust according to your distribution)
2) Run the command find -name dynamixel_const.py
3) Open the filedynamixel_const.py
located in the previous step, as sudo
or root
:
(for example sudo gedit [location of the file from step 2] &
, assuming you'd use gedit
to edit the file.
Failing to open the file as sudo
or root
will result in not being able to save it, as it will open read-only.)
Modify the following:
3a) Scroll to the bottom of the file and locate the last }
3b) Immediately before this last }
, add the following code:
# BEGIN SEED ROBOTICS ADDITIONS # SEED Robotics Main board Model Nrs 400: { 'name': 'SEED_ARESBRD', 'encoder_resolution': 4096, # Electronic board; parameter not relevant 'range_degrees': 360.0, # Electronic board; parameter not relevant 'torque_per_volt': 8.4 / 12.0, # Electronic board; parameter not relevant 'velocity_per_volt': (45 * RPM_TO_RADSEC) / 12.0, # Electronic board; parameter not relevant 'rpm_per_tick': 0.114, 'features': [] }, 405: { 'name': 'SEED_EROSBRD', 'encoder_resolution': 4096, # Electronic board; parameter not relevant 'range_degrees': 360.0, # Electronic board; parameter not relevant 'torque_per_volt': 8.4 / 12.0, # Electronic board; parameter not relevant 'velocity_per_volt': (45 * RPM_TO_RADSEC) / 12.0, # Electronic board; parameter not relevant 'rpm_per_tick': 0.114, 'features': [] }, # SEED Robotics Actuator Model Nrs 407: { 'name': 'SEED_XL320EMU', 'encoder_resolution': 4096, 'range_degrees': 360.0, 'torque_per_volt': 0.39 / 7.4, # 0.39 NM @ 7.4V 'velocity_per_volt': (114 * RPM_TO_RADSEC) / 7.4, # 114 RPM @ 7.4V 'rpm_per_tick': 0.114, 'features': [] }, 404: { 'name': 'SEED28_ACTUATOR', 'encoder_resolution': 4096, 'range_degrees': 360.0, 'torque_per_volt': 0.18 / 5.0, # 0.18 NM @ 5V 'velocity_per_volt': (55 * RPM_TO_RADSEC) / 5.0, # 55 RPM @ 5V 'rpm_per_tick': 0.114, 'features': [] }, 406: { 'name': 'SEED56_ACTUATOR', 'encoder_resolution': 4096, 'range_degrees': 360.0, 'torque_per_volt': 0.55 / 5.0, # 0.55 NM @ 5V 'velocity_per_volt': (55 * RPM_TO_RADSEC) / 5.0, # 55 RPM @ 5V 'rpm_per_tick': 0.114, 'features': [] }, 408: { 'name': 'SEED44_ACTUATOR', 'encoder_resolution': 4096, 'range_degrees': 360.0, 'torque_per_volt': 0.4 / 5.0, # 0.4 NM @ 5V 'velocity_per_volt': (60 * RPM_TO_RADSEC) / 5.0, # 60 RPM @ 5V 'rpm_per_tick': 0.114, 'features': [] }, 409: { 'name': 'SEED67_ACTUATOR', 'encoder_resolution': 4096, 'range_degrees': 360.0, 'torque_per_volt': 0.70 / 5.0, # 0.7 NM @ 5V 'velocity_per_volt': (55 * RPM_TO_RADSEC) / 5.0, # 55 RPM @ 5V 'rpm_per_tick': 0.114, 'features': [] }, 410: { 'name': 'SEED58_ACTUATOR', 'encoder_resolution': 4096, 'range_degrees': 360.0, 'torque_per_volt': 0.52 / 5.0, # 0.52 NM @ 5V 'velocity_per_volt': (55 * RPM_TO_RADSEC) / 5.0, # 55 RPM @ 5V 'rpm_per_tick': 0.114, 'features': [] }, # END SEED ADDITIONS
Save and close the file.
4) Run the command find -name dynamixel_serial_proxy.py
5) Now open the filedynamixel_serial_proxy.py
located in the previous step, as sudo
or root
:
(for example sudo gedit [location of the file from step 4] &
, assuming you'd use gedit
to edit the file.
Failing to open the file as sudo
or root
will result in not being able to save it, as it will open read-only.)
Modify the following:
5a) Find the line that contains:
voltage = self.dxl_io.get_voltage(motor_id)
5b) Delete this line and Replace it with the following sequence of code:
(place this code at the exact location where the line in 5a was)
# BEGIN SEED ROBOTICS ADDITIONS if (model_number == 400 or # Seed Robotics Ares board model_number == 405 or # Seed Robotics Eros board model_number == 407 or # Seed Robotics XL_320_EMU model_number == 404 or # Seed Robotics SEED28 actuator model_number == 406 or # Seed Robotics SEED56 actuator model_number == 408 or # Seed Robotics SEED44 actuator model_number == 409 or # Seed Robotics SEED67 actuator model_number == 410): # Seed Robotics SEED58 actuator # internal operation of Seed Robotics actuators # occurs at 5V steady regardless of the voltage applied voltage = 5 else: # default behavior for all other Dynamixel protocol devices voltage = self.dxl_io.get_voltage(motor_id) # END SEED ROBOTICS ADDITIONS
Close and Save the file.
Step 6 and 7 are optional. They should be performed in scenarios when you have Virtualization or other scenarios of increased latency. (under Linux, with an authentic FTDI adapter, these steps are typically not necessary)
These changes resolve errors of (1) being unable to detect the Actuators or (2) correct/unexpected messages (out of sync communication).
6) Run the command find -name dynamixel_io.py
7) Now open the filedynamixel_io.py
located in the previous step, as sudo
or root
:
(for example sudo gedit [location of the file from step 6] &
, assuming you'd use gedit
to edit the file.
Failing to open the file as sudo
or root
will result in not being able to save it, as it will open read-only.)
Modify the following:
7a) Locate the line from dynamixel_const import *
7b) Below that line, type:
#SEED ROBOTICS # Fine tuning the reply wait time. # Default is 0.0013 # If you have communication or latency issues, we recommend, setting this to 0.005 REPLY_WAIT_TIME_SECS = 0.005
7c) Locate the line: time.sleep(0.0013)#0.00235)
7d) Delete and Replace that line with:
time.sleep(REPLY_WAIT_TIME_SECS) # SEED ROBOTICS
7e) Locate the next line: time.sleep(0.0013)
7f) Delete and Replace that line with:
time.sleep(REPLY_WAIT_TIME_SECS) # SEED ROBOTICS
7g) Locate the next line: time.sleep(0.0013)
7h) Delete and Replace that line with:
time.sleep(REPLY_WAIT_TIME_SECS) # SEED ROBOTICS
( Overall there will be 3 replacements of the time.sleep(..)
command in the this file. )
Close and Save the file.
Getting started and Operation
Important!
If you had previously created any catkin
packages that include the dynamixel_controllers package before applying the code changes above, you should delete those packages and recreate them.
To get started with this ROS package, there are a few tutorials that can help you get started:
- #2 Next you need to create the controllers. We recommend creating a
joint controller
for each of the joint ID's on the hand. Depending on your model, you will have a different number of joints available. Again, please refer to the documentation of your model to obtain the default IDs and further information.
Due to the design of our firmware, Seed Robotics units supports joint
controllers only. joint torque
controllers are un tested and unsupported at this point.
How target positions are set and queried
The ROS package maps the RAW position range used by the motors (0~1023/4095 range) to Radians in the range 0 to 2Pi, meaning you read and write values in Radians only.
While Seed Robotics units expose the Raw positions in the 0 to 1023/4095 range (depending on the unit configuration), these don't necessarily map to a 360º angle; however the library observes this abstraction and the users should too. Have this in mind when using the library: 0
will correspond to the minimum position in the motion range of the joint, where 2Pi
will correspond to the maximum position in the range.
Other considerations
The Punch and Compliance Slope parameters - used to tune motion behavior - are not supported by the Seed Robotics units. This is because Seed Robotics employs more advanced PID algortihms for motion control. Therefore, using Punch and Slope functions on the library will not produce any effect.
As a final note, please regard this information as introductory. Seed Robotics aims to design their products in compliance with industry standard, well-supported protocols such as Dynamixel.
The purpose of this document is to introduce users to operating the unit under ROS using the packages that handle the Dynamixel protocol. An exhaustive explanation of ROS or the dynamixel_driver
package (a third party package) is beyond the scope of this document.
However, should you have any specific question or integration issue, get in touch with support@seedrobotics.com.
Copyright © 2015-2023 Seed Robotics Ltd