新增可以控制dynamixel舵机机械臂的代码,5自由度机械臂

src/arbotix_ros/.gitignore

@@ -0,0 +1,4 @@
+*.pyc
+.project
+.cproject
+.pydevproject

src/arbotix_ros/README.md

@@ -0,0 +1,18 @@
+## Arbotix Drivers
+
+This repository contains the Arbotix ROS drivers, catkinized, and ready for ROS Groovy and newer.
+
+## Changes for version 0.11.x
+Controller will now break when goal is reached.  
+Added support for PhantomX Pincher.   
+   To use the PhantomX Pincher, set environment variable "TURTLEBOT_ARM1" to pincher. You will need turtlebot_arm version 0.4.0 or higher for PhantomX Pincher 
+
+## Changes for Groovy (version 0.8.x)
+
+Several executables are now installed in /opt/ros/groovy/bin allowing you to run them without using rosrun:
+ * controllerGUI.py is now arbotix_gui
+ * terminal.py is now arbotix_terminal
+
+Other executables have been renamed to alleviate any name collisions:
+ * driver.py is now renamed arbotix_driver
+

src/arbotix_ros/arbotix/CHANGELOG.rst

@@ -0,0 +1,27 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2014-07-14)
+-------------------
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+
+0.8.2 (2013-03-28)
+------------------
+* update metapackage
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* fix package.xml
+* add metapackage for arbotix

src/arbotix_ros/arbotix/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arbotix)
+find_package(catkin REQUIRED)
+catkin_metapackage()

src/arbotix_ros/arbotix/package.xml

@@ -0,0 +1,23 @@
+<package>
+  <name>arbotix</name>
+  <version>0.10.0</version>
+  <description>ArbotiX Drivers</description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>arbotix_msgs</run_depend>
+  <run_depend>arbotix_python</run_depend>
+  <run_depend>arbotix_sensors</run_depend>
+  <run_depend>arbotix_controllers</run_depend>
+  <run_depend>arbotix_firmware</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>
+
+

src/arbotix_ros/arbotix_controllers/CHANGELOG.rst

@@ -0,0 +1,33 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2014-07-14)
+-------------------
+* Set queue_size=5 on all publishers
+* Check if command exceeds opening limits
+* Contributors: Jorge Santos
+
+0.9.2 (2014-02-12)
+------------------
+* cleanup gripper controllers, mark deprecations
+* Contributors: Michael Ferguson
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+* fix joint_states subscriber
+* fix name of singlesided model
+* add new gripper action controller
+
+0.8.2 (2013-03-28)
+------------------
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* import drivers and catkinize

src/arbotix_ros/arbotix_controllers/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arbotix_controllers)
+
+find_package(catkin REQUIRED)
+catkin_package()
+
+install(
+  PROGRAMS
+    bin/gripper_controller
+    bin/one_side_gripper_controller.py
+    bin/parallel_gripper_controller.py
+    bin/parallel_single_servo_controller.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/arbotix_ros/arbotix_controllers/bin/gripper_controller

@@ -0,0 +1,265 @@
+#!/usr/bin/env python
+
+"""
+  gripper_controller - action based controller for grippers.
+  Copyright (c) 2011-2014 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+import thread
+
+from control_msgs.msg import GripperCommandAction
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+from math import asin, sin
+from multiprocessing import Lock
+from arbotix_python.parallel_convert import *
+
+
+class GripperModel:
+    """ Base Gripper Model """
+    def __init__(self):
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.50)
+        self.center = rospy.get_param('~center', 0.0)
+        self.invert = rospy.get_param('~invert', False)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publisher
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        """ Take an input command of width (m) to open gripper and publish angle to Servo. """
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%.5f) exceeds max limit ( %.5f) or min limit ( %.5f)", command.position, self.max_opening, self.max_opening)
+            return False
+        # compute angle
+        angle = self.widthToAngle(command.position)
+        # publish desired angle
+        if self.invert:
+            res = -angle + self.center
+        else:
+            res = angle + self.center
+        self.pub.publish(res)
+        rospy.loginfo("Gripper publish servo goal: %.4f rad", res )
+        return True
+
+    def getEffort(self, joint_states):
+        # TODO
+        return 1.0
+
+    # Convert opening width in meters into Servo radians
+    def widthToAngle(self, w):
+        return asin((w - self.pad_width)/(2*self.finger_length))
+
+    # Convert gripper position in radians to gap opening in meters
+    def angToWidth(self, ang):
+        if self.invert:
+            return (sin(self.center-ang) * 2 * self.finger_length) + self.pad_width
+        else:
+            return (sin(ang - self.center) * 2 * self.finger_length) + self.pad_width
+
+    # Convert current joint status position (radians) to meters
+    def jstToWidth(self, ang):
+        return self.angToWidth(ang)
+
+
+class TrapezoidGripperModel(GripperModel):
+    """ A simple gripper with two opposing servos to open/close non-parallel jaws. """
+
+    def __init__(self):
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center_l = rospy.get_param('~center_left', 0.0)
+        self.center_r = rospy.get_param('~center_right', 0.0)
+        self.invert_l = rospy.get_param('~invert_left', False)
+        self.invert_r = rospy.get_param('~invert_right', False)
+
+        self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
+        self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
+
+        # publishers
+        self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64, queue_size=5)
+        self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angles
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+        return True
+
+    def getPosition(self, js):
+        left = right = 0
+        for i in range(len(js.name)):
+            if js.name[i] == self.left_joint:
+                left = js.position[i]
+            elif js.name[i] == self.right_joint:
+                right = js.position[i]
+        # TODO
+
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+class ParallelGripperModel(GripperModel):
+	# One servo to open/close parallel jaws, typically via linkage.
+	# TODO - currently we only show one finger, and report twice the distance.  Opening
+	# reports correct width but center is not correct
+    def __init__(self):
+        GripperModel.__init__(self)
+        self.convertor = ParallelConvert(self.joint)
+
+    def widthToAngle(self, width):
+        return self.convertor.widthToAngle(width)
+
+    def angleToWidth(self, ang):
+        return self.convertor.angleToWidth(ang)
+
+    def jstToWidth(self, pos):
+        # For parallel gripper, joint status is already in width (m)
+        return pos
+
+
+class OneSideGripperModel(GripperModel):
+    """ Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
+    # Use base classes for all
+
+
+class GripperActionController:
+    """ The actual action callbacks. """
+    def __init__(self):
+        rospy.init_node('gripper_controller')
+        self.mutex = Lock()
+        self.tolerance = rospy.get_param('~tolerance', .001) # tolerance for meeting goals (radians)
+
+        # setup model
+        try:
+            model = rospy.get_param('~model')
+            self.type = model
+            rospy.loginfo("Started %s Gripper Controller.", model)
+        except:
+            rospy.logerr('no model specified, exiting')
+            exit()
+
+        if model == 'dualservo':
+            self.model = TrapezoidGripperModel()
+        elif model == 'parallel':
+            self.model = ParallelGripperModel()
+        elif model == 'singlesided':
+            self.model = OneSideGripperModel()
+        else:
+            rospy.logerr('unknown model specified, exiting')
+            exit()
+
+        # subscribe to joint_states
+        rospy.Subscriber('joint_states', JointState, self.stateCb)
+
+        # get joint name so we can track our joint status
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+        self.state = JointState()
+        # TODO handle joint names and joint status for Trapezoidal
+
+        # wait for first joint status update, needed for getPosition
+        for i in range(0, 20):
+            if self.getPosition(errlog=False) != -1:
+                break
+            rospy.sleep(0.1)
+
+        # subscribe to command and then spin
+        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
+        self.server.start()
+        rospy.spin()
+
+    def actionCb(self, goal):
+        """ Take an input command of width to open gripper. """
+        rospy.loginfo('Gripper controller action goal recieved:%f m' % goal.command.position)
+        # send command to gripper
+        if self.model.setCommand(goal.command) == False:
+            self.server.set_aborted(text="Command exceeds range")
+            return
+        # publish feedback
+        while True:
+            if self.type == 'trapezodal':            # TODO handle trapezoidal
+                break
+            if self.server.is_preempt_requested():
+                self.server.set_preempted()
+                rospy.logwarn('Gripper Controller: Preempted.')
+                return
+            # get joint position, break when we have reached goal
+            if  abs(self.getPosition() - goal.command.position) < self.tolerance:
+                rospy.loginfo("Done. Pos=%.5f Goal=%.5f Tol=%.5f", self.getPosition(), goal.command.position, self.tolerance)
+                break
+            rospy.sleep(0.01)
+        self.server.set_succeeded()
+        rospy.loginfo('Gripper Controller: Succeeded.')
+
+    def stateCb(self, msg):
+        with self.mutex:
+            if self.joint in msg.name:
+                self.state = msg
+
+    def getPosition(self, errlog=True):
+        """ Return current gripper position from Joint status in meters """
+        with self.mutex:  # make copy of joint state and extract gripper position
+            st = self.state
+        for name, position in zip(st.name, st.position):
+             if name == self.joint:
+                return self.model.jstToWidth(position)
+        if errlog == True:
+            rospy.logerr("No position update for " + self.joint )
+        return -1
+
+if __name__=='__main__':
+    try:
+        GripperActionController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo('Hasta la Vista...')
+

src/arbotix_ros/arbotix_controllers/bin/gripper_controller.py

@@ -0,0 +1,212 @@
+#!/usr/bin/env python
+
+"""
+  gripper_controller - action based controller for grippers.
+  Copyright (c) 2011-2014 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+import thread
+
+from control_msgs.msg import GripperCommandAction
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+from math import asin
+
+class TrapezoidGripperModel:
+    """ A simple gripper with two opposing servos to open/close non-parallel jaws. """
+
+    def __init__(self):
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center_l = rospy.get_param('~center_left', 0.0)
+        self.center_r = rospy.get_param('~center_right', 0.0)
+        self.invert_l = rospy.get_param('~invert_left', False)
+        self.invert_r = rospy.get_param('~invert_right', False)
+
+        self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
+        self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
+
+        # publishers
+        self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64, queue_size=5)
+        self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angles
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+        return True
+
+    def getPosition(self, js):
+        left = right = 0
+        for i in range(len(js.name)):
+            if js.name[i] == self.left_joint:
+                left = js.position[i]
+            elif js.name[i] == self.right_joint:
+                right = js.position[i]
+        # TODO
+
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+class ParallelGripperModel:
+    """ One servo to open/close parallel jaws, typically via linkage. """
+
+    def __init__(self):
+        self.center = rospy.get_param('~center', 0.0)
+        self.scale = rospy.get_param('~scale', 1.0)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publishers
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        self.pub.publish((command.position * self.scale) + self.center)
+
+    def getPosition(self, joint_states):
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+
+class OneSideGripperModel:
+    """ Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
+
+    def __init__(self):
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center = rospy.get_param('~center', 0.0)
+        self.invert = rospy.get_param('~invert', False)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publishers
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angle
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        # publish message
+        if self.invert:
+            self.pub.publish(-angle + self.center)
+        else:
+            self.pub.publish(angle + self.center)
+
+    def getPosition(self, joint_states):
+        # TODO
+        return 0.0
+
+    def getEffort(self, joint_states):
+        # TODO
+        return 1.0
+
+
+class GripperActionController:
+    """ The actual action callbacks. """
+    def __init__(self):
+        rospy.init_node('gripper_controller')
+
+        # setup model
+        try:
+            model = rospy.get_param('~model')
+        except:
+            rospy.logerr('no model specified, exiting')
+            exit()
+        if model == 'dualservo':
+            self.model = TrapezoidGripperModel()
+        elif model == 'parallel':
+            self.model = ParallelGripperModel()
+        elif model == 'singlesided':
+            self.model = OneSideGripperModel()
+        else:
+            rospy.logerr('unknown model specified, exiting')
+            exit()
+
+        # subscribe to joint_states
+        rospy.Subscriber('joint_states', JointState, self.stateCb)
+
+        # subscribe to command and then spin
+        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
+        self.server.start()
+        rospy.spin()
+
+    def actionCb(self, goal):
+        """ Take an input command of width to open gripper. """
+        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
+        # send command to gripper
+        self.model.setCommand(goal.command)
+        # publish feedback
+        while True:
+            if self.server.is_preempt_requested():
+                self.server.set_preemtped()
+                rospy.loginfo('Gripper Controller: Preempted.')
+                return
+            # TODO: get joint position, break when we have reached goal
+            break
+        self.server.set_succeeded()
+        rospy.loginfo('Gripper Controller: Succeeded.')
+
+    def stateCb(self, msg):
+        self.state = msg
+
+if __name__=='__main__':
+    try:
+        rospy.logwarn("Please use gripper_controller (no .py)")
+        GripperActionController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo('Hasta la Vista...')
+

src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_controller.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+
+"""
+  parallel_gripper_controller.py - controls a gripper built of two servos
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+import thread
+
+from std_msgs.msg import Float64
+from math import asin
+
+class ParallelGripperController:
+    """ A simple controller that operates two opposing servos to
+        open/close to a particular size opening. """
+    def __init__(self):
+        rospy.init_node("parallel_gripper_controller")
+        rospy.logwarn("parallel_gripper_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
+
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param("~pad_width", 0.01)
+        self.finger_length = rospy.get_param("~finger_length", 0.02)
+        self.min_opening = rospy.get_param("~min", 0.0)
+        self.max_opening = rospy.get_param("~max", 2*self.finger_length)
+
+        self.center_l = rospy.get_param("~center_left", 0.0)
+        self.center_r = rospy.get_param("~center_right", 0.0)
+        self.invert_l = rospy.get_param("~invert_left", False)
+        self.invert_r = rospy.get_param("~invert_right", False)
+
+        # publishers
+        self.l_pub = rospy.Publisher("l_gripper_joint/command", Float64, queue_size=5)
+        self.r_pub = rospy.Publisher("r_gripper_joint/command", Float64, queue_size=5)
+
+        # subscribe to command and then spin
+        rospy.Subscriber("~command", Float64, self.commandCb)
+        rospy.spin()
+
+    def commandCb(self, msg):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if msg.data > self.max_opening or msg.data < self.min_opening:
+            rospy.logerr("Command exceeds limits.")
+            return
+        # compute angles
+        angle = asin((msg.data - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+
+if __name__=="__main__": 
+    try:
+        ParallelGripperController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Hasta la Vista...")
+        

src/arbotix_ros/arbotix_controllers/bin/parallel_single_servo_controller.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+
+"""
+  parallel_single_servo_controller.py - controls a single-servo parallel-jaw gripper
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, tf
+import thread
+
+from std_msgs.msg import Float64
+from sensor_msgs.msg import JointState
+from math import asin
+
+class ParallelGripperController:
+    """ A simple controller that operates a single servo parallel jaw gripper. """
+    def __init__(self):
+        rospy.init_node("gripper_controller")
+
+        # TODO: load calibration data. Form: opening->servo angle
+        self.calib = { 0.0000 : 1.8097, 0.0159: 1.2167, 0.0254 : 0.8997, 0.0381 : 0.4499, 0.042 : 0.1943 }
+        #self.calib = { 0.0000 : 866, 0.0159: 750, 0.0254 : 688, 0.0381 : 600, 0.042 : 550 }
+
+        # parameters
+        self.min = rospy.get_param("~min", 0.0)
+        self.max = rospy.get_param("~max", 0.042)
+        self.center = rospy.get_param("~center", 512)
+        self.invert = rospy.get_param("~invert", False)
+        
+        # publishers
+        self.commandPub = rospy.Publisher("gripper_joint/command", Float64, queue_size=5)
+        self.br = tf.TransformBroadcaster()
+    
+        # current width of gripper
+        self.width = 0.0
+
+        # subscribe to command and then spin
+        rospy.Subscriber("~command", Float64, self.commandCb)
+        rospy.Subscriber("joint_states", JointState, self.stateCb)
+        
+        r = rospy.Rate(15)
+        while not rospy.is_shutdown():
+            # output tf
+            self.br.sendTransform((0, -self.width/2.0, 0),
+                                   tf.transformations.quaternion_from_euler(0, 0, 0),
+                                   rospy.Time.now(),
+                                   "gripper_left_link",
+                                   "gripper_link")
+            self.br.sendTransform((0, self.width/2.0, 0),
+                                   tf.transformations.quaternion_from_euler(0, 0, 0),
+                                   rospy.Time.now(),
+                                   "gripper_right_link",
+                                   "gripper_link")
+            r.sleep()
+
+    def getCommand(self, width):
+        """ Get servo command for an opening width. """
+        keys = self.calib.keys(); keys.sort()   
+        # find end points of segment
+        low = keys[0]; 
+        high = keys[-1]
+        for w in keys[1:-1]:
+            if w > low and w < width:
+                low = w
+            if w < high and w > width:
+                high = w
+        # linear interpolation
+        scale = (width-low)/(high-low)
+        return ((self.calib[high]-self.calib[low])*scale) + self.calib[low]
+
+    def getWidth(self, command):
+        """ Get opening width for a particular servo command. """
+        reverse_calib = dict()
+        for k, v in self.calib.items():
+            reverse_calib[v] = k
+        keys = reverse_calib.keys(); keys.sort()
+        # find end points of segment
+        low = keys[0]
+        high = keys[-1]
+        for c in keys[1:-1]:
+            if c > low and c < command:
+                low = c
+            if c < high and c > command:
+                high = c
+        # linear interpolation
+        scale = (command-low)/(high-low)
+        return ((reverse_calib[high]-reverse_calib[low])*scale) + reverse_calib[low]
+
+    def commandCb(self, msg):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if msg.data > self.max or msg.data < self.min:
+            rospy.logerr("Command exceeds limits.")
+            return
+        # compute angle
+        self.commandPub.publish( Float64( self.getCommand(msg.data) ) )
+
+    def stateCb(self, msg):
+        """ The callback that listens for joint_states. """
+        try:
+            index = msg.name.index("gripper_joint")
+        except ValueError:
+            return
+        self.width = self.getWidth(msg.position[index])
+
+if __name__=="__main__": 
+    try:
+        ParallelGripperController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Hasta la Vista...")
+        

src/arbotix_ros/arbotix_controllers/package.xml

@@ -0,0 +1,17 @@
+<package>
+  <name>arbotix_controllers</name>
+  <version>0.10.0</version>
+  <description>
+    Extends the arbotix_python package with a number of more sophisticated ROS wrappers for common devices.
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_controllers</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>arbotix_python</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>tf</run_depend>
+</package>

src/arbotix_ros/arbotix_firmware/CHANGELOG.rst

@@ -0,0 +1,25 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_firmware
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2014-07-14)
+-------------------
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+
+0.8.2 (2013-03-28)
+------------------
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* import drivers and catkinize

src/arbotix_ros/arbotix_firmware/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arbotix_firmware)
+
+find_package(catkin REQUIRED)
+catkin_package()

src/arbotix_ros/arbotix_firmware/package.xml

@@ -0,0 +1,13 @@
+<package>
+  <name>arbotix_firmware</name>
+  <version>0.10.0</version>
+  <description>
+    Firmware source code for ArbotiX ROS bindings.
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_firmware</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+</package>

src/arbotix_ros/arbotix_firmware/src/diff_controller.h

@@ -0,0 +1,148 @@
+/* 
+  ArbotiX Firmware for ROS driver
+  Copyright (c) 2009-2011 Vanadium Labs LLC.  All right reserved.
+ 
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+*/ 
+
+#include <Arduino.h>
+#include <math.h>
+
+/* Register Storage */
+int left_pwm;
+int right_pwm;
+int left_speed;
+int right_speed;
+
+/* PID Parameters */
+int Kp;
+int Kd;
+int Ki;
+int Ko;                      // output scale
+int maxAccel;                // maximum acceleration per frame (ticks)
+
+/* PID modes */
+unsigned int PIDmode;
+#define PID_NONE        0
+#define PID_SPEED       1
+
+#define FRAME_RATE      33   // frame rate in millis (30Hz)
+#define FRAMES          30
+unsigned long f_time;        // last frame
+
+unsigned char moving = 0;    // base in motion
+unsigned char paused = 0;    // base was in motion, can resume
+#define MAXOUTPUT       255  // motor PWM
+
+/* Setpoint Info For a Motor */
+typedef struct{
+  int Velocity;              // desired actual speed (count/frame)
+  long Encoder;              // actual reading
+  long PrevEnc;              // last reading
+  int PrevErr;
+  int Ierror;   
+  int output;                // last motor setting
+} SetPointInfo;
+
+SetPointInfo left, right;
+
+/* Initialize PID parameters to something known */
+void setupPID(){
+  // Default values for the PR-MINI
+  Kp = 25;
+  Kd = 30;
+  Ki = 0;
+  Ko = 100;
+  maxAccel = 50;
+  f_time = 0;
+}
+
+/* PID control of motor speed */
+void DoPid(SetPointInfo * p){
+  long Perror;
+  long output;
+  
+  Perror = p->Velocity - (p->Encoder-p->PrevEnc);
+          
+  // Derivative error is the delta Perror
+  output = (Kp*Perror + Kd*(Perror - p->PrevErr) + Ki*p->Ierror)/Ko;
+  p->PrevErr = Perror;
+  p->PrevEnc = p->Encoder;
+  
+  output += p->output;   
+  // Accumulate Integral error *or* Limit output.
+  // Stop accumulating when output saturates
+  if (output >= MAXOUTPUT)
+    output = MAXOUTPUT;
+  else if (output <= -MAXOUTPUT)
+    output = -MAXOUTPUT;
+  else
+    p->Ierror += Perror;
+  
+  p->output = output;
+}
+
+/* Clear accumulators */
+void ClearPID(){
+  PIDmode = 0; moving = 0;
+  left.PrevErr = 0;
+  left.Ierror = 0;
+  left.output = 0;
+  right.PrevErr = 0;
+  right.Ierror = 0;
+  right.output = 0;
+}
+
+/* This is called by the main loop, does a X HZ PID loop. */
+void updatePID(){
+  if((moving > 0) && (PIDmode > 0)){  // otherwise, just return
+    unsigned long j = millis();
+    if(j > f_time){
+      // update encoders
+      left.Encoder = Encoders.left;
+      right.Encoder = Encoders.right;
+      // do PID update on PWM
+      DoPid(&left);
+      DoPid(&right);
+      // set updated motor outputs      
+      if(PIDmode > 0){
+        drive.set(left.output, right.output);
+      }
+      // update timing
+      f_time = j + FRAME_RATE;
+    }
+  }
+}
+
+void clearAll(){
+  PIDmode = 0;
+  left.Encoder = 0;
+  right.Encoder = 0;
+  left.PrevEnc = 0;
+  right.PrevEnc = 0;
+  left.output = 0;
+  right.output = 0;
+  Encoders.Reset();  
+}
+

src/arbotix_ros/arbotix_firmware/src/ros.h

@@ -0,0 +1,76 @@
+/* 
+  Common Definitions for ROS driver ArbotiX Firmware
+  Copyright (c) 2008-2012 Vanadium Labs LLC.  All right reserved.
+ 
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+*/ 
+
+/* ArbotiX (id:253) Instruction Definitions */
+#define ARB_SIZE_POSE       7    // pose size: a single param for size of pose
+#define ARB_LOAD_POSE       8    // load pose: index, then pose positions (# of params = 2*pose_size)
+#define ARB_LOAD_SEQ        9    // seq size: a single param for the size of the seq
+#define ARB_PLAY_SEQ        10   // load seq: index/times (# of params = 3*seq_size)
+#define ARB_LOOP_SEQ        11   // play seq: no params
+//#define ARB_TEST          25   // hardware test: no params
+#define ARB_CONTROL_SETUP   26   // write ids: id of controller, params (usually ids of servos, # of params = pose_size + 1)
+#define ARB_CONTROL_WRITE   27   // write positions: positions in order of servos (# of params = 2*pose_size)
+#define ARB_CONTROL_STAT    28   // retrieve status: id of controller
+#define ARB_SYNC_READ       0x84
+
+/* ArbotiX (id:253) Register Table Definitions */
+#define REG_MODEL_NUMBER_L  0
+#define REG_MODEL_NUMBER_H  1
+#define REG_VERSION         2
+#define REG_ID              3
+#define REG_BAUD_RATE       4
+
+#define REG_DIGITAL_IN0     5  // First block of digital pins to read
+#define REG_DIGITAL_IN1     6
+#define REG_DIGITAL_IN2     7
+#define REG_DIGITAL_IN3     8
+
+#define REG_RESCAN          15
+#define REG_RETURN_LEVEL    16
+#define REG_ALARM_LED       17
+#define REG_ANA_BASE        18  // First Analog Port
+#define REG_SERVO_BASE      26  // Up to 10 servos, each uses 2 bytes (L, then H), pulse width (0, 1000-2000ms)
+#define REG_MOVING          46
+
+#define REG_DIGITAL_OUT0    47  // First digital pin to write
+                                // base + index, bit 1 = value (0,1), bit 0 = direction (0,1)
+
+#define REG_RESERVED        79  // 79 -- 99 are reserved for future use
+#define REG_USER            100 // 
+
+/* Packet Decoding */
+int mode = 0;                   // where we are in the frame
+
+unsigned char id = 0;           // id of this frame
+unsigned char length = 0;       // length of this frame
+unsigned char ins = 0;          // instruction of this frame
+
+unsigned char params[143];      // parameters (match RX-64 buffer size)
+unsigned char index = 0;        // index in param buffer
+
+int checksum;                   // checksum

src/arbotix_ros/arbotix_firmware/src/ros.ino

@@ -0,0 +1,548 @@
+/* 
+  ArbotiX Firmware for ROS driver
+  Copyright (c) 2008-2012 Vanadium Labs LLC.  All right reserved.
+ 
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+*/ 
+
+/* Build Configuration */
+#define USE_BASE            // Enable support for a mobile base
+#define USE_HW_SERVOS       // Enable only 2/8 servos, but using hardware control
+
+#define CONTROLLER_COUNT    5
+/* Hardware Constructs */
+#include <ax12.h>
+#include <BioloidController.h>
+BioloidController controllers[CONTROLLER_COUNT];
+
+#include "ros.h"
+
+#ifdef USE_HW_SERVOS
+  #include <HServo.h>
+  HServo servos[2];
+  int servo_vals[2];            // in millis
+#else 
+  #include <Servo.h>
+  Servo servos[10];
+  int servo_vals[10];           // in millis
+#endif
+
+#ifdef USE_BASE
+  #include <Motors2.h>
+  Motors2 drive = Motors2();
+  #include <EncodersAB.h>
+  #include "diff_controller.h"
+#endif
+
+/* Register Storage */
+unsigned char baud = 7;         // ?
+unsigned char ret_level = 1;    // ?
+unsigned char alarm_led = 0;    // ?
+
+/* Pose & Sequence Structures */
+typedef struct{
+  unsigned char pose;           // index of pose to transition to 
+  int time;                     // time for transition
+} sp_trans_t;
+int poses[30][AX12_MAX_SERVOS]; // poses [index][servo_id-1]
+sp_trans_t sequence[50];        // sequence
+int seqPos;                     // step in current sequence
+
+#include "user_hooks.h"
+
+/* 
+ * Setup Functions
+ */
+void scan(){
+#if defined(AX_RX_SWITCHED)
+  // do a search for devices on the RX bus, default to AX if not found
+  int i;
+  for(i=0;i<AX12_MAX_SERVOS;i++){
+    dynamixel_bus_config[i] = 1;
+    if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
+      dynamixel_bus_config[i] = 0;
+    }
+  }
+#endif
+}
+
+void setup(){
+  Serial.begin(115200);
+  ax12Init(1000000);
+
+#ifdef USE_BASE
+  drive.init();
+  Encoders.Begin();
+  setupPID();
+#endif
+
+#if defined(AX_RX_SWITCHED)
+  delay(1000);
+  scan();
+#endif
+
+  userSetup();
+  pinMode(0,OUTPUT);     // status LED
+}
+
+/*
+ * Handle Write Requests to ArbotiX Registers 
+ */
+unsigned char handleWrite(){
+  int addr  = params[0];  // address to write
+  int bytes = length-3;   // # of bytes left to write
+  int k = 1;              // index in parameters of value to write
+
+  while(bytes > 0){
+    if(addr < REG_BAUD_RATE){
+      return ERR_INSTRUCTION;
+    }else if(addr == REG_BAUD_RATE){
+      UBRR1L = params[k];
+    }else if(addr < REG_RESCAN){
+      return ERR_INSTRUCTION; // can't write digital inputs
+    }else if(addr == REG_RESCAN){
+      scan();
+    }else if(addr == REG_RETURN_LEVEL){
+      ret_level = params[k];
+    }else if(addr == REG_ALARM_LED){
+      // TODO: 
+    }else if(addr < REG_SERVO_BASE){
+      return ERR_INSTRUCTION; // error - analog are read only
+    }else if(addr < REG_MOVING){
+      // write servo
+      int s = addr - REG_SERVO_BASE;
+ #ifdef USE_HW_SERVO
+      if( s >= 4 ){
+ #else
+      if( s >= 20){
+ #endif 
+        return ERR_INSTRUCTION;
+      }else{
+        if( s%2 == 0 ){ // low byte
+          s = s/2;
+          servo_vals[s] = params[k];
+        }else{          // high byte
+          s = s/2;
+          servo_vals[s] += (params[k]<<8);
+          if(servo_vals[s] > 500 && servo_vals[s] < 2500){
+            servos[s].writeMicroseconds(servo_vals[s]);
+            if(!servos[s].attached())            
+              servos[s].attach(s);
+          }else if(servo_vals[s] == 0){
+            servos[s].detach();
+          }
+        }
+      }
+    }else if(addr == REG_MOVING){
+      return ERR_INSTRUCTION;
+    }else if(addr < REG_RESERVED){
+      // write digital pin
+      int pin = addr - REG_DIGITAL_OUT0;
+    #ifdef SERVO_STIK
+      if(pin < 8)
+        pin = pin+24;
+      else
+        pin = pin+5; // servo stick 8 = D13...
+    #endif
+      if(params[k] & 0x02)    // high
+        digitalWrite(pin, HIGH);
+      else
+        digitalWrite(pin, LOW);
+      if(params[k] & 0x01)    // output
+        pinMode(pin, OUTPUT);
+      else
+        pinMode(pin, INPUT);
+    }else{
+      int ret = userWrite(addr, params[k]);
+      if(ret > ERR_NONE) return ret;
+    }
+    addr++;k++;bytes--;
+  }
+  return ERR_NONE;
+}
+
+
+/*
+ * Handle a read from ArbotiX registers.
+ */
+int handleRead(){
+  int checksum = 0;
+  int addr = params[0];
+  int bytes = params[1];
+  
+  unsigned char v;
+  while(bytes > 0){
+    if(addr == REG_MODEL_NUMBER_L){ 
+      v = 44;
+    }else if(addr == REG_MODEL_NUMBER_H){
+      v = 1;  // 300 
+    }else if(addr == REG_VERSION){
+      v = 0;
+    }else if(addr == REG_ID){
+      v = 253;
+    }else if(addr == REG_BAUD_RATE){
+      v = 34; // 56700
+    }else if(addr == REG_DIGITAL_IN0){
+      // digital 0-7
+    #ifdef SERVO_STIK
+      v = PINA;
+    #else
+      v = PINB;
+    #endif
+    }else if(addr == REG_DIGITAL_IN1){
+      // digital 8-15
+    #ifdef SERVO_STIK
+      v = (PINB>>1);
+    #else
+      v = PIND;
+    #endif        
+    }else if(addr == REG_DIGITAL_IN2){
+      // digital 16-23
+      v = PINC;
+    }else if(addr == REG_DIGITAL_IN3){
+      // digital 24-31
+      v = PINA;
+    }else if(addr == REG_RETURN_LEVEL){
+      v = ret_level;
+    }else if(addr == REG_ALARM_LED){
+      // TODO
+    }else if(addr < REG_SERVO_BASE){
+      // send analog reading
+      int x = analogRead(addr-REG_ANA_BASE)>>2;
+      x += analogRead(addr-REG_ANA_BASE)>>2;
+      x += analogRead(addr-REG_ANA_BASE)>>2;
+      x += analogRead(addr-REG_ANA_BASE)>>2;
+      v = x/4;
+    }else if(addr < REG_MOVING){
+      // send servo position
+      v = 0;      
+    }else{
+      v = userRead(addr);  
+    } 
+    checksum += v;
+    Serial.write(v);
+    addr++;bytes--;
+  }
+  
+  return checksum;
+}
+
+int doPlaySeq(){
+  seqPos = 0; int i;
+  while(sequence[seqPos].pose != 0xff){
+    int p = sequence[seqPos].pose;
+    // are we HALT?
+    if(Serial.read() == 'H') return 1;
+    // load pose
+    for(i=0; i<controllers[0].poseSize; i++)
+      controllers[0].setNextPose(i+1,poses[p][i]); 
+    controllers[0].interpolateSetup(sequence[seqPos].time);
+    while(controllers[0].interpolating)
+      controllers[0].interpolateStep();
+    // next transition
+    seqPos++;
+  }
+  return 0;
+}
+
+/*
+ * Send status packet
+ */
+void statusPacket(int id, int err){
+  Serial.write(0xff);
+  Serial.write(0xff);
+  Serial.write(id);
+  Serial.write(2);
+  Serial.write(err);
+  Serial.write(255-((id+2+err)%256));
+}
+
+/* 
+ * decode packets: ff ff id length ins params checksum
+ *   same as ax-12 table, except, we define new instructions for Arbotix 
+ */
+void loop(){
+  int i;
+    
+  // process messages
+  while(Serial.available() > 0){
+    // We need to 0xFF at start of packet
+    if(mode == 0){         // start of new packet
+      if(Serial.read() == 0xff){
+        mode = 2;
+        digitalWrite(0,HIGH-digitalRead(0));
+      }
+    //}else if(mode == 1){   // another start byte
+    //    if(Serial.read() == 0xff)
+    //        mode = 2;
+    //    else
+    //        mode = 0;
+    }else if(mode == 2){   // next byte is index of servo
+      id = Serial.read();    
+      if(id != 0xff)
+        mode = 3;
+    }else if(mode == 3){   // next byte is length
+      length = Serial.read();
+      checksum = id + length;
+      mode = 4;
+    }else if(mode == 4){   // next byte is instruction
+      ins = Serial.read();
+      checksum += ins;
+      index = 0;
+      mode = 5;
+    }else if(mode == 5){   // read data in 
+      params[index] = Serial.read();
+      checksum += (int) params[index];
+      index++;
+      if(index + 1 == length){  // we've read params & checksum
+        mode = 0;
+        if((checksum%256) != 255){ 
+          // return an error packet: FF FF id Len Err=bad checksum, params=None check
+          statusPacket(id, ERR_CHECKSUM);
+        }else if(id == 253){  // ID = 253, ArbotiX instruction
+          switch(ins){     
+            case AX_WRITE_DATA:
+              // send return packet
+              statusPacket(id,handleWrite());
+              break;
+             
+            case AX_READ_DATA:
+              checksum = id + params[1] + 2;                            
+              Serial.write(0xff);
+              Serial.write(0xff);
+              Serial.write(id);
+              Serial.write((unsigned char)2+params[1]);
+              Serial.write((unsigned char)0);
+              // send actual data
+              checksum += handleRead();
+              Serial.write(255-((checksum)%256));
+              break;
+             
+            case ARB_SIZE_POSE:                   // Pose Size = 7, followed by single param: size of pose
+              statusPacket(id,0);
+              if(controllers[0].poseSize == 0)
+                controllers[0].setup(18);
+              controllers[0].poseSize = params[0];
+              controllers[0].readPose();    
+              break;
+             
+            case ARB_LOAD_POSE:                   // Load Pose = 8, followed by index, then pose positions (# of param = 2*pose_size)
+              statusPacket(id,0);
+              for(i=0; i<controllers[0].poseSize; i++)
+                poses[params[0]][i] = params[(2*i)+1]+(params[(2*i)+2]<<8); 
+              break;
+             
+            case ARB_LOAD_SEQ:                    // Load Seq = 9, followed by index/times (# of parameters = 3*seq_size) 
+              statusPacket(id,0);
+              for(i=0;i<(length-2)/3;i++){
+                sequence[i].pose = params[(i*3)];
+                sequence[i].time = params[(i*3)+1] + (params[(i*3)+2]<<8);
+              }
+              break;
+             
+            case ARB_PLAY_SEQ:                   // Play Seq = A, no params   
+              statusPacket(id,0);
+              doPlaySeq();
+              break;
+             
+            case ARB_LOOP_SEQ:                   // Play Seq until we recieve a 'H'alt
+              statusPacket(id,0);
+              while(doPlaySeq() > 0);
+              break;
+
+            // ARB_TEST is deprecated and removed
+
+            case ARB_CONTROL_SETUP:              // Setup a controller
+              statusPacket(id,0);
+              if(params[0] < CONTROLLER_COUNT){
+                controllers[params[0]].setup(length-3);
+                for(int i=0; i<length-3; i++){
+                  controllers[params[0]].setId(i, params[i+1]);
+                }
+#ifdef USE_BASE
+              }else if(params[0] == 10){
+                Kp = params[1];
+                Kd = params[2];
+                Ki = params[3];
+                Ko = params[4];
+#endif
+              }
+              break;
+
+            case ARB_CONTROL_WRITE:              // Write values to a controller
+              statusPacket(id,0);
+              if(params[0] < CONTROLLER_COUNT){
+                for(int i=0; i<length-4; i+=2){
+                  controllers[params[0]].setNextPose(controllers[params[0]].getId(i/2), params[i+1]+(params[i+2]<<8));
+                }
+                controllers[params[0]].readPose();
+                controllers[params[0]].interpolateSetup(params[length-3]*33);
+#ifdef USE_BASE
+              }else if(params[0] == 10){
+                left_speed = params[1];
+                left_speed += (params[2]<<8);
+                right_speed = params[3];
+                right_speed += (params[4]<<8); 
+                if((left_speed == 0) && (right_speed == 0)){
+                  drive.set(0,0);
+                  ClearPID();
+                }else{
+                  if((left.Velocity == 0) && (right.Velocity == 0)){
+                    PIDmode = 1; moving = 1;
+                    left.PrevEnc = Encoders.left;
+                    right.PrevEnc = Encoders.right;
+                  }
+                }   
+                left.Velocity = left_speed;
+                right.Velocity = right_speed; 
+#endif
+              }
+              break;
+
+            case ARB_CONTROL_STAT:               // Read status of a controller
+              if(params[0] < CONTROLLER_COUNT){             
+                Serial.write((unsigned char)0xff);
+                Serial.write((unsigned char)0xff);
+                Serial.write((unsigned char)id);
+                Serial.write((unsigned char)3);
+                Serial.write((unsigned char)0);
+                checksum = controllers[params[0]].interpolating;
+                Serial.write((unsigned char)checksum);
+                checksum += id + 3;
+                Serial.write((unsigned char)255-((checksum)%256));
+#ifdef USE_BASE
+              }else if(params[0] == 10){
+                checksum = id + 2 + 8;                            
+                Serial.write((unsigned char)0xff);
+                Serial.write((unsigned char)0xff);
+                Serial.write((unsigned char)id);
+                Serial.write((unsigned char)2+8);
+                Serial.write((unsigned char)0);   // error level
+                int v = ((unsigned long)Encoders.left>>0)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.left>>8)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.left>>16)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.left>>24)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.right>>0)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.right>>8)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.right>>16)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                v = ((unsigned long)Encoders.right>>24)%256;
+                Serial.write((unsigned char)v);
+                checksum += v;
+                Serial.write((unsigned char)255-((checksum)%256));
+#endif
+              }
+              break;
+          }
+        }else if(id == 0xFE){
+          // sync read or write
+          if(ins == ARB_SYNC_READ){
+            int start = params[0];    // address to read in control table
+            int bytes = params[1];    // # of bytes to read from each servo
+            int k = 2;
+            checksum = id + (bytes*(length-4)) + 2;                            
+            Serial.write((unsigned char)0xff);
+            Serial.write((unsigned char)0xff);
+            Serial.write((unsigned char)id);
+            Serial.write((unsigned char)2+(bytes*(length-4)));
+            Serial.write((unsigned char)0);     // error code
+            // send actual data
+            for(k=2; k<length-2; k++){
+              if( ax12GetRegister(params[k], start, bytes) >= 0){
+                for(i=0;i<bytes;i++){
+                  checksum += ax_rx_buffer[5+i];
+                  Serial.write((unsigned char)ax_rx_buffer[5+i]);
+                }
+              }else{
+                for(i=0;i<bytes;i++){
+                  checksum += 255;
+                  Serial.write((unsigned char)255);
+                }
+              }
+            }
+            Serial.write((unsigned char)255-((checksum)%256));
+          }else{    
+            // TODO: sync write pass thru
+            int k;
+            setTXall();
+            ax12write(0xff);
+            ax12write(0xff);
+            ax12write(id);
+            ax12write(length);
+            ax12write(ins);
+            for(k=0; k<length; k++)
+                ax12write(params[k]);
+            // no return
+          }       
+        }else{ // ID != 253, pass thru 
+          switch(ins){
+            // TODO: streamline this
+            case AX_READ_DATA:
+              ax12GetRegister(id, params[0], params[1]);
+              // return a packet: FF FF id Len Err params check
+              if(ax_rx_buffer[3] > 0){
+                for(i=0;i<ax_rx_buffer[3]+4;i++)
+                  Serial.write(ax_rx_buffer[i]);
+              }
+              ax_rx_buffer[3] = 0;
+              break;
+             
+            case AX_WRITE_DATA:
+              if(length == 4){
+                ax12SetRegister(id, params[0], params[1]);
+              }else{
+                int x = params[1] + (params[2]<<8);
+                ax12SetRegister2(id, params[0], x);
+              }
+              statusPacket(id,0);
+              break;
+             
+          }
+        }
+      }
+    } // end mode == 5
+  } // end while(available)
+  // update joints
+  for(int i=0; i<5; i++)
+    controllers[i].interpolateStep();
+ 
+#ifdef USE_BASE
+  // update pid
+  updatePID();
+#endif
+
+}

src/arbotix_ros/arbotix_firmware/src/user_hooks.h

@@ -0,0 +1,22 @@
+/*
+ * This file can be used to define custom register addresses
+ * Just use addresses 100 and above
+ */
+
+void userSetup()
+{
+  // do any setup here
+}
+
+unsigned char userWrite(int addr, unsigned char param)
+{
+  // use the register value
+  return ERR_INSTRUCTION;
+}
+
+/* Read one byte from register located at addr */
+int userRead(int addr)
+{
+  // return the register value
+  return 0; 
+}

src/arbotix_ros/arbotix_msgs/CHANGELOG.rst

@@ -0,0 +1,28 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2014-07-14)
+-------------------
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+* Added set_speed service to servo controller
+
+0.9.0 (2013-08-22)
+------------------
+* add new enable service
+
+0.8.2 (2013-03-28)
+------------------
+* updates to cmakelists.txt and package.xml, fixes `#2 <https://github.com/vanadiumlabs/arbotix_ros/issues/2>`_
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* import drivers and catkinize

src/arbotix_ros/arbotix_msgs/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arbotix_msgs)
+
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
+
+add_message_files(FILES
+                  Analog.msg
+                  Digital.msg
+                 )
+
+add_service_files(FILES
+                  Enable.srv
+                  Relax.srv
+                  SetupChannel.srv
+                  SetSpeed.srv
+                 )
+
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

src/arbotix_ros/arbotix_msgs/msg/Analog.msg

@@ -0,0 +1,3 @@
+# Reading from a single analog IO pin.
+Header header
+uint16 value

src/arbotix_ros/arbotix_msgs/msg/Digital.msg

@@ -0,0 +1,14 @@
+# Reading or command to a single digital IO pin.
+Header header
+
+# value of pin
+uint8 LOW=0
+uint8 HIGH=255
+
+uint8 value
+
+# direction of pin
+uint8 INPUT=0
+uint8 OUTPUT=255
+
+uint8 direction

src/arbotix_ros/arbotix_msgs/package.xml

@@ -0,0 +1,21 @@
+<package>
+  <name>arbotix_msgs</name>
+  <version>0.10.0</version>
+  <description>
+    Messages and Services definitions for the ArbotiX.
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_msgs</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+</package>
+
+

src/arbotix_ros/arbotix_msgs/srv/Enable.srv

@@ -0,0 +1,3 @@
+bool enable
+---
+bool state

src/arbotix_ros/arbotix_msgs/srv/Relax.srv

@@ -0,0 +1,3 @@
+
+---
+

src/arbotix_ros/arbotix_msgs/srv/SetSpeed.srv

@@ -0,0 +1,2 @@
+float64 speed
+---

src/arbotix_ros/arbotix_msgs/srv/SetupChannel.srv

@@ -0,0 +1,7 @@
+# message to setup an IO channel
+string topic_name
+uint8 pin
+uint8 value
+uint8 rate
+---
+

src/arbotix_ros/arbotix_python/CHANGELOG.rst

@@ -0,0 +1,36 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_python
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2014-07-14)
+-------------------
+* Set queue_size=5 on all publishers
+* Contributors: Jorge Santos
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+* set velocity when in sim/fake mode
+* Added set_speed service to servo controller
+* Added 'set spd' option to arbotix_terminal
+
+0.9.0 (2013-08-22)
+------------------
+* Add new enable service
+* remove roslib manifest loading
+* remove old dynamixels block, closes `#6 <https://github.com/vanadiumlabs/arbotix_ros/issues/6>`_
+* Warn of extra joints in joint trajectory, but only fail when missing a joint we control
+
+0.8.2 (2013-03-28)
+------------------
+
+0.8.1 (2013-03-09)
+------------------
+* fix depend for proper release
+
+0.8.0 (2013-02-21)
+------------------
+* fix follow controller issues with zeros in header timestamps, cleanup logging a bit
+* import drivers and catkinize

src/arbotix_ros/arbotix_python/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arbotix_python)
+
+find_package(catkin REQUIRED)
+catkin_package()
+
+catkin_python_setup()
+
+install(DIRECTORY demos
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(PROGRAMS bin/arbotix_driver
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/arbotix_ros/arbotix_python/bin/arbotix_driver

@@ -0,0 +1,181 @@
+#!/usr/bin/env python
+
+"""
+  ArbotiX Node: serial connection to an ArbotiX board w/ PyPose/NUKE/ROS
+  Copyright (c) 2008-2011 Michael E. Ferguson.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+import sys
+
+from arbotix_msgs.msg import *
+from arbotix_msgs.srv import *
+
+from arbotix_python.arbotix import ArbotiX
+from arbotix_python.diff_controller import DiffController
+from arbotix_python.follow_controller import FollowController
+from arbotix_python.servo_controller import *
+from arbotix_python.linear_controller import *
+from arbotix_python.publishers import *
+from arbotix_python.io import *
+
+# name: [ControllerClass, pause]
+controller_types = { "follow_controller" : FollowController,
+                     "diff_controller"   : DiffController,
+#                    "omni_controller"   : OmniController,
+                     "linear_controller" : LinearControllerAbsolute,
+                     "linear_controller_i" : LinearControllerIncremental }
+
+###############################################################################
+# Main ROS interface
+class ArbotixROS(ArbotiX):
+    
+    def __init__(self):
+        pause = False
+
+        # load configurations    
+        port = rospy.get_param("~port", "/dev/arbotix")
+        baud = int(rospy.get_param("~baud", "115200"))
+
+        self.rate = rospy.get_param("~rate", 100.0)
+        self.fake = rospy.get_param("~sim", False)
+
+        self.use_sync_read = rospy.get_param("~sync_read",True)      # use sync read?
+        self.use_sync_write = rospy.get_param("~sync_write",True)    # use sync write?
+
+        # setup publishers
+        self.diagnostics = DiagnosticsPublisher()
+        self.joint_state_publisher = JointStatePublisher()
+
+        # start an arbotix driver
+        if not self.fake:
+            ArbotiX.__init__(self, port, baud)        
+            rospy.sleep(1.0)
+            rospy.loginfo("Started ArbotiX connection on port " + port + ".")
+        else:
+            rospy.loginfo("ArbotiX being simulated.")
+
+        # setup joints
+        self.joints = dict()
+        for name in rospy.get_param("~joints", dict()).keys():
+            joint_type = rospy.get_param("~joints/"+name+"/type", "dynamixel")
+            if joint_type == "dynamixel":
+                self.joints[name] = DynamixelServo(self, name)
+            elif joint_type == "prismatic":
+                self.joints[name] = PrismaticDynamixelServo(self, name)
+            elif joint_type == "hobby_servo":
+                self.joints[name] = HobbyServo(self, name)
+            elif joint_type == "calibrated_linear":
+                self.joints[name] = LinearJoint(self, name)
+            else:
+                rospy.logerr("Unknown joint type [%s] for joints/%s", joint_type, name )
+
+        # setup controller
+        self.controllers = [ServoController(self, "servos"), ]
+        controllers = rospy.get_param("~controllers", dict())
+        for name, params in controllers.items():
+            try:
+                controller = controller_types[params["type"]](self, name)
+                self.controllers.append( controller )
+                pause = pause or controller.pause
+            except Exception as e:
+                if type(e) == KeyError:
+                    rospy.logerr("Unrecognized controller: " + params["type"])
+                else:  
+                    rospy.logerr(str(type(e)) + str(e))
+
+        # wait for arbotix to start up (especially after reset)
+        if not self.fake:
+            if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"):
+                pause = True
+            if pause:
+                while self.getDigital(1) == -1 and not rospy.is_shutdown():
+                    rospy.loginfo("Waiting for response...")
+                    rospy.sleep(0.25)
+            rospy.loginfo("ArbotiX connected.")
+
+        for controller in self.controllers:
+            controller.startup()
+
+        # services for io
+        rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb)
+        rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb)
+        rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb)
+        # initialize digital/analog IO streams
+        self.io = dict()
+        if not self.fake:
+            for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items():
+                temp = rospy.get_param("~"+v,dict())
+                for name in temp.keys():
+                    pin = rospy.get_param('~'+v+'/'+name+'/pin',1)
+                    value = rospy.get_param('~'+v+'/'+name+'/value',0)
+                    rate = rospy.get_param('~'+v+'/'+name+'/rate',10)
+                    leng = rospy.get_param('~'+v+'/'+name+'/length',1)  # just for analog sensors
+                    self.io[name] = t(name, pin, value, rate, leng, self)
+        
+        r = rospy.Rate(self.rate)
+
+        # main loop -- do all the read/write here
+        while not rospy.is_shutdown():
+    
+            # update controllers
+            for controller in self.controllers:
+                controller.update()
+
+            # update io
+            for io in self.io.values():
+                io.update()
+
+            # publish feedback
+            self.joint_state_publisher.update(self.joints.values(), self.controllers)
+            self.diagnostics.update(self.joints.values(), self.controllers)
+
+            r.sleep()
+
+        # do shutdown
+        for controller in self.controllers:
+            controller.shutdown()
+
+    def analogInCb(self, req):
+        # TODO: Add check, only 1 service per pin
+        if not self.fake:
+            self.io[req.topic_name] = AnalogSensor(req.topic_name, req.pin, req.value, req.rate, req.leng, self)
+        return SetupChannelResponse()
+
+    def digitalInCb(self, req):
+        if not self.fake:
+            self.io[req.topic_name] = DigitalSensor(req.topic_name, req.pin, req.value, req.rate, self) 
+        return SetupChannelResponse()
+
+    def digitalOutCb(self, req):
+        if not self.fake:
+            self.io[req.topic_name] = DigitalServo(req.topic_name, req.pin, req.value, req.rate, self) 
+        return SetupChannelResponse()
+
+
+if __name__ == "__main__":
+    rospy.init_node('arbotix')
+    a = ArbotixROS()
+

src/arbotix_ros/arbotix_python/bin/arbotix_gui

@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+
+"""
+  A simple Controller GUI to drive robots and pose heads.
+  Copyright (c) 2008-2011 Michael E. Ferguson.  All right reserved.
+"""
+
+import rospy
+import wx
+
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+from arbotix_msgs.srv import Relax
+from arbotix_python.joints import *
+
+
+class servoSlider():
+    def __init__(self, parent, min_angle, max_angle, name, i):
+        self.name = name
+        if name.find("_joint") > 0:   # remove _joint for display name
+            name = name[0:-6]
+        self.position = wx.Slider(parent, -1, 0, int(min_angle*100), int(max_angle*100), wx.DefaultPosition, (150, -1), wx.SL_HORIZONTAL)
+        self.enabled = wx.CheckBox(parent, i, name+":")
+        self.enabled.SetValue(False)
+        self.position.Disable()
+
+    def setPosition(self, angle):
+        self.position.SetValue(int(angle*100))
+
+    def getPosition(self):
+        return self.position.GetValue()/100.0
+
+class controllerGUI(wx.Frame):
+    TIMER_ID = 100
+
+    def __init__(self, parent, debug = False):
+        wx.Frame.__init__(self, parent, -1, "Arm Controller GUI", style = wx.DEFAULT_FRAME_STYLE & ~ (wx.RESIZE_BORDER | wx.MAXIMIZE_BOX))
+        sizer = wx.GridBagSizer(5,5)
+
+        # Move Servos
+        servo = wx.StaticBox(self, -1, 'Move Servos')
+        servo.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
+        servoBox = wx.StaticBoxSizer(servo, orient=wx.VERTICAL)
+        servoSizer = wx.GridBagSizer(5, 5)
+
+        joint_defaults = getJointsFromURDF()
+
+        i = 0
+        dynamixels = rospy.get_param('/arbotix/dynamixels', dict())
+        self.servos = list()
+        self.publishers = list()
+        self.relaxers = list()
+
+        joints = rospy.get_param('/arbotix/joints', dict())
+        # create sliders and publishers
+        for name in sorted(joints.keys()):
+            # pull angles
+            min_angle, max_angle = getJointLimits(name, joint_defaults)
+
+            # create publisher
+            self.publishers.append(rospy.Publisher(name+'/command', Float64, queue_size=5))
+            if rospy.get_param('/arbotix/joints/'+name+'/type','dynamixel') == 'dynamixel':
+                self.relaxers.append(rospy.ServiceProxy(name+'/relax', Relax))
+            else:
+                self.relaxers.append(None)
+
+            # create slider
+            s = servoSlider(self, min_angle, max_angle, name, i)
+            servoSizer.Add(s.enabled,(i,0), wx.GBSpan(1,1),  wx.ALIGN_CENTER_VERTICAL)
+            servoSizer.Add(s.position,(i,1), wx.GBSpan(1,1), wx.ALIGN_CENTER_VERTICAL)
+            self.servos.append(s)
+            i += 1
+
+        # add everything
+        servoBox.Add(servoSizer)
+        sizer.Add(servoBox, (0,1), wx.GBSpan(1,1), wx.EXPAND|wx.TOP|wx.BOTTOM|wx.RIGHT,5)
+        self.Bind(wx.EVT_CHECKBOX, self.enableSliders)
+        # now we can subscribe
+        rospy.Subscriber('joint_states', JointState, self.stateCb)
+
+        # timer for output
+        self.timer = wx.Timer(self, self.TIMER_ID)
+        self.timer.Start(50)
+        wx.EVT_CLOSE(self, self.onClose)
+        wx.EVT_TIMER(self, self.TIMER_ID, self.onTimer)
+
+        self.SetSizerAndFit(sizer)
+        self.Show(True)
+
+    def onClose(self, event):
+        self.timer.Stop()
+        self.Destroy()
+
+    def enableSliders(self, event):
+        servo = event.GetId()
+        if event.IsChecked():
+            self.servos[servo].position.Enable()
+        else:
+            self.servos[servo].position.Disable()
+            if self.relaxers[servo]:
+                self.relaxers[servo]()
+
+    def stateCb(self, msg):
+        for servo in self.servos:
+            if not servo.enabled.IsChecked():
+                try:
+                    idx = msg.name.index(servo.name)
+                    servo.setPosition(msg.position[idx])
+                except:
+                    pass
+
+    def onTimer(self, event=None):
+        # send joint updates
+        for s,p in zip(self.servos,self.publishers):
+            if s.enabled.IsChecked():
+                d = Float64()
+                d.data = s.getPosition()
+                p.publish(d)
+
+if __name__ == '__main__':
+    # initialize GUI
+    rospy.init_node('controllerGUI')
+    app = wx.App(False)
+    frame = controllerGUI(None, True)
+    app.MainLoop()
+

src/arbotix_ros/arbotix_python/bin/arbotix_terminal

@@ -0,0 +1,189 @@
+#!/usr/bin/env python
+
+"""
+  ArbotiX Terminal - command line terminal to interact with an ArbotiX
+  Copyright (c) 2008-2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import sys
+
+from arbotix_python.arbotix import ArbotiX # does this look ridiculous to anyone else?
+from arbotix_python.ax12 import *
+
+# help phrases
+help = ["ArbotiX Terminal V0.1",
+"",
+"valid commands:",
+" ls [i b]- list the servos found on the bus. Optional parameters: i - highest ID to query, b - baudrate to query at.",
+" mv id id2 - rename any servo with ID=id, to id2",
+" baud b - set baud rate of bus to b",
+" get param id - get a parameter value from a servo",
+" set param id val - set parameter on servo ID=id to val",
+"",
+"valid parameters",
+" pos - current position of a servo, 0-1023",
+" spd - current goal speed of a servo, 0-1023",
+" baud - baud rate",
+" temp - current temperature, degrees C, READ ONLY"]
+
+
+class Terminal(ArbotiX):
+    OKBLUE = '\033[94m'
+    OKGREEN = '\033[92m'
+    WARNING = '\033[93m'
+    FAIL = '\033[91m'
+    ENDC = '\033[0m'
+
+    def __init__(self, port = "/dev/ttyUSB0", baud = 115200):
+        # start
+        ArbotiX.__init__(self, port, baud)
+        print "ArbotiX Terminal --- Version 0.1"
+        print "Copyright 2011 Vanadium Labs LLC"
+
+        # loop
+        while True:
+            print ">> ",
+            kmd = raw_input().split(" ")
+            try:
+                if kmd[0] == "help":        # display help data
+                    if len(kmd) > 1:        # for a particular command
+                        if kmd[1] == "ls":
+                            print help[3]
+                        elif kmd[1] == "mv":
+                            print help[4]
+                        elif kmd[1] == "baud":
+                            print help[5]
+                        elif kmd[1] == "get":
+                            print help[6]
+                        elif kmd[1] == "set":
+                            print help[7]
+                        else:
+                            print "help: unrecognized command"
+                    else:
+                        for h in help:
+                            print h
+
+                elif kmd[0] == "ls":       # list servos
+                    self._ser.timeout = 0.25
+                    if len(kmd) > 2:
+                        self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))])
+                        self.query()
+                    self.query()
+
+                elif kmd[0] == "mv":         # rename a servo
+                    if self.write( int(kmd[1]), P_ID, [int(kmd[2]),] ) == 0:
+                        print self.OKBLUE+"OK"+self.ENDC
+
+                elif kmd[0] == "baud":      # set bus baud rate
+                    self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))])
+                    print self.OKBLUE+"OK"+self.ENDC
+
+                elif kmd[0] == "set":
+                    if kmd[1] == "baud":
+                        self.write( int(kmd[2]), P_BAUD_RATE, [self.convertBaud(int(kmd[3]))] )
+                        print self.OKBLUE+"OK"+self.ENDC
+                    elif kmd[1] == "pos" or kmd[1] == "position":
+                        self.setPosition( int(kmd[2]), int(kmd[3]) )
+                        print self.OKBLUE+"OK"+self.ENDC
+                    elif kmd[1] == "spd" or kmd[1] == "speed":
+                        self.setSpeed( int(kmd[2]), int(kmd[3]) )
+                        print self.OKBLUE+"OK"+self.ENDC
+
+                elif kmd[0] == "get":
+                    if kmd[1] == "temp":
+                        value = self.getTemperature(int(kmd[2]))
+                        if value >= 60 or value < 0:
+                            print self.FAIL+str(value)+self.ENDC
+                        elif value > 40:
+                            print self.WARNING+str(value)+self.ENDC
+                        else:
+                            print self.OKGREEN+str(value)+self.ENDC
+                    elif kmd[1] == "pos" or kmd[1] == "position":
+                        value = self.getPosition(int(kmd[2]))
+                        if value >= 0:
+                            print self.OKGREEN+str(value)+self.ENDC
+                        else:
+                            print self.FAIL+str(value)+self.ENDC
+                    elif kmd[1] == "spd" or kmd[1] == "speed":
+                        value = self.getGoalSpeed(int(kmd[2]))
+                        if value >= 0:
+                            print self.OKGREEN+str(value)+self.ENDC
+                        else:
+                            print self.FAIL+str(value)+self.ENDC
+
+            except Exception as e:
+                print "error...", e
+
+    def query(self, max_id = 18, baud = 1000000):
+        k = 0                   # how many id's have we printed
+        for i in range(max_id):
+            if self.getPosition(i+1) != -1:
+                if k > 8:
+                    k = 0
+                    print ""
+                print repr(i+1).rjust(4),
+                k = k + 1
+            else:
+                if k > 8:
+                    k = 0
+                    print ""
+                print "....",
+                k = k + 1
+            sys.stdout.flush()
+        print ""
+
+    def convertBaud(self, b):
+        if b == 500000:
+            return 3
+        elif b == 400000:
+            return 4
+        elif b == 250000:
+            return 7
+        elif b == 200000:
+            return 9
+        elif b == 115200:
+            return 16
+        elif b == 57600:
+            return 34
+        elif b == 19200:
+            return 103
+        elif b == 9600:
+            return 207
+        else:
+            return 1    # default to 1Mbps
+
+
+if __name__ == "__main__":
+    try:
+        if len(sys.argv) > 2:
+            t = Terminal(sys.argv[1], int(sys.argv[2]))
+        elif len(sys.argv) > 1:
+            t = Terminal(sys.argv[1])
+        else:
+            t = Terminal()
+    except KeyboardInterrupt:
+        print "\nExiting..."
+
+

src/arbotix_ros/arbotix_python/demos/diagnostics.yaml

@@ -0,0 +1,13 @@
+pub_rate: 1.0 # Optional
+base_path: '' # Optional, prepended to all diagnostic output
+analyzers:
+  joints:
+    type: GenericAnalyzer
+    path: 'Joints'
+    timeout: 5.0
+    contains: '_joint'
+  encoders:
+    type: GenericAnalyzer
+    path: 'Controllers'
+    timeout: 5.0
+    contains: '_controller'

src/arbotix_ros/arbotix_python/mainpage.dox

@@ -0,0 +1,11 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+In addition to the ROS API offered by the driver.py node, arbotix_python offers a Python API for interacting with the ArbotiX or other Dynamixel-like devices.
+
+\section ArbotixPythonAPI ArbotiX Python API
+- \link arbotix_python::arbotix::ArbotiX ArbotiX Class API \endlink
+- \link arbotix_python::ax12 Dynamixel register table defintions \endlink
+
+*/

src/arbotix_ros/arbotix_python/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>arbotix_python</name>
+  <version>0.10.0</version>
+  <description>
+    Bindings and low-level controllers for ArbotiX-powered robots.
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_python</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>arbotix_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>diagnostic_msgs</run_depend>
+  <run_depend>control_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>python-serial</run_depend>
+</package>
+
+

src/arbotix_ros/arbotix_python/setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    scripts=['bin/arbotix_gui', 'bin/arbotix_terminal'],
+    packages=['arbotix_python'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)

src/arbotix_ros/arbotix_python/src/arbotix_python/arbotix.py

@@ -0,0 +1,517 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2008-2011 Vanadium Labs LLC. 
+# All right reserved.
+#
+# 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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+
+# Author: Michael Ferguson
+
+## @file arbotix.py Low-level code to control an ArbotiX.
+
+import serial, time, sys, thread
+from ax12 import *
+from struct import unpack
+
+## @brief This class controls an ArbotiX, USBDynamixel, or similar board through a serial connection.
+class ArbotiX:
+
+    ## @brief Constructs an ArbotiX instance and opens the serial connection.
+    ##
+    ## @param port The name of the serial port to open.
+    ## 
+    ## @param baud The baud rate to run the port at. 
+    ##
+    ## @param timeout The timeout to use for the port. When operating over a wireless link, you may need to
+    ## increase this.
+    def __init__(self, port="/dev/arbotix",baud=115200, timeout = 0.1):
+        self._mutex = thread.allocate_lock()
+        self._ser = serial.Serial()
+        
+        self._ser.baudrate = baud
+        self._ser.port = port
+        self._ser.timeout = timeout
+        self._ser.open()
+
+        ## The last error level read back
+        self.error = 0
+
+    ## @brief Read a dynamixel return packet in an iterative attempt.
+    ##
+    ## @param mode This should be 0 to start reading packet. 
+    ##
+    ## @return The error level returned by the device. 
+    def getPacket(self, mode, id=-1, leng=-1, error=-1, params = None):
+        try:
+            d = self._ser.read()     
+        except Exception as e:
+            print e
+            return None
+        # need a positive byte
+        if d == '':
+            return None
+
+        # now process our byte
+        if mode == 0:           # get our first 0xFF
+            if ord(d) == 0xff:   
+                return self.getPacket(1)
+            else:
+                return self.getPacket(0)
+        elif mode == 1:         # get our second 0xFF
+            if ord(d) == 0xff:
+                return self.getPacket(2)
+            else:
+                return self.getPacket(0)
+        elif mode == 2:         # get id
+            if d != 0xff:
+                return self.getPacket(3, ord(d))
+            else:              
+                return self.getPacket(0)
+        elif mode == 3:         # get length
+            return self.getPacket(4, id, ord(d))
+        elif mode == 4:         # read error    
+            self.error = ord(d)
+            if leng == 2:
+                return self.getPacket(6, id, leng, ord(d), list())
+            else:
+                return self.getPacket(5, id, leng, ord(d), list())
+        elif mode == 5:         # read params
+            params.append(ord(d))
+            if len(params) + 2 == leng:
+                return self.getPacket(6, id, leng, error, params)
+            else:
+                return self.getPacket(5, id, leng, error, params)
+        elif mode == 6:         # read checksum
+            checksum = id + leng + error + sum(params) + ord(d)
+            if checksum % 256 != 255:
+                return None
+            return params
+        # fail
+        return None
+
+    ## @brief Send an instruction to the device. 
+    ##
+    ## @param index The ID of the servo to write.
+    ##
+    ## @param ins The instruction to send.
+    ##
+    ## @param params A list of the params to send.
+    ##
+    ## @param ret Whether to read a return packet.
+    ##
+    ## @return The return packet, if read.
+    def execute(self, index, ins, params, ret=True):
+        values = None
+        self._mutex.acquire()  
+        try:      
+            self._ser.flushInput()
+        except Exception as e:
+            print e
+        length = 2 + len(params)
+        checksum = 255 - ((index + length + ins + sum(params))%256)
+        try: 
+            self._ser.write(chr(0xFF)+chr(0xFF)+chr(index)+chr(length)+chr(ins))
+        except Exception as e:
+            print e
+            self._mutex.release()
+            return None
+        for val in params:
+            try:
+                self._ser.write(chr(val))
+            except Exception as e:
+                print e
+                self._mutex.release()
+                return None
+        try:
+            self._ser.write(chr(checksum))
+        except Exception as e:
+            print e
+            self._mutex.release()
+            return None
+        if ret:
+            values = self.getPacket(0)
+        self._mutex.release()
+        return values
+    
+    ## @brief Read values of registers.
+    ##
+    ## @param index The ID of the servo.
+    ## 
+    ## @param start The starting register address to begin the read at.
+    ##
+    ## @param length The number of bytes to read.
+    ##
+    ## @return A list of the bytes read, or -1 if failure.
+    def read(self, index, start, length):
+        values = self.execute(index, AX_READ_DATA, [start, length])
+        if values == None:
+            return -1        
+        else:
+            return values
+
+    ## @brief Write values to registers.
+    ##
+    ## @param index The ID of the servo.
+    ##
+    ## @param start The starting register address to begin writing to.
+    ##
+    ## @param values The data to write, in a list.
+    ##
+    ## @return The error level.
+    def write(self, index, start, values):
+        self.execute(index, AX_WRITE_DATA, [start] + values)
+        return self.error     
+
+    ## @brief Write values to registers on many servos.
+    ##
+    ## @param start The starting register address to begin writing to.
+    ##
+    ## @param values The data to write, in a list of lists. Format should be
+    ## [(id1, val1, val2), (id2, val1, val2)]
+    def syncWrite(self, start, values):
+        output = list()
+        for i in values:
+            output = output + i 
+        length = len(output) + 4                # length of overall packet
+        lbytes = len(values[0])-1               # length of bytes to write to a servo               
+        self._mutex.acquire()  
+        try:      
+            self._ser.flushInput()
+        except:
+            pass  
+        self._ser.write(chr(0xFF)+chr(0xFF)+chr(254)+chr(length)+chr(AX_SYNC_WRITE))        
+        self._ser.write(chr(start))              # start address
+        self._ser.write(chr(lbytes))             # bytes to write each servo
+        for i in output:
+            self._ser.write(chr(i))
+        checksum = 255 - ((254 + length + AX_SYNC_WRITE + start + lbytes + sum(output))%256)
+        self._ser.write(chr(checksum))
+        self._mutex.release()
+
+    ## @brief Read values of registers on many servos.
+    ##
+    ## @param servos A list of the servo IDs to read from.
+    ##
+    ## @param start The starting register address to begin reading at.
+    ##
+    ## @param length The number of bytes to read from each servo.
+    ##
+    ## @return A list of bytes read.
+    def syncRead(self, servos, start, length):
+        return self.execute(0xFE, AX_SYNC_READ, [start, length] + servos )
+    
+    ## @brief Set baud rate of a device.
+    ##
+    ## @param index The ID of the device to write (Note: ArbotiX is 253).
+    ##
+    ## @param baud The baud rate.
+    ##
+    ## @return The error level.
+    def setBaud(self, index, baud):
+        return self.write(index, P_BAUD_RATE, [baud, ])
+
+    ## @brief Get the return level of a device.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return The return level, .
+    def getReturnLevel(self, index):
+        try:
+            return int(self.read(index, P_RETURN_LEVEL, 1)[0])
+        except:
+            return -1
+
+    ## @brief Set the return level of a device.
+    ##
+    ## @param index The ID of the device to write.
+    ##
+    ## @param value The return level.
+    ##
+    ## @return The error level.
+    def setReturnLevel(self, index, value):
+        return self.write(index, P_RETURN_LEVEL, [value])        
+
+    ## @brief Turn on the torque of a servo.
+    ##
+    ## @param index The ID of the device to enable.
+    ##
+    ## @return The error level.
+    def enableTorque(self, index):
+        return self.write(index, P_TORQUE_ENABLE, [1])
+
+    ## @brief Turn on the torque of a servo.
+    ##
+    ## @param index The ID of the device to disable.
+    ##
+    ## @return The error level.
+    def disableTorque(self, index):
+        return self.write(index, P_TORQUE_ENABLE, [0])
+
+    ## @brief Set the status of the LED on a servo.
+    ##
+    ## @param index The ID of the device to write.
+    ##
+    ## @param value 0 to turn the LED off, >0 to turn it on
+    ##
+    ## @return The error level.
+    def setLed(self, index, value):
+        return self.write(index, P_LED, [value])
+
+    ## @brief Set the position of a servo.
+    ##
+    ## @param index The ID of the device to write.
+    ##
+    ## @param value The position to go to in, in servo ticks.
+    ##
+    ## @return The error level.
+    def setPosition(self, index, value):
+        return self.write(index, P_GOAL_POSITION_L, [value%256, value>>8])
+
+    ## @brief Set the speed of a servo.
+    ##
+    ## @param index The ID of the device to write.
+    ##
+    ## @param value The speed to write.
+    ##
+    ## @return The error level.
+    def setSpeed(self, index, value):
+        return self.write(index, P_GOAL_SPEED_L, [value%256, value>>8])
+
+    ## @brief Get the position of a servo.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return The servo position.
+    def getPosition(self, index):
+        values = self.read(index, P_PRESENT_POSITION_L, 2)
+        try:
+            return int(values[0]) + (int(values[1])<<8)
+        except:
+            return -1
+
+    ## @brief Get the speed of a servo.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return The servo speed.
+    def getSpeed(self, index):
+        values = self.read(index, P_PRESENT_SPEED_L, 2)
+        try:
+            return int(values[0]) + (int(values[1])<<8)
+        except:
+            return -1
+        
+    ## @brief Get the goal speed of a servo.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return The servo goal speed.
+    def getGoalSpeed(self, index):
+        values = self.read(index, P_GOAL_SPEED_L, 2)
+        try:
+            return int(values[0]) + (int(values[1])<<8)
+        except:
+            return -1
+
+    ## @brief Get the voltage of a device.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return The voltage, in Volts.
+    def getVoltage(self, index):
+        try:
+            return int(self.read(index, P_PRESENT_VOLTAGE, 1)[0])/10.0
+        except:
+            return -1    
+
+    ## @brief Get the temperature of a device.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return The temperature, in degrees C.
+    def getTemperature(self, index):
+        try:
+            return int(self.read(index, P_PRESENT_TEMPERATURE, 1)[0])
+        except:
+            return -1
+
+    ## @brief Determine if a device is moving.
+    ##
+    ## @param index The ID of the device to read.
+    ##
+    ## @return True if servo is moving.
+    def isMoving(self, index):
+        try:
+            d = self.read(index, P_MOVING, 1)[0]
+        except:
+            return True
+        return d != 0
+    
+    ## @brief Put a servo into wheel mode (continuous rotation).
+    ##
+    ## @param index The ID of the device to write.
+    def enableWheelMode(self, index):
+        self.write(index, P_CCW_ANGLE_LIMIT_L, [0,0])
+
+    ## @brief Put a servo into servo mode.
+    ##
+    ## @param index The ID of the device to write.
+    ##
+    ## @param resolution The resolution of the encoder on the servo. NOTE: if using 
+    ## 12-bit resolution servos (EX-106, MX-28, etc), you must pass resolution = 12.
+    ##
+    ## @return 
+    def disableWheelMode(self, index, resolution=10):
+        resolution = (2 ** resolution) - 1
+        self.write(index, P_CCW_ANGLE_LIMIT_L, [resolution%256,resolution>>8])
+
+    ## Direction definition for setWheelSpeed
+    FORWARD = 0
+    ## Direction definition for setWheelSpeed
+    BACKWARD = 1
+
+    ## @brief Set the speed and direction of a servo which is in wheel mode (continuous rotation).
+    ##
+    ## @param index The ID of the device to write.
+    ##
+    ## @param direction The direction of rotation, either FORWARD or BACKWARD
+    ##
+    ## @param speed The speed to move at (0-1023).
+    ##
+    ## @return 
+    def setWheelSpeed(self, index, direction, speed):
+        if speed > 1023:
+            speed = 1023
+        if direction == self.FORWARD:
+            # 0~1023 is forward, it is stopped by setting to 0 while rotating to CCW direction.
+            self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
+        else:
+            # 1024~2047 is backward, it is stopped by setting to 1024 while rotating to CW direction.
+            speed += 1024
+            self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
+
+    ###########################################################################
+    # Extended ArbotiX Driver
+
+    ## Helper definition for analog and digital access.
+    LOW = 0
+    ## Helper definition for analog and digital access.
+    HIGH = 0xff
+    ## Helper definition for analog and digital access.
+    INPUT = 0
+    ## Helper definition for analog and digital access.
+    OUTPUT = 0xff
+
+    # ArbotiX-specific register table
+    # We do Model, Version, ID, Baud, just like the AX-12
+    ## Register base address for reading digital ports
+    REG_DIGITAL_IN0 = 5
+    REG_DIGITAL_IN1 = 6
+    REG_DIGITAL_IN2 = 7
+    REG_DIGITAL_IN3 = 8
+    ## Register address for triggering rescan
+    REG_RESCAN = 15
+    # 16, 17 = RETURN, ALARM
+    ## Register address of first analog port (read only).
+    ## Each additional port is BASE + index.
+    ANA_BASE = 18
+    ## Register address of analog servos. Up to 10 servos, each
+    ## uses 2 bytes (L, then H), pulse width (0, 1000-2000ms) (Write only)
+    SERVO_BASE = 26
+    # Address 46 is Moving, just like an AX-12
+    REG_DIGITAL_OUT0 = 47
+
+    ## @brief Force the ArbotiX2 to rescan the Dynamixel busses.
+    def rescan(self):
+        self.write(253, self.REG_RESCAN, [1,])
+
+    ## @brief Get the value of an analog input pin.
+    ##
+    ## @param index The ID of the pin to read (0 to 7).
+    ##
+    ## @param leng The number of bytes to read (1 or 2).
+    ##
+    ## @return 8-bit/16-bit analog value of the pin, -1 if error.
+    def getAnalog(self, index, leng=1):
+        try:
+            val = self.read(253, self.ANA_BASE+int(index), leng)
+            return sum(val[i] << (i * 8) for i in range(leng))
+        except:
+            return -1
+
+    ## @brief Get the value of an digital input pin.
+    ##
+    ## @param index The ID of the pin to read (0 to 31).
+    ##
+    ## @return 0 for low, 255 for high, -1 if error.
+    def getDigital(self, index):
+        try:
+            if index < 32:
+                x = self.read(253, self.REG_DIGITAL_IN0 + int(index/8), 1)[0]
+            else:
+                return -1
+        except:
+            return -1
+        if x & (2**(index%8)):
+            return 255
+        else:
+            return 0
+
+    ## @brief Get the value of an digital input pin.
+    ##
+    ## @param index The ID of the pin to write (0 to 31).
+    ##
+    ## @param value The value of the port, >0 is high.
+    ##
+    ## @param direction The direction of the port, >0 is output.
+    ##
+    ## @return -1 if error.
+    def setDigital(self, index, value, direction=0xff):
+        if index > 31: return -1
+        if value == 0 and direction > 0:
+            self.write(253, self.REG_DIGITAL_OUT0 + int(index), [1])
+        elif value > 0 and direction > 0:
+            self.write(253, self.REG_DIGITAL_OUT0 + int(index), [3])
+        elif value > 0 and direction == 0:
+            self.write(253, self.REG_DIGITAL_OUT0 + int(index), [2])
+        else:
+            self.write(253, self.REG_DIGITAL_OUT0 + int(index), [0])
+        return 0
+
+    ## @brief Set the position of a hobby servo.
+    ##
+    ## @param index The ID of the servo to write (0 to 7).
+    ##
+    ## @param value The position of the servo in milliseconds (1500-2500). 
+    ## A value of 0 disables servo output.
+    ##
+    ## @return -1 if error.
+    def setServo(self, index, value):
+        if index > 7: return -1
+        if value != 0 and (value < 500 or value > 2500):
+            print "ArbotiX Error: Servo value out of range:", value
+        else:
+            self.write(253, self._SERVO_BASE + 2*index, [value%256, value>>8])
+        return 0
+

src/arbotix_ros/arbotix_python/src/arbotix_python/ax12.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2008-2011 Vanadium Labs LLC. 
+# All right reserved.
+#
+# 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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+
+# Author: Michael Ferguson
+
+## @file ax12.py Definitions of AX-12 control table.
+
+# Control Table Symbolic Constants - EEPROM AREA
+P_MODEL_NUMBER_L = 0
+P_MODEL_NUMBER_H = 1
+P_VERSION = 2
+P_ID = 3
+P_BAUD_RATE = 4
+P_RETURN_DELAY_TIME = 5
+P_CW_ANGLE_LIMIT_L = 6
+P_CW_ANGLE_LIMIT_H = 7
+P_CCW_ANGLE_LIMIT_L = 8
+P_CCW_ANGLE_LIMIT_H = 9
+P_SYSTEM_DATA2 = 10
+P_LIMIT_TEMPERATURE = 11
+P_DOWN_LIMIT_VOLTAGE = 12
+P_UP_LIMIT_VOLTAGE = 13
+P_MAX_TORQUE_L = 14
+P_MAX_TORQUE_H = 15
+P_RETURN_LEVEL = 16
+P_ALARM_LED = 17
+P_ALARM_SHUTDOWN = 18
+P_OPERATING_MODE = 19
+P_DOWN_CALIBRATION_L = 20
+P_DOWN_CALIBRATION_H = 21
+P_UP_CALIBRATION_L = 22
+P_UP_CALIBRATION_H = 23
+# Control Table Symbolic Constants - RAM AREA
+P_TORQUE_ENABLE = 24
+P_LED = 25
+P_CW_COMPLIANCE_MARGIN = 26
+P_CCW_COMPLIANCE_MARGIN = 27
+P_CW_COMPLIANCE_SLOPE = 28
+P_CCW_COMPLIANCE_SLOPE = 29
+P_GOAL_POSITION_L = 30
+P_GOAL_POSITION_H = 31
+P_GOAL_SPEED_L = 32
+P_GOAL_SPEED_H = 33
+P_TORQUE_LIMIT_L = 34
+P_TORQUE_LIMIT_H = 35
+P_PRESENT_POSITION_L = 36
+P_PRESENT_POSITION_H = 37
+P_PRESENT_SPEED_L = 38
+P_PRESENT_SPEED_H = 39
+P_PRESENT_LOAD_L = 40
+P_PRESENT_LOAD_H = 41
+P_PRESENT_VOLTAGE = 42
+P_PRESENT_TEMPERATURE = 43
+P_REGISTERED_INSTRUCTION = 44
+P_PAUSE_TIME = 45
+P_MOVING = 46
+P_LOCK = 47
+P_PUNCH_L = 48
+P_PUNCH_H = 49
+
+# Status Return Levels
+AX_RETURN_NONE = 0
+AX_RETURN_READ = 1
+AX_RETURN_ALL = 2
+
+# Instruction Set
+AX_PING = 1
+AX_READ_DATA = 2
+AX_WRITE_DATA = 3
+AX_REG_WRITE = 4
+AX_ACTION = 5
+AX_RESET = 6
+AX_SYNC_WRITE = 131
+AX_SYNC_READ = 132
+
+AX_CONTROL_SETUP = 26
+AX_CONTROL_WRITE = 27
+AX_CONTROL_STAT = 28
+

src/arbotix_ros/arbotix_python/src/arbotix_python/controllers.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2010-2011 Vanadium Labs LLC. 
+# All right reserved.
+#
+# 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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+
+## @file controllers.py Base class and support functions for a controllers.
+
+## @brief Controllers interact with ArbotiX hardware.
+class Controller:
+
+    ## @brief Constructs a Controller instance.
+    ##
+    ## @param device The arbotix instance.
+    ## 
+    ## @param name The controller name.
+    def __init__(self, device, name):
+        self.name = name
+        self.device = device
+        self.fake = device.fake
+        self.pause = False
+
+        # output for joint states publisher
+        self.joint_names = list()
+        self.joint_positions = list()
+        self.joint_velocities = list()
+
+    ## @brief Start the controller, do any hardware setup needed.
+    def startup(self):
+        pass
+
+    ## @brief Do any read/writes to device.
+    def update(self):
+        pass
+
+    ## @brief Stop the controller, do any hardware shutdown needed.
+    def shutdown(self):
+        pass
+
+    ## @brief Is the controller actively sending commands to joints?
+    def active(self):
+        return False
+        
+    ## @brief Get a diagnostics message for this joint.
+    ##
+    ## @return Diagnostics message. 
+    def getDiagnostics(self):
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        msg.level = DiagnosticStatus.OK
+        msg.message = "OK"
+        return msg
+

src/arbotix_ros/arbotix_python/src/arbotix_python/diff_controller.py

@@ -0,0 +1,261 @@
+#!/usr/bin/env python
+
+"""
+  diff_controller.py - controller for a differential drive
+  Copyright (c) 2010-2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+
+from math import sin,cos,pi
+
+from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from diagnostic_msgs.msg import *
+from tf.broadcaster import TransformBroadcaster
+
+from ax12 import *
+from controllers import *
+from struct import unpack
+
+class DiffController(Controller):
+    """ Controller to handle movement & odometry feedback for a differential 
+            drive mobile base. """
+    def __init__(self, device, name):
+        Controller.__init__(self, device, name)
+        self.pause = True
+        self.last_cmd = rospy.Time.now()
+
+        # parameters: rates and geometry
+        self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
+        self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
+        self.t_delta = rospy.Duration(1.0/self.rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+        self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
+        self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))
+
+        self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
+        self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')
+
+        # parameters: PID
+        self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
+        self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
+        self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
+        self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)
+
+        # parameters: acceleration
+        self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
+        self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)
+
+        # output for joint states publisher
+        self.joint_names = ["base_l_wheel_joint","base_r_wheel_joint"]
+        self.joint_positions = [0,0]
+        self.joint_velocities = [0,0]
+
+        # internal data            
+        self.v_left = 0                 # current setpoint velocity
+        self.v_right = 0
+        self.v_des_left = 0             # cmd_vel setpoint
+        self.v_des_right = 0
+        self.enc_left = None            # encoder readings
+        self.enc_right = None
+        self.x = 0                      # position in xy plane
+        self.y = 0
+        self.th = 0
+        self.dx = 0                     # speeds in x/rotation
+        self.dr = 0
+        self.then = rospy.Time.now()    # time for determining dx/dy
+
+        # subscriptions
+        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
+        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
+        self.odomBroadcaster = TransformBroadcaster()
+		
+        rospy.loginfo("Started DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
+
+    def startup(self):
+        if not self.fake:
+            self.setup(self.Kp,self.Kd,self.Ki,self.Ko) 
+    
+    def update(self):
+        now = rospy.Time.now()
+        if now > self.t_next:
+            elapsed = now - self.then
+            self.then = now
+            elapsed = elapsed.to_sec()
+
+            if self.fake:
+                x = cos(self.th)*self.dx*elapsed
+                y = -sin(self.th)*self.dx*elapsed
+                self.x += cos(self.th)*self.dx*elapsed
+                self.y += sin(self.th)*self.dx*elapsed
+                self.th += self.dr*elapsed
+            else:
+                # read encoders
+                try:
+                    left, right = self.status()
+                except Exception as e:
+                    rospy.logerr("Could not update encoders: " + str(e))
+                    return
+                rospy.logdebug("Encoders: " + str(left) +","+ str(right))
+
+                # calculate odometry
+                if self.enc_left == None:
+                    d_left = 0
+                    d_right = 0
+                else:
+                    d_left = (left - self.enc_left)/self.ticks_meter
+                    d_right = (right - self.enc_right)/self.ticks_meter
+                self.enc_left = left
+                self.enc_right = right
+
+                d = (d_left+d_right)/2
+                th = (d_right-d_left)/self.base_width
+                self.dx = d / elapsed
+                self.dr = th / elapsed
+
+                if (d != 0):
+                    x = cos(th)*d
+                    y = -sin(th)*d
+                    self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
+                    self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
+                if (th != 0):
+                    self.th = self.th + th
+
+            # publish or perish
+            quaternion = Quaternion()
+            quaternion.x = 0.0 
+            quaternion.y = 0.0
+            quaternion.z = sin(self.th/2)
+            quaternion.w = cos(self.th/2)
+            self.odomBroadcaster.sendTransform(
+                (self.x, self.y, 0), 
+                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
+                rospy.Time.now(),
+                self.base_frame_id,
+                self.odom_frame_id
+                )
+
+            odom = Odometry()
+            odom.header.stamp = now
+            odom.header.frame_id = self.odom_frame_id
+            odom.pose.pose.position.x = self.x
+            odom.pose.pose.position.y = self.y
+            odom.pose.pose.position.z = 0
+            odom.pose.pose.orientation = quaternion
+            odom.child_frame_id = self.base_frame_id
+            odom.twist.twist.linear.x = self.dx
+            odom.twist.twist.linear.y = 0
+            odom.twist.twist.angular.z = self.dr
+            self.odomPub.publish(odom)
+
+            if now > (self.last_cmd + rospy.Duration(self.timeout)):
+                self.v_des_left = 0
+                self.v_des_right = 0
+
+            # update motors
+            if not self.fake:
+                if self.v_left < self.v_des_left:
+                    self.v_left += self.max_accel
+                    if self.v_left > self.v_des_left:
+                        self.v_left = self.v_des_left
+                else:
+                    self.v_left -= self.max_accel
+                    if self.v_left < self.v_des_left:
+                        self.v_left = self.v_des_left
+                
+                if self.v_right < self.v_des_right:
+                    self.v_right += self.max_accel
+                    if self.v_right > self.v_des_right:
+                        self.v_right = self.v_des_right
+                else:
+                    self.v_right -= self.max_accel
+                    if self.v_right < self.v_des_right:
+                        self.v_right = self.v_des_right
+                self.write(self.v_left, self.v_right)
+
+            self.t_next = now + self.t_delta
+ 
+    def shutdown(self):
+        if not self.fake:
+            self.write(0,0)
+
+    def cmdVelCb(self,req):
+        """ Handle movement requests. """
+        self.last_cmd = rospy.Time.now()
+        if self.fake:
+            self.dx = req.linear.x        # m/s
+            self.dr = req.angular.z       # rad/s
+        else:
+            # set motor speeds in ticks per 1/30s
+            self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
+            self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
+
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        msg.level = DiagnosticStatus.OK
+        msg.message = "OK"
+        if not self.fake:
+            msg.values.append(KeyValue("Left", str(self.enc_left)))
+            msg.values.append(KeyValue("Right", str(self.enc_right)))
+        msg.values.append(KeyValue("dX", str(self.dx)))
+        msg.values.append(KeyValue("dR", str(self.dr)))
+        return msg
+
+    ###
+    ### Controller Specification: 
+    ###
+    ###  setup: Kp, Kd, Ki, Ko (all unsigned char)
+    ###
+    ###  write: left_speed, right_speed (2-byte signed, ticks per frame)
+    ###
+    ###  status: left_enc, right_enc (4-byte signed)
+    ### 
+    
+    def setup(self, kp, kd, ki, ko):
+        success = self.device.execute(253, AX_CONTROL_SETUP, [10, kp, kd, ki, ko])
+
+    def write(self, left, right):
+        """ Send a closed-loop speed. Base PID loop runs at 30Hz, these values
+                are therefore in ticks per 1/30 second. """
+        left = left&0xffff
+        right = right&0xffff
+        success = self.device.execute(253, AX_CONTROL_WRITE, [10, left%256, left>>8, right%256, right>>8])
+
+    def status(self):
+        """ read 32-bit (signed) encoder values. """
+        values = self.device.execute(253, AX_CONTROL_STAT, [10])
+        left_values = "".join([chr(k) for k in values[0:4] ])        
+        right_values = "".join([chr(k) for k in values[4:] ])
+        try:
+            left = unpack('=l',left_values)[0]
+            right = unpack('=l',right_values)[0]
+            return [left, right]
+        except:
+            return None
+

src/arbotix_ros/arbotix_python/src/arbotix_python/follow_controller.py

@@ -0,0 +1,165 @@
+#!/usr/bin/env python
+
+"""
+  follow_controller.py - controller for a kinematic chain
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+
+from control_msgs.msg import FollowJointTrajectoryAction
+from trajectory_msgs.msg import JointTrajectory
+from diagnostic_msgs.msg import *
+
+from ax12 import *
+from controllers import *
+
+class FollowController(Controller):
+    """ A controller for joint chains, exposing a FollowJointTrajectory action. """
+
+    def __init__(self, device, name):
+        Controller.__init__(self, device, name)
+        self.interpolating = 0
+
+        # parameters: rates and joints
+        self.rate = rospy.get_param('~controllers/'+name+'/rate',50.0)
+        self.joints = rospy.get_param('~controllers/'+name+'/joints')
+        self.index = rospy.get_param('~controllers/'+name+'/index', len(device.controllers))
+        for joint in self.joints:
+            self.device.joints[joint].controller = self
+
+        # action server
+        name = rospy.get_param('~controllers/'+name+'/action_name','follow_joint_trajectory')
+        self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
+
+        # good old trajectory
+        rospy.Subscriber(self.name+'/command', JointTrajectory, self.commandCb)
+        self.executing = False
+
+        rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints) + " on C" + str(self.index))
+
+    def startup(self):
+        self.server.start()
+
+    def actionCb(self, goal):
+        rospy.loginfo(self.name + ": Action goal recieved.")
+        traj = goal.trajectory
+
+        if set(self.joints) != set(traj.joint_names):
+            for j in self.joints:
+                if j not in traj.joint_names:
+                    msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
+                    rospy.logerr(msg)
+                    self.server.set_aborted(text=msg)
+                    return
+            rospy.logwarn("Extra joints in trajectory")
+
+        if not traj.points:
+            msg = "Trajectory empy."
+            rospy.logerr(msg)
+            self.server.set_aborted(text=msg)
+            return
+
+        try:
+            indexes = [traj.joint_names.index(joint) for joint in self.joints]
+        except ValueError as val:
+            msg = "Trajectory invalid."
+            rospy.logerr(msg)
+            self.server.set_aborted(text=msg)
+            return
+
+        if self.executeTrajectory(traj):   
+            self.server.set_succeeded()
+        else:
+            self.server.set_aborted(text="Execution failed.")
+
+        rospy.loginfo(self.name + ": Done.")
+    
+    def commandCb(self, msg):
+        # don't execute if executing an action
+        if self.server.is_active():
+            rospy.loginfo(self.name+": Received trajectory, but action is active")
+            return
+        self.executing = True
+        self.executeTrajectory(msg)
+        self.executing = False    
+
+    def executeTrajectory(self, traj):
+        rospy.loginfo("Executing trajectory")
+        rospy.logdebug(traj)
+        # carry out trajectory
+        try:
+            indexes = [traj.joint_names.index(joint) for joint in self.joints]
+        except ValueError as val:
+            rospy.logerr("Invalid joint in trajectory.")
+            return False
+
+        # get starting timestamp, MoveIt uses 0, need to fill in
+        start = traj.header.stamp
+        if start.secs == 0 and start.nsecs == 0:
+            start = rospy.Time.now()
+
+        r = rospy.Rate(self.rate)
+        last = [ self.device.joints[joint].position for joint in self.joints ]
+        for point in traj.points:
+            while rospy.Time.now() + rospy.Duration(0.01) < start:
+                rospy.sleep(0.01)
+            desired = [ point.positions[k] for k in indexes ]
+            endtime = start + point.time_from_start
+            while rospy.Time.now() + rospy.Duration(0.01) < endtime:
+                err = [ (d-c) for d,c in zip(desired,last) ]
+                velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ]
+                rospy.logdebug(err)
+                for i in range(len(self.joints)):
+                    if err[i] > 0.001 or err[i] < -0.001:
+                        cmd = err[i] 
+                        top = velocity[i]
+                        if cmd > top:
+                            cmd = top
+                        elif cmd < -top:
+                            cmd = -top
+                        last[i] += cmd
+                        self.device.joints[self.joints[i]].setControlOutput(last[i])
+                    else:
+                        velocity[i] = 0
+                r.sleep()
+        return True
+
+    def active(self):
+        """ Is controller overriding servo internal control? """
+        return self.server.is_active() or self.executing
+
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        msg.level = DiagnosticStatus.OK
+        msg.message = "OK"
+        if self.active():
+            msg.values.append(KeyValue("State", "Active"))
+        else:
+            msg.values.append(KeyValue("State", "Not Active"))
+        return msg
+

src/arbotix_ros/arbotix_python/src/arbotix_python/io.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+
+"""
+  io.py - ROS wrappers for ArbotiX I/O
+  Copyright (c) 2010-2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+from arbotix_msgs.msg import *
+
+class DigitalServo:
+    """ Class for a digital output. """
+    def __init__(self, name, pin, value, rate, device):
+        self.device = device
+        self.value = value
+        self.direction = 0
+        self.pin = pin
+        self.device.setDigital(self.pin, self.value, self.direction)
+        rospy.Subscriber('~'+name, Digital, self.stateCb)
+        self.t_delta = rospy.Duration(1.0/rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+    def stateCb(self, msg):
+        self.value = msg.value
+        self.direction = msg.direction
+    def update(self):
+        if rospy.Time.now() > self.t_next:
+            self.device.setDigital(self.pin, self.value, self.direction)
+            self.t_next = rospy.Time.now() + self.t_delta
+
+class DigitalSensor:
+    """ Class for a digital input. """
+    def __init__(self, name, pin, value, rate, device):
+        self.device = device
+        self.pin = pin
+        self.device.setDigital(pin, value, 0)
+        self.pub = rospy.Publisher('~'+name, Digital, queue_size=5)
+        self.t_delta = rospy.Duration(1.0/rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+    def update(self):
+        if rospy.Time.now() > self.t_next:
+            msg = Digital()
+            msg.header.stamp = rospy.Time.now()
+            msg.value = self.device.getDigital(self.pin)
+            self.pub.publish(msg)
+            self.t_next = rospy.Time.now() + self.t_delta
+
+class AnalogSensor:
+    """ Class for an analog input. """
+    def __init__(self, name, pin, value, rate, leng, device):
+        self.device = device
+        self.pin = pin
+        self.device.setDigital(pin, value, 0)
+        self.pub = rospy.Publisher('~'+name, Analog, queue_size=5)
+        self.t_delta = rospy.Duration(1.0/rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+        self.leng = leng
+    def update(self):
+        if rospy.Time.now() > self.t_next:
+            msg = Analog()
+            msg.header.stamp = rospy.Time.now()
+            msg.value = self.device.getAnalog(self.pin, self.leng)
+            if msg.value >= 0:
+                self.pub.publish(msg)
+            self.t_next = rospy.Time.now() + self.t_delta
+

src/arbotix_ros/arbotix_python/src/arbotix_python/joints.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2010-2011 Vanadium Labs LLC. 
+# All right reserved.
+#
+# 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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+
+## @file joints.py Base class and support functions for joints.
+
+## @brief Joints hold current values.
+class Joint:
+
+    ## @brief Constructs a Joint instance.
+    ##
+    ## @param device The arbotix instance.
+    ## 
+    ## @param name The joint name.
+    def __init__(self, device, name):
+        self.device = device
+        self.name = name
+        self.controller = None
+
+        self.position = 0.0
+        self.velocity = 0.0
+        self.last = rospy.Time.now()
+
+    ## @brief Get new output, in raw data format.
+    ##
+    ## @param frame The frame length in seconds to interpolate forward.
+    ##
+    ## @return The new output, in raw data format. 
+    def interpolate(self, frame):
+        return None
+
+    ## @brief Set the current position from feedback data.
+    ##
+    ## @param raw_data The current feedback.
+    ##
+    ## @return The current position, in radians/meters. 
+    def setCurrentFeedback(self, raw_data):
+        return None
+
+    ## @brief Set the goal position.
+    ##
+    ## @param position The goal position, in radians/meters. 
+    ##
+    ## @return The output position, in raw data format. 
+    def setControlOutput(self, position):
+        return None
+
+    ## @brief Get a diagnostics message for this joint.
+    ##
+    ## @return Diagnostics message. 
+    def getDiagnostics(self):
+        return None
+
+
+import rospy
+import xml.dom.minidom
+
+from math import pi, radians
+
+## @brief Get joint parameters from URDF
+def getJointsFromURDF():
+    try:
+        description = rospy.get_param("robot_description")
+        robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
+        joints = {}
+        # Find all non-fixed joints
+        for child in robot.childNodes:
+            if child.nodeType is child.TEXT_NODE:
+                continue
+            if child.localName == 'joint':
+                jtype = child.getAttribute('type')
+                if jtype == 'fixed':
+                  continue
+                name = child.getAttribute('name')
+                if jtype == 'continuous':
+                    minval = -pi
+                    maxval = pi
+                else:
+                    limit = child.getElementsByTagName('limit')[0]
+                    minval = float(limit.getAttribute('lower'))
+                    maxval = float(limit.getAttribute('upper'))
+
+                if minval > 0 or maxval < 0:
+                    zeroval = (maxval + minval)/2
+                else:
+                    zeroval = 0
+
+                joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
+                joints[name] = joint
+        return joints
+    except:
+        rospy.loginfo('No URDF defined, proceeding with defaults')
+        return dict()
+
+
+## @brief Get limits of servo, from YAML, then URDF, then defaults if neither is defined.
+def getJointLimits(name, joint_defaults, default_min=-150, default_max=150):
+    min_angle = radians(default_min)
+    max_angle = radians(default_max)
+    
+    try: 
+        min_angle = joint_defaults[name]['min']
+    except:
+        pass
+    try: 
+        min_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/min_angle"))
+    except:
+        pass
+    try: 
+        min_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/min_angle"))
+    except:
+        pass
+    try: 
+        min_angle = rospy.get_param("/arbotix/joints/"+name+"/min_position")
+    except:
+        pass
+
+    try: 
+        max_angle = joint_defaults[name]['max']
+    except:
+        pass
+    try: 
+        max_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/max_angle"))
+    except:
+        pass
+    try: 
+        max_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/max_angle"))
+    except:
+        pass
+    try: 
+        max_angle = rospy.get_param("/arbotix/joints/"+name+"/max_position")
+    except:
+        pass
+
+    return (min_angle, max_angle)
+

src/arbotix_ros/arbotix_python/src/arbotix_python/linear_controller.py

@@ -0,0 +1,285 @@
+#!/usr/bin/env python
+
+"""
+  linear_controller.py - controller for a linear actuator with analog feedback
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+
+from joints import *
+from controllers import *
+from std_msgs.msg import Float64
+from diagnostic_msgs.msg import *
+from std_srvs.srv import *
+
+from struct import unpack
+
+class LinearJoint(Joint):
+    def __init__(self, device, name):
+        Joint.__init__(self, device, name)
+
+        self.dirty = False
+        self.position = 0.0                     # current position, as returned by feedback (meters)
+        self.desired = 0.0                      # desired position (meters)
+        self.velocity = 0.0                     # moving speed
+        self.last = rospy.Time.now()
+
+        # TODO: load these from URDF
+        self.min = rospy.get_param('~joints/'+name+'/min_position',0.0)
+        self.max = rospy.get_param('~joints/'+name+'/max_position',0.5)
+        self.max_speed = rospy.get_param('~joints/'+name+'/max_speed',0.0508)
+
+        # calibration data {reading: position}
+        self.cal = { -1: -1, 1: 1 }
+        self.cal_raw = rospy.get_param('~joints/'+name+'/calibration_data', self.cal)
+        self.cal = dict()
+        for key, value in self.cal_raw.items():
+            self.cal[int(key)] = value
+        self.keys = sorted(self.cal.keys())
+
+        rospy.Subscriber(name+'/command', Float64, self.commandCb)
+        
+    def interpolate(self, frame):
+        """ Get new output: 1 = increase position, -1 is decrease position. """
+        if self.dirty:
+            cmd = self.desired - self.position
+            if self.device.fake: 
+                self.position = self.desired
+                self.dirty = False
+                return None
+            if cmd > 0.01:
+                return 1
+            elif cmd < -0.01:
+                return -1
+            else:
+                self.dirty = False
+                return 0
+        else:
+            return None
+
+    def setCurrentFeedback(self, reading):
+        if reading >= self.keys[0] and reading <= self.keys[-1]:
+            last_angle = self.position
+            self.position = self.readingToPosition(reading)
+            # update velocity estimate
+            t = rospy.Time.now()
+            self.velocity = (self.position - last_angle)/((t - self.last).to_nsec()/1000000000.0)
+            self.last = t
+        else:
+            rospy.logerr(self.name + ": feedback reading out of range")
+
+    def setControlOutput(self, position):
+        """ Set the position that controller is moving to. 
+            Returns output value in raw_data format. """
+        if position <= self.max and position >= self.min:
+            self.desired = position
+            self.dirty = True
+        else:
+            rospy.logerr(self.name + ": requested position is out of range: " + str(position))
+        return None # TODO
+    
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        msg.level = DiagnosticStatus.OK
+        if self.dirty:
+            msg.message = "Moving"
+        else:
+            msg.message = "OK"
+        msg.values.append(KeyValue("Position", str(self.position)))
+        return msg
+
+    def commandCb(self, req):
+        """ Float64 style command input. """
+        if self.device.fake:
+            self.position = req.data
+        else:
+            if req.data <= self.max and req.data >= self.min:
+                self.desired = req.data
+                self.dirty = True
+            else:
+                rospy.logerr(self.name + ": requested position is out of range: " + str(req))
+
+    def readingToPosition(self, reading):
+        low = 0
+        while reading > self.keys[low+1]:
+            low += 1
+        high = len(self.keys) - 1
+        while reading < self.keys[high-1]:
+            high += -1
+        x = self.keys[high] - self.keys[low]
+        y = self.cal[self.keys[high]] - self.cal[self.keys[low]]
+        x1 = reading - self.keys[low]
+        y1 = y * ( float(x1)/float(x) )
+        return self.cal[self.keys[low]] + y1
+
+
+class LinearControllerAbsolute(Controller):
+    """ A controller for a linear actuator, with absolute positional feedback. """
+
+    def __init__(self, device, name):
+        Controller.__init__(self, device, name)
+
+        self.a = rospy.get_param('~controllers/'+name+'/motor_a',29)
+        self.b = rospy.get_param('~controllers/'+name+'/motor_b',30)
+        self.p = rospy.get_param('~controllers/'+name+'/motor_pwm',31)
+        self.analog = rospy.get_param('~controllers/'+name+'/feedback',0)
+        self.last = 0
+        self.last_reading = 0
+
+        self.delta = rospy.Duration(1.0/rospy.get_param('~controllers/'+name+'/rate', 10.0))
+        self.next = rospy.Time.now() + self.delta
+
+        self.joint = device.joints[rospy.get_param('~controllers/'+name+'/joint')]
+
+        rospy.loginfo("Started LinearController ("+self.name+").")
+
+    def startup(self):
+        if not self.fake:
+            self.joint.setCurrentFeedback(self.device.getAnalog(self.analog))
+
+    def update(self):
+        now = rospy.Time.now()
+        if now > self.next:
+            # read current position
+            if self.joint.dirty:
+                if not self.fake:
+                    try:
+                        self.last_reading = self.getPosition()
+                        self.joint.setCurrentFeedback(self.last_reading)
+                    except Exception as e:
+                        print "linear error: ", e
+                # update movement
+                output = self.joint.interpolate(1.0/self.delta.to_sec())
+                if self.last != output and not self.fake: 
+                    self.setSpeed(output)
+                    self.last = output
+            self.next = now + self.delta
+    
+    def setSpeed(self, speed):
+        """ Set speed of actuator. """
+        if speed > 0:
+            self.device.setDigital(self.a, 0); self.device.setDigital(self.b, 1);   # up
+            self.device.setDigital(self.p, 1)
+        elif speed < 0:
+            self.device.setDigital(self.a, 1); self.device.setDigital(self.b, 0);   # down
+            self.device.setDigital(self.p, 1)
+        else:
+            self.device.setDigital(self.p, 0)
+
+    def getPosition(self):
+        return self.device.getAnalog(self.analog)
+
+    def shutdown(self):
+        if not self.fake:
+            self.device.setDigital(self.p, 0)
+
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+
+        msg.level = DiagnosticStatus.OK
+        msg.message = "OK"
+        if not self.fake:
+            msg.values.append(KeyValue("Encoder Reading", str(self.last_reading)))
+        
+        return msg
+
+
+class LinearControllerIncremental(LinearControllerAbsolute):
+    """ A controller for a linear actuator, without absolute encoder. """
+    POSITION_L  = 100
+    POSITION_H  = 101
+    DIRECTION   = 102
+
+    def __init__(self, device, name):
+        Controller.__init__(self, device, name)
+        self.pause = True
+
+        self.a = rospy.get_param('~controllers/'+name+'/motor_a',29)
+        self.b = rospy.get_param('~controllers/'+name+'/motor_b',30)
+        self.p = rospy.get_param('~controllers/'+name+'/motor_pwm',31)
+        self.last = 0
+        self.last_reading = 0
+
+        self.delta = rospy.Duration(1.0/rospy.get_param('~controllers/'+name+'/rate', 10.0))
+        self.next = rospy.Time.now() + self.delta
+
+        self.joint = device.joints[rospy.get_param('~controllers/'+name+'/joint')]
+
+        rospy.Service(name+'/zero', Empty, self.zeroCb)
+        rospy.loginfo("Started LinearControllerIncremental ("+self.name+").")
+
+    def startup(self):
+        if not self.fake:
+            self.zeroEncoder()
+
+    def setSpeed(self, speed):
+        """ Set speed of actuator. We need to set direction for encoder. """
+        if speed > 0:
+            self.device.write(253, self.DIRECTION, [1])
+            self.device.setDigital(self.a, 0); self.device.setDigital(self.b, 1);   # up
+            self.device.setDigital(self.p, 1)
+        elif speed < 0:
+            self.device.write(253, self.DIRECTION, [0])
+            self.device.setDigital(self.a, 1); self.device.setDigital(self.b, 0);   # down
+            self.device.setDigital(self.p, 1)
+        else:
+            self.device.setDigital(self.p, 0)
+
+    def getPosition(self):
+        return unpack('=h', "".join([chr(k) for k in self.device.read(253, self.POSITION_L, 2)]) )[0]
+
+    def zeroEncoder(self, timeout=15.0):
+        rospy.loginfo(self.name + ': zeroing encoder')
+        self.setSpeed(1)
+        last_pos = None
+        for i in range(int(timeout)):
+            if rospy.is_shutdown():
+                return
+            try:
+                new_pos = self.getPosition()
+            except:
+                pass
+            if last_pos == new_pos:
+                break
+            last_pos = new_pos
+            rospy.sleep(1)
+        self.setSpeed(0)
+        self.device.write(253, self.POSITION_L, [0, 0])
+        self.joint.setCurrentFeedback(0)
+
+    def zeroCb(self, msg):
+        if not self.fake:
+            self.zeroEncoder(15.0)
+        return EmptyResponse()
+
+    def shutdown(self):
+        if not self.fake:
+            self.setSpeed(0)
+

src/arbotix_ros/arbotix_python/src/arbotix_python/parallel_convert.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+"""
+  ParallelConvert:  For Parallel/Prismatic joint, convert Angle to Width
+  Copyright (c) 2014 Vanadium Labs LLC.  All right reserved.
+
+  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 HOLDER 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 THEY 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.
+"""
+import rospy
+from math import asin, sin, cos, sqrt, acos
+
+#  Parallel Gripper gap calculation:
+#
+#        o           (S) is servo axis
+#       /:\    ||      R is radius of servo horn
+#    R / :  \C ||      C is connector to finger        
+#     /  :x   \||      Offset is for foam and offset from connection back to finger
+#    /a  :      \        
+#  (S). . . . . . o
+#      n    y  ||
+#              || <-- offset
+#              ||finger 
+
+PRISM_PARAM_NS = "/arbotix/joints/"
+
+class ParallelConvert:
+    """ For Parallel/Prismatic joint, convert Angle to Width and vice versa"""
+    def __init__(self, joint_name):
+        ns = PRISM_PARAM_NS + joint_name + "/"
+        self.r = rospy.get_param(ns+'radius', .0078)      # Radius of servo horn
+        self.c = rospy.get_param(ns+'connector', 0.024)   # connector from horn to finger
+        # offset back from connection to actual foam pad      
+        self.offset = rospy.get_param(ns+'offset', 0.016)
+
+    def widthToAngle(self, width):
+        """ Convert width to servo angle """
+        leg = (width / 2) + self.offset  # Remove double for two fingers and add offset
+        # Law of Cosines
+        return -1 * acos ( (self.r * self.r + leg * leg - self.c * self.c) / (2 * self.r * leg) )
+
+    def angleToWidth(self, ang):
+        """ Convert angle to width for this gripper """
+        n = cos(ang) * self.r               # CAH
+        x = sin(ang) * self.r               # SOH
+        y = sqrt(self.c * self.c - x * x)   # Pythagorean
+        return (n + y - self.offset) * 2  # Remove offset and double to cover two fingers
+

src/arbotix_ros/arbotix_python/src/arbotix_python/publishers.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python
+
+"""
+  diagnostics.py - diagnostic output code
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+from diagnostic_msgs.msg import DiagnosticArray
+from sensor_msgs.msg import JointState
+
+class DiagnosticsPublisher:
+    """ Class to handle publications of joint_states message. """
+
+    def __init__(self):
+        self.t_delta = rospy.Duration(1.0/rospy.get_param("~diagnostic_rate", 1.0))
+        self.t_next = rospy.Time.now() + self.t_delta
+        self.pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=5)
+
+    def update(self, joints, controllers):
+        """ Publish diagnostics. """    
+        now = rospy.Time.now()
+        if now > self.t_next:
+            # create message
+            msg = DiagnosticArray()
+            msg.header.stamp = now
+            for controller in controllers:
+                d = controller.getDiagnostics()
+                if d:
+                    msg.status.append(d)
+            for joint in joints:
+                d = joint.getDiagnostics()
+                if d:
+                    msg.status.append(d)
+            # publish and update stats
+            self.pub.publish(msg)
+            self.t_next = now + self.t_delta
+        
+
+class JointStatePublisher:
+    """ Class to handle publications of joint_states message. """
+
+    def __init__(self):
+        # parameters: throttle rate and geometry
+        self.rate = rospy.get_param("~read_rate", 10.0)
+        self.t_delta = rospy.Duration(1.0/self.rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+
+        # subscriber
+        self.pub = rospy.Publisher('joint_states', JointState, queue_size=5)
+
+    def update(self, joints, controllers):
+        """ publish joint states. """
+        if rospy.Time.now() > self.t_next:   
+            msg = JointState()
+            msg.header.stamp = rospy.Time.now()
+            msg.name = list()
+            msg.position = list()
+            msg.velocity = list()
+            for joint in joints:
+                msg.name.append(joint.name)
+                msg.position.append(joint.jst_position)
+                msg.velocity.append(joint.velocity)
+            for controller in controllers:
+                msg.name += controller.joint_names
+                msg.position += controller.joint_positions
+                msg.velocity += controller.joint_velocities
+            self.pub.publish(msg)
+            self.t_next = rospy.Time.now() + self.t_delta
+

src/arbotix_ros/arbotix_python/src/arbotix_python/servo_controller.py

@@ -0,0 +1,479 @@
+#!/usr/bin/env python
+
+"""
+  servo_controller.py: classes for servo interaction
+  Copyright (c) 2011-2013 Vanadium Labs LLC. All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+
+from math import radians
+
+from std_msgs.msg import Float64
+from arbotix_msgs.srv import *
+from diagnostic_msgs.msg import *
+
+from ax12 import *
+from joints import *
+from arbotix_python.parallel_convert import *
+
+class DynamixelServo(Joint):
+
+    def __init__(self, device, name, ns="~joints"):
+        Joint.__init__(self, device, name)
+        n = ns+"/"+name+"/"
+        
+        self.id = int(rospy.get_param(n+"id"))
+        self.ticks = rospy.get_param(n+"ticks", 1024)
+        self.neutral = rospy.get_param(n+"neutral", self.ticks/2)
+        if self.ticks == 4096:
+            self.range = 360.0
+        else:
+            self.range = 300.0
+        self.range = rospy.get_param(n+"range", self.range)
+        self.rad_per_tick = radians(self.range)/self.ticks
+
+        # TODO: load these from URDF
+        self.max_angle = radians(rospy.get_param(n+"max_angle",self.range/2.0))
+        self.min_angle = radians(rospy.get_param(n+"min_angle",-self.range/2.0))
+        self.max_speed = radians(rospy.get_param(n+"max_speed",684.0)) 
+                                                # max speed = 114 rpm - 684 deg/s
+        self.invert = rospy.get_param(n+"invert",False)
+        self.readable = rospy.get_param(n+"readable",True)
+
+        self.status = "OK"
+        self.level = DiagnosticStatus.OK
+
+        self.dirty = False                      # newly updated position?
+        self.position = 0.0                     # current position, as returned by servo (radians)
+        self.jst_position= 0.0                  # position to report as Joint Status
+        self.desired = 0.0                      # desired position (radians)
+        self.last_cmd = 0.0                     # last position sent (radians)
+        self.velocity = 0.0                     # moving speed
+        self.enabled = True                     # can we take commands?
+        self.active = False                     # are we under torque control?
+        self.last = rospy.Time.now()
+
+        self.reads = 0.0                        # number of reads
+        self.errors = 0                         # number of failed reads
+        self.total_reads = 0.0                  
+        self.total_errors = [0.0]
+
+        self.voltage = 0.0
+        self.temperature = 0.0
+        self.tolerance = rospy.get_param(n + "tolerance", 0.05)   # Default 0.05 radians
+        rospy.loginfo("Started Servo %d  %s", self.id, name)
+
+        # ROS interfaces
+        rospy.Subscriber(name+'/command', Float64, self.commandCb)
+        rospy.Service(name+'/relax', Relax, self.relaxCb)
+        rospy.Service(name+'/enable', Enable, self.enableCb)
+        rospy.Service(name+'/set_speed', SetSpeed, self.setSpeedCb)
+
+    def interpolate(self, frame):
+        """ Get the new position to move to, in ticks. """
+        if self.enabled and self.active and self.dirty:
+            # cap movement
+            if abs(self.last_cmd - self.desired) < self.tolerance:
+                self.dirty = False
+            # compute command, limit velocity
+            cmd = self.desired - self.last_cmd
+            if cmd > self.max_speed/frame:
+                cmd = self.max_speed/frame
+            elif cmd < -self.max_speed/frame:
+                cmd = -self.max_speed/frame
+            # compute angle, apply limits
+            ticks = self.angleToTicks(self.last_cmd + cmd)
+            self.last_cmd = self.ticksToAngle(ticks)
+            self.speed = cmd*frame
+            # when fake, need to set position/velocity here
+            if self.device.fake:
+                last_angle = self.position
+                self.position = self.last_cmd
+                t = rospy.Time.now()
+                self.velocity = (self.position - last_angle)/((t - self.last).to_nsec()/1000000000.0)
+                self.last = t
+                return None
+            return int(ticks)
+        else:
+            # when fake, need to reset velocity to 0 here.
+            if self.device.fake:
+                self.velocity = 0.0
+                self.last = rospy.Time.now()
+            return None
+            
+    def jstPosition(self, pos):
+        return self.ticksToAngle(pos)  # report Joint Status in radians
+
+    def setCurrentFeedback(self, reading):
+        """ Update angle in radians by reading from servo, or by 
+            using position passed in from a sync read (in ticks). """
+        if reading > -1 and reading < self.ticks:     # check validity
+            self.reads += 1
+            self.total_reads += 1
+            last_angle = self.position
+            self.position = self.ticksToAngle(reading)
+            self.jst_position = self.jstPosition(reading)
+            # update velocity estimate
+            t = rospy.Time.now()
+            self.velocity = (self.position - last_angle)/((t - self.last).to_nsec()/1000000000.0)
+            self.last = t
+        else:
+            rospy.logdebug("Invalid read of servo: id " + str(self.id) + ", value " + str(reading))
+            self.errors += 1
+            self.total_reads += 1
+            return
+        if not self.active:
+            self.last_cmd = self.position
+
+    def setControlOutput(self, position):
+        """ Set the position that controller is moving to. 
+            Returns output value in ticks. """
+        if self.enabled:
+            ticks = self.angleToTicks(position)
+            self.desired = position
+            self.dirty = True
+            self.active = True
+            return int(ticks)
+        return -1
+
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        if self.temperature > 60:   # TODO: read this value from eeprom
+            self.status = "OVERHEATED, SHUTDOWN"
+            self.level = DiagnosticStatus.ERROR
+        elif self.temperature > 50: #TODO: RREMOVE and self.status != "OVERHEATED, SHUTDOWN":
+            self.status = "OVERHEATING"
+            self.level = DiagnosticStatus.WARN
+        #TODO: RREMOVE elif self.status != "OVERHEATED, SHUTDOWN":
+        else:
+            self.status = "OK"
+            self.level = DiagnosticStatus.OK
+        msg.level = self.level
+        msg.message = self.status
+        msg.values.append(KeyValue("Position", str(self.position)))
+        msg.values.append(KeyValue("Temperature", str(self.temperature)))
+        msg.values.append(KeyValue("Voltage", str(self.voltage)))
+        if self.reads + self.errors > 100:
+            self.total_errors.append((self.errors*100.0)/(self.reads+self.errors))
+            if len(self.total_errors) > 10:
+                self.total_errors = self.total_errors[-10:]
+            self.reads = 0
+            self.errors = 0
+        msg.values.append(KeyValue("Reads", str(self.total_reads)))
+        msg.values.append(KeyValue("Error Rate", str(sum(self.total_errors)/len(self.total_errors))+"%" ))
+        if self.active:
+            msg.values.append(KeyValue("Torque", "ON"))
+        else:
+            msg.values.append(KeyValue("Torque", "OFF"))
+        return msg
+
+    def angleToTicks(self, angle):
+        """ Convert an angle to ticks, applying limits. """
+        ticks = self.neutral + (angle/self.rad_per_tick)
+        if self.invert:
+            ticks = self.neutral - (angle/self.rad_per_tick)
+        if ticks >= self.ticks:
+            return self.ticks-1.0
+        if ticks < 0:
+            return 0
+        return ticks
+
+    def ticksToAngle(self, ticks):
+        """ Convert an ticks to angle, applying limits. """
+        angle = (ticks - self.neutral) * self.rad_per_tick
+        if self.invert:
+            angle = -1.0 * angle
+        return angle
+
+    def speedToTicks(self, rads_per_sec):
+        """ Convert speed in radians per second to ticks, applying limits. """
+        ticks = self.ticks * rads_per_sec / self.max_speed
+        if ticks >= self.ticks:
+            return self.ticks-1.0
+        if ticks < 0:
+            return 0
+        return ticks  
+
+    def enableCb(self, req):
+        """ Turn on/off servo torque, so that it is pose-able. """
+        if req.enable:
+            self.enabled = True
+        else:
+            if not self.device.fake:
+                self.device.disableTorque(self.id)
+            self.dirty = False
+            self.enabled = False
+            self.active = False
+        return EnableResponse(self.enabled)
+
+    def relaxCb(self, req):
+        """ Turn off servo torque, so that it is pose-able. """
+        if not self.device.fake:
+            self.device.disableTorque(self.id)
+        self.dirty = False
+        self.active = False
+        return RelaxResponse()
+
+    def commandCb(self, req):
+        """ Float64 style command input. """
+        if self.enabled:
+            if self.controller and self.controller.active():
+                # Under and action control, do not interfere
+                return
+            elif self.desired != req.data or not self.active:
+                self.dirty = True
+                self.active = True
+                self.desired = req.data
+                
+    def setSpeedCb(self, req):
+        """ Set servo speed. Requested speed is in radians per second.
+            Don't allow 0 which means "max speed" to a Dynamixel in joint mode. """
+        if not self.device.fake:
+            ticks_per_sec = max(1, int(self.speedToTicks(req.speed)))
+            self.device.setSpeed(self.id, ticks_per_sec)
+        return SetSpeedResponse()
+        
+class PrismaticDynamixelServo(DynamixelServo):
+
+    def __init__(self, device, name, ns="~joints"):
+        DynamixelServo.__init__(self, device, name)
+        self.convertor = ParallelConvert(name)
+        rospy.loginfo(name + " prismatic joint")
+                        
+    def jstPosition(self, pos):
+        return self.convertor.angleToWidth(self.ticksToAngle(pos)) # report Joint Status in meters
+        
+
+class HobbyServo(Joint):
+
+    def __init__(self, device, name, ns="~joints"):
+        Joint.__init__(self, device, name)
+        n = ns+"/"+name+"/"
+        
+        self.id = int(rospy.get_param(n+"id"))
+        self.ticks = rospy.get_param(n+"ticks", 2000)
+        self.neutral = rospy.get_param(n+"neutral", 1500)
+        self.range = rospy.get_param(n+"range", 180)
+        self.rad_per_tick = radians(self.range)/self.ticks
+
+        # TODO: load these from URDF
+        self.max_angle = radians(rospy.get_param(n+"max_angle",self.range/2.0))
+        self.min_angle = radians(rospy.get_param(n+"min_angle",-self.range/2.0))
+        self.max_speed = radians(rospy.get_param(n+"max_speed",90.0)) 
+
+        self.invert = rospy.get_param(n+"invert",False)
+
+        self.dirty = False                      # newly updated position?
+        self.position = 0.0                     # current position, as returned by servo (radians)
+        self.desired = 0.0                      # desired position (radians)
+        self.last_cmd = 0.0                     # last position sent (radians)
+        self.velocity = 0.0                     # moving speed
+        self.last = rospy.Time.now()
+        
+        # ROS interfaces
+        rospy.Subscriber(name+'/command', Float64, self.commandCb)
+
+    def interpolate(self, frame):
+        """ Get the new position to move to, in ticks. """
+        if self.dirty:
+            # compute command, limit velocity
+            cmd = self.desired - self.last_cmd
+            if cmd > self.max_speed/frame:
+                cmd = self.max_speed/frame
+            elif cmd < -self.max_speed/frame:
+                cmd = -self.max_speed/frame
+            # compute angle, apply limits
+            ticks = self.angleToTicks(self.last_cmd + cmd)
+            self.last_cmd = self.ticksToAngle(ticks)
+            self.speed = cmd*frame
+            # cap movement
+            if self.last_cmd == self.desired:
+                self.dirty = False
+            if self.device.fake:
+                self.position = self.last_cmd
+                return None
+            return int(ticks)
+        else:
+            return None
+
+    def setCurrentFeedback(self, raw_data):
+        """ Update angle in radians by reading from servo, or by 
+            using position passed in from a sync read (in ticks). """
+        return None
+
+    def setControlOutput(self, position):
+        """ Set the position that controller is moving to. 
+            Returns output value in ticks. """
+        ticks = self.angleToTicks(position)
+        self.desired = position
+        self.dirty = True
+        return int(ticks)
+
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        msg.level = DiagnosticStatus.OK
+        msg.message = "OK"
+        msg.values.append(KeyValue("Position", str(self.position)))
+        return msg
+
+    def angleToTicks(self, angle):
+        """ Convert an angle to ticks, applying limits. """
+        ticks = self.neutral + (angle/self.rad_per_tick)
+        if self.invert:
+            ticks = self.neutral - (angle/self.rad_per_tick)
+        if ticks >= self.ticks:
+            return self.ticks-1.0
+        if ticks < 0:
+            return 0
+        return ticks
+
+    def ticksToAngle(self, ticks):
+        """ Convert an ticks to angle, applying limits. """
+        angle = (ticks - self.neutral) * self.rad_per_tick
+        if self.invert:
+            angle = -1.0 * angle
+        return angle        
+
+    def commandCb(self, req):
+        """ Float64 style command input. """
+        if self.controller and self.controller.active():
+            # Under and action control, do not interfere
+            return
+        else:
+            self.dirty = True
+            self.desired = req.data
+
+
+from controllers import *
+
+class ServoController(Controller):
+
+    def __init__(self, device, name):
+        Controller.__init__(self, device, name)
+        self.dynamixels = list()
+        self.hobbyservos = list()
+        self.iter = 0
+
+        # steal some servos
+        for joint in device.joints.values():
+            if isinstance(joint, DynamixelServo):
+                self.dynamixels.append(joint)
+            elif isinstance(joint, PrismaticDynamixelServo):
+                self.dynamixels.append(joint)
+            elif isinstance(joint, HobbyServo):
+                self.hobbyservos.append(joint)
+
+        self.w_delta = rospy.Duration(1.0/rospy.get_param("~write_rate", 10.0))
+        self.w_next = rospy.Time.now() + self.w_delta
+
+        self.r_delta = rospy.Duration(1.0/rospy.get_param("~read_rate", 10.0))
+        self.r_next = rospy.Time.now() + self.r_delta
+
+    def update(self):
+        """ Read servo positions, update them. """
+        if rospy.Time.now() > self.r_next and not self.fake:
+            if self.device.use_sync_read:
+                # arbotix/servostik/wifi board sync_read
+                synclist = list()
+                for joint in self.dynamixels:
+                    if joint.readable:
+                        synclist.append(joint.id)
+                if len(synclist) > 0:
+                    val = self.device.syncRead(synclist, P_PRESENT_POSITION_L, 2)
+                    if val: 
+                        for joint in self.dynamixels:
+                            try:
+                                i = synclist.index(joint.id)*2
+                                joint.setCurrentFeedback(val[i]+(val[i+1]<<8))
+                            except Exception as e:
+                                # not a readable servo
+                                rospy.logerr("Servo read error: " + str(e) )
+                                continue 
+            else:
+                # direct connection, or other hardware with no sync_read capability
+                for joint in self.dynamixels:
+                    joint.setCurrentFeedback(self.device.getPosition(joint.id))
+            self.r_next = rospy.Time.now() + self.r_delta
+
+        if rospy.Time.now() > self.w_next:
+            if self.device.use_sync_write and not self.fake:
+                syncpkt = list()
+                for joint in self.dynamixels:
+                    v = joint.interpolate(1.0/self.w_delta.to_sec())
+                    if v != None:   # if was dirty
+                        syncpkt.append([joint.id,int(v)%256,int(v)>>8])                         
+                if len(syncpkt) > 0:      
+                    self.device.syncWrite(P_GOAL_POSITION_L,syncpkt)
+            else:
+                for joint in self.dynamixels:
+                    v = joint.interpolate(1.0/self.w_delta.to_sec())
+                    if v != None:   # if was dirty      
+                        self.device.setPosition(joint.id, int(v))
+            for joint in self.hobbyservos: 
+                v = joint.interpolate(1.0/self.w_delta.to_sec())
+                if v != None:   # if it was dirty   
+                    self.device.setServo(joint.id, v)
+            self.w_next = rospy.Time.now() + self.w_delta
+
+    def getDiagnostics(self):
+        """ Update status of servos (voltages, temperatures). """
+        if self.iter % 5 == 0 and not self.fake:
+            if self.device.use_sync_read:
+                # arbotix/servostik/wifi board sync_read
+                synclist = list()
+                for joint in self.dynamixels:
+                    if joint.readable:
+                        synclist.append(joint.id)
+                if len(synclist) > 0:
+                    val = self.device.syncRead(synclist, P_PRESENT_VOLTAGE, 2)
+                    if val:
+                        for joint in self.dynamixels:
+                            try:
+                                i = synclist.index(joint.id)*2
+                                if val[i] < 250:
+                                    joint.voltage = val[i]/10.0
+                                if val[i+1] < 100:
+                                    joint.temperature = val[i+1]
+                            except:
+                                # not a readable servo
+                                continue 
+            else:
+                # direct connection, or other hardware with no sync_read capability
+                for joint in self.dynamixels:
+                    if joint.readable:
+                        val = self.device.read(joint.id, P_PRESENT_VOLTAGE, 2)
+                        try:
+                            joint.voltage = val[0]
+                            joint.temperature = val[1]
+                        except:
+                            continue
+        self.iter += 1
+        return None
+

src/arbotix_ros/arbotix_sensors/CHANGELOG.rst

@@ -0,0 +1,27 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_sensors
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2014-07-14)
+-------------------
+* Set queue_size=5 on all publishers
+* Contributors: Jorge Santos
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+
+0.8.2 (2013-03-28)
+------------------
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* import drivers and catkinize

src/arbotix_ros/arbotix_sensors/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arbotix_sensors)
+
+find_package(catkin REQUIRED)
+catkin_package()
+catkin_python_setup()
+
+install(PROGRAMS bin/ir_ranger.py bin/max_sonar.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/arbotix_ros/arbotix_sensors/bin/ir_ranger.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+
+"""
+  ir_ranger.py - convert analog stream into range measurements
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+
+from sensor_msgs.msg import Range
+from arbotix_msgs.msg import Analog
+from arbotix_msgs.srv import SetupChannel, SetupChannelRequest
+
+from arbotix_python.sensors import *
+
+class ir_ranger:
+    def __init__(self):
+        rospy.init_node("ir_ranger")
+        
+        # sensor type: choices are A710YK (40-216"), A02YK (8-60"), A21YK (4-30")
+        self.sensor_t = rospy.get_param("~type","GP2D12")
+        if self.sensor_t == "A710YK" or self.sensor_t == "ultralong":
+            self.sensor = gpA710YK()
+        elif self.sensor_t == "A02YK" or self.sensor_t == "long":
+            self.sensor = gpA02YK()
+        else:
+            self.sensor = gp2d12() 
+
+        # start channel broadcast using SetupAnalogIn
+        rospy.wait_for_service('arbotix/SetupAnalogIn')
+        analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel) 
+        
+        req = SetupChannelRequest()
+        req.topic_name = rospy.get_param("~name")
+        req.pin = rospy.get_param("~pin")
+        req.rate = int(rospy.get_param("~rate",10))
+        analog_srv(req)
+
+        # setup a range message to use
+        self.msg = Range()
+        self.msg.radiation_type = self.sensor.radiation_type
+        self.msg.field_of_view = self.sensor.field_of_view
+        self.msg.min_range = self.sensor.min_range
+        self.msg.max_range = self.sensor.max_range
+
+        # publish/subscribe
+        self.pub = rospy.Publisher("ir_range", Range, queue_size=5)
+        rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)
+
+        rospy.spin()
+        
+    def readingCb(self, msg):
+        # convert msg.value into range.range
+        self.msg.header.stamp = rospy.Time.now()
+        self.msg.range = self.sensor.convert(msg.value<<2)
+        self.pub.publish(self.msg)
+
+if __name__=="__main__":
+    ir_ranger()
+

src/arbotix_ros/arbotix_sensors/bin/max_sonar.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+
+"""
+  max_sonar.py - convert analog stream into range measurements
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+
+from sensor_msgs.msg import Range
+from arbotix_msgs.msg import Analog
+from arbotix_msgs.srv import SetupChannel, SetupChannelRequest
+
+from arbotix_python.sensors import *
+
+class max_sonar:
+    def __init__(self):
+        rospy.init_node("max_sonar")
+        
+        self.sensor = maxSonar() 
+
+        # start channel broadcast using SetupAnalogIn
+        rospy.wait_for_service('arbotix/SetupAnalogIn')
+        analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel) 
+        
+        req = SetupChannelRequest()
+        req.topic_name = rospy.get_param("~name")
+        req.pin = rospy.get_param("~pin")
+        req.rate = int(rospy.get_param("~rate",10))
+        analog_srv(req)
+
+        # setup a range message to use
+        self.msg = Range()
+        self.msg.radiation_type = self.sensor.radiation_type
+        self.msg.field_of_view = self.sensor.field_of_view
+        self.msg.min_range = self.sensor.min_range
+        self.msg.max_range = self.sensor.max_range
+
+        # publish/subscribe
+        self.pub = rospy.Publisher("sonar_range", Range, queue_size=5)
+        rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)
+
+        rospy.spin()
+        
+    def readingCb(self, msg):
+        # convert msg.value into range.range
+        self.msg.header.stamp = rospy.Time.now()
+        self.msg.range = self.sensor.convert(msg.value<<2)
+        self.pub.publish(self.msg)
+
+if __name__=="__main__":
+    max_sonar()
+

src/arbotix_ros/arbotix_sensors/package.xml

@@ -0,0 +1,15 @@
+<package>
+  <name>arbotix_sensors</name>
+  <version>0.10.0</version>
+  <description>
+    Extends the arbotix_node package with a number of more sophisticated ROS wrappers for common devices. 
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_sensors</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>arbotix_python</run_depend>
+</package>

src/arbotix_ros/arbotix_sensors/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['arbotix_sensors'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)

src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/sensors.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+
+"""
+  sensors.py - various conversions from raw analog to sensor range
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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 sensor_msgs.msg import Range
+
+class SharpIR:
+    radiation_type = Range.INFRARED
+    field_of_view = 0.001
+    min_range = 0.0
+    max_range = 1.0
+
+    def convert(self, raw):
+        """ Convert raw analog (10-bit) to distance. """
+        return raw
+
+class gpA710YK(SharpIR):
+    """ Ultra long-range Sharp IR sensor. """
+    min_range = 0.75
+    max_range = 5.50
+
+    def convert(self, raw):
+        """ Convert raw analog (10-bit) to distance. """
+        if raw > 100:
+            return (497.0/(raw-56))
+        else:
+            return self.max_range+0.1
+        
+class gpA02YK(SharpIR):
+    min_range = 0.20
+    max_range = 1.50
+
+    def convert(self, raw):
+        """ Convert raw analog (10-bit) to distance. """
+        if raw > 80:
+            return (115.0/(raw-12))
+        else:
+            return self.max_range+0.1
+
+class gp2d12(SharpIR):
+    """ The typical GP2D12 IR ranger. """
+    min_range = 0.10
+    max_range = 0.80
+
+    def convert(self, raw):
+        """ Convert raw analog (10-bit) to distance. """
+        if raw > 40:
+            return (52.0/(raw-12))
+        else:
+            return self.max_range+0.1
+
+class maxSonar():
+    radiation_type = Range.ULTRASOUND
+    field_of_view = 0.785398163
+    min_range = 0.0
+    max_range = 6.4516 
+
+    def convert(self, raw):
+        """ Convert raw analog (10-bit) to distance. """
+        return 12.7 * (raw/1024.0)
+

src/dynamixel_motor/.gitignore

@@ -0,0 +1,35 @@
+*.py[cod]
+
+# C extensions
+*.so
+
+# Packages
+*.egg
+*.egg-info
+dist
+build
+eggs
+parts
+bin
+var
+sdist
+develop-eggs
+.installed.cfg
+lib
+lib64
+
+# Installer logs
+pip-log.txt
+
+# Unit test / coverage reports
+.coverage
+.tox
+nosetests.xml
+
+# Translations
+*.mo
+
+# Mr Developer
+.mr.developer.cfg
+.project
+.pydevproject

src/dynamixel_motor/.travis.yaml

@@ -0,0 +1,30 @@
+language:
+  - cpp
+  - python
+python:
+  - "2.7"
+compiler:
+  - gcc
+env:
+  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=true
+  - ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin    USE_DEB=true
+# matrix:
+#   allow_failures:
+#     - env: ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=true
+script:
+  - export CI_SOURCE_PATH=$(pwd)
+  - export REPOSITORY_NAME=${PWD##*/}
+  - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
+  - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
+  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+  - sudo apt-get update -qq
+  - sudo apt-get install -qq -y python-catkin-pkg python-rosdep python-wstool python-catkin-tools ros-$ROS_DISTRO-catkin
+  - sudo rosdep init
+  - rosdep update; while [ $? != 0 ]; do sleep 1; rosdep update; done
+  - mkdir -p ~/ros/ws_$REPOSITORY_NAME/src
+  - cd ~/ros/ws_$REPOSITORY_NAME/src
+  - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
+  - cd ..
+  - rosdep install -r -n --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
+  - source /opt/ros/$ROS_DISTRO/setup.bash
+  - catkin build

src/dynamixel_motor/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2020, Antons Rebguns, Cody Jorgensen
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. 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.
+
+3. 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.

src/dynamixel_motor/README.md

@@ -0,0 +1,4 @@
+dynamixel_motor
+===============
+
+ROS stack for interfacing with Robotis Dynamixel line of servo motors.

src/dynamixel_motor/dynamixel_controllers/CHANGELOG.rst

@@ -0,0 +1,38 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package dynamixel_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.1 (2017-01-19)
+------------------
+* bug fixes for issue `#33 <https://github.com/arebgun/dynamixel_motor/issues/33>`_ and warnnings on queue_size
+* Fixed syntax error due to missing parentheses
+* add error message when joint names did not match
+* added in a queue size as none to remove warning messages
+* Readback echo for simple one-wire (TTL) converters
+* Support for reading current, setting acceleration, torque control mode (mainly MX series). Available features specified for each model.
+* Always release lock on return
+* Do not modify a controllers list we are iterating over.
+  It's probably a bad idea to modify a list we are looping over as is the case with meta controller dependencies. List copies everywhere!
+* Acquire lock before manipulating controllers.
+  Make sure that no two threads can manipulate (start, stop or restart) controllers simultaneously.
+* Allow negative speed values.
+  There was a copy paste error where we were checking to make sure the speed wasn't set to 0 (meaning maximum speed), even though speed has different meaning when in wheel mode (0 is for stop, negative values for counterclockwise rotation and positive - for clockwise rotation). The slave motor should rotate in the opposite direction (assumes motors are mounted back to back with horns facing in opposite directions).
+* fixed multi_packet[port] indentation
+* added test to fix process_motor_states bug
+* got action controller working with dual motor controller, still get an occasional key error with dual motor controller process_motor_states
+* patch so action controller works with dual motor controllers
+* Merge pull request `#3 <https://github.com/arebgun/dynamixel_motor/issues/3>`_ from arebgun/groovy
+  Velocity bug fix
+* Fix bug for velocity raw to radsec conversion in all remaining controllers
+* Fix bug in velocity conversion
+  The factor for the velocity conversion from raw to radsec and the other way
+  round is model-specific and does not depend on the maximum motor speed.
+  Therefore, a conversion factor is added to the motor data and employed
+  for all velocity conversions. The set motor velocity and the motor velocity
+  in the servo state does now correspond with the real servo speed in
+  radians per second.
+* Contributors: Andreas Wachaja, Antons Rebguns, Kei Okada, Nicolas, Nicolas Alt, Richard-Lab, Russell Toris, Tyler Slabinski, Yam Geva, richard-virtualros
+
+0.4.0 (2013-07-26)
+------------------
+* stack is now catkin compatible

src/dynamixel_motor/dynamixel_controllers/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamixel_controllers)
+
+find_package(catkin REQUIRED COMPONENTS message_generation)
+
+add_service_files(
+  FILES
+  RestartController.srv
+  SetComplianceMargin.srv
+  SetCompliancePunch.srv
+  SetComplianceSlope.srv
+  SetSpeed.srv
+  SetTorqueLimit.srv
+  StartController.srv
+  StopController.srv
+  TorqueEnable.srv
+)
+
+catkin_python_setup()
+
+generate_messages()
+
+catkin_package(
+  CATKIN_DEPENDS
+  rospy
+  actionlib
+  dynamixel_driver
+  std_msgs
+  trajectory_msgs
+  diagnostic_msgs
+  dynamixel_msgs
+  control_msgs
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(PROGRAMS
+  nodes/controller_manager.py
+  nodes/controller_spawner.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/dynamixel_motor/dynamixel_controllers/launch/controller_manager.launch

@@ -0,0 +1,39 @@
+<!-- -*- mode: XML -*- -->
+
+<launch>
+    <!-- Start Dynamixel controller manager -->
+    <!-- 'namespace' is the name of this manager (it has to be a valid ROS graph name). -->
+    <!-- 'serial_ports' contains a map of serial ports with all their configuration paramters. -->
+    <!--                It is possible to specify multiple ports for a single controller_manager. -->
+    <!--                Each port name also has to be a valid ROS graph name. -->
+    <!-- Once the controller manager start up it will provide serices to start, stop and restart -->
+    <!-- dynamixel controllers, e.g. the manager below with advertise the following: -->
+    <!-- 1. /dynamixel_controller_manager/dxl_tty1/start_controller -->
+    <!-- 2. /dynamixel_controller_manager/dxl_tty1/stop_controller -->
+    <!-- 3. /dynamixel_controller_manager/dxl_tty1/restart_controller -->
+    <!-- If the second serial port is uncommented, this manager will provide the same three -->
+    <!-- services but with different path, e.g. for the configuration below: -->
+    <!-- 4. /dynamixel_controller_manager/dxl_tty2/start_controller -->
+    <!-- 5. /dynamixel_controller_manager/dxl_tty2/stop_controller -->
+    <!-- 6. /dynamixel_controller_manager/dxl_tty2/restart_controller -->
+    <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
+        <rosparam>
+            namespace: dynamixel_controller_manager
+            serial_ports:
+                dxl_tty1:
+                    port_name: "/dev/ttyUSB0"
+                    baud_rate: 1000000
+                    min_motor_id: 1
+                    max_motor_id: 30
+                    update_rate: 10
+                <!--
+                dxl_tty2:
+                    port_name: "/dev/ttyUSB1"
+                    baud_rate: 1000000
+                    min_motor_id: 10
+                    max_motor_id: 20
+                    update_rate: 15
+                -->
+        </rosparam>
+    </node>
+</launch>

src/dynamixel_motor/dynamixel_controllers/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b dynamixel_controllers is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py

@@ -0,0 +1,269 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010, Antons Rebguns, Cody Jorgensen, Cara Slutter
+#               2010-2011, Antons Rebguns
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns, Cody Jorgensen, Cara Slutter'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns, Cody Jorgensen, Cara Slutter'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+from threading import Thread, Lock
+
+import sys
+
+import rospy
+
+from dynamixel_driver.dynamixel_serial_proxy import SerialProxy
+
+from diagnostic_msgs.msg import DiagnosticArray
+from diagnostic_msgs.msg import DiagnosticStatus
+from diagnostic_msgs.msg import KeyValue
+
+from dynamixel_controllers.srv import StartController
+from dynamixel_controllers.srv import StartControllerResponse
+from dynamixel_controllers.srv import StopController
+from dynamixel_controllers.srv import StopControllerResponse
+from dynamixel_controllers.srv import RestartController
+from dynamixel_controllers.srv import RestartControllerResponse
+
+class ControllerManager:
+    def __init__(self):
+        rospy.init_node('dynamixel_controller_manager', anonymous=True)
+        rospy.on_shutdown(self.on_shutdown)
+        
+        self.waiting_meta_controllers = []
+        self.controllers = {}
+        self.serial_proxies = {}
+        self.diagnostics_rate = rospy.get_param('~diagnostics_rate', 1)
+        
+        self.start_controller_lock = Lock()
+        self.stop_controller_lock = Lock()
+
+        manager_namespace = rospy.get_param('~namespace')
+        serial_ports = rospy.get_param('~serial_ports')
+        
+        for port_namespace,port_config in serial_ports.items():
+            port_name = port_config['port_name']
+            baud_rate = port_config['baud_rate']
+            readback_echo = port_config['readback_echo'] if 'readback_echo' in port_config else False
+            min_motor_id = port_config['min_motor_id'] if 'min_motor_id' in port_config else 0
+            max_motor_id = port_config['max_motor_id'] if 'max_motor_id' in port_config else 253
+            update_rate = port_config['update_rate'] if 'update_rate' in port_config else 5
+            error_level_temp = 75
+            warn_level_temp = 70
+            
+            if 'diagnostics' in port_config:
+                if 'error_level_temp' in port_config['diagnostics']:
+                    error_level_temp = port_config['diagnostics']['error_level_temp']
+                if 'warn_level_temp' in port_config['diagnostics']:
+                    warn_level_temp = port_config['diagnostics']['warn_level_temp']
+                    
+            serial_proxy = SerialProxy(port_name,
+                                       port_namespace,
+                                       baud_rate,
+                                       min_motor_id,
+                                       max_motor_id,
+                                       update_rate,
+                                       self.diagnostics_rate,
+                                       error_level_temp,
+                                       warn_level_temp,
+                                       readback_echo)
+            serial_proxy.connect()
+            
+            # will create a set of services for each serial port under common manager namesapce
+            # e.g. /dynamixel_manager/robot_arm_port/start_controller
+            #      /dynamixel_manager/robot_head_port/start_controller
+            # where 'dynamixel_manager' is manager's namespace
+            #       'robot_arm_port' and 'robot_head_port' are human readable names for serial ports
+            rospy.Service('%s/%s/start_controller' % (manager_namespace, port_namespace), StartController, self.start_controller)
+            rospy.Service('%s/%s/stop_controller' % (manager_namespace, port_namespace), StopController, self.stop_controller)
+            rospy.Service('%s/%s/restart_controller' % (manager_namespace, port_namespace), RestartController, self.restart_controller)
+            
+            self.serial_proxies[port_namespace] = serial_proxy
+            
+        # services for 'meta' controllers, e.g. joint trajectory controller
+        # these controllers don't have their own serial port, instead they rely
+        # on regular controllers for serial connection. The advantage of meta
+        # controller is that it can pack commands for multiple motors on multiple
+        # serial ports.
+        # NOTE: all serial ports that meta controller needs should be managed by
+        # the same controler manager.
+        rospy.Service('%s/meta/start_controller' % manager_namespace, StartController, self.start_controller)
+        rospy.Service('%s/meta/stop_controller' % manager_namespace, StopController, self.stop_controller)
+        rospy.Service('%s/meta/restart_controller' % manager_namespace, RestartController, self.restart_controller)
+        
+        self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
+        if self.diagnostics_rate > 0: Thread(target=self.diagnostics_processor).start()
+
+    def on_shutdown(self):
+        for serial_proxy in self.serial_proxies.values():
+            serial_proxy.disconnect()
+
+    def diagnostics_processor(self):
+        diag_msg = DiagnosticArray()
+        
+        rate = rospy.Rate(self.diagnostics_rate)
+        while not rospy.is_shutdown():
+            diag_msg.status = []
+            diag_msg.header.stamp = rospy.Time.now()
+            
+            for controller in self.controllers.values():
+                try:
+                    joint_state = controller.joint_state
+                    temps = joint_state.motor_temps
+                    max_temp = max(temps)
+                    
+                    status = DiagnosticStatus()
+                    status.name = 'Joint Controller (%s)' % controller.joint_name
+                    status.hardware_id = 'Robotis Dynamixel %s on port %s' % (str(joint_state.motor_ids), controller.port_namespace)
+                    status.values.append(KeyValue('Goal', str(joint_state.goal_pos)))
+                    status.values.append(KeyValue('Position', str(joint_state.current_pos)))
+                    status.values.append(KeyValue('Error', str(joint_state.error)))
+                    status.values.append(KeyValue('Velocity', str(joint_state.velocity)))
+                    status.values.append(KeyValue('Load', str(joint_state.load)))
+                    status.values.append(KeyValue('Moving', str(joint_state.is_moving)))
+                    status.values.append(KeyValue('Temperature', str(max_temp)))
+                    status.level = DiagnosticStatus.OK
+                    status.message = 'OK'
+                        
+                    diag_msg.status.append(status)
+                except:
+                    pass
+                    
+            self.diagnostics_pub.publish(diag_msg)
+            rate.sleep()
+
+    def check_deps(self):
+        controllers_still_waiting = []
+        
+        for i,(controller_name,deps,kls) in enumerate(self.waiting_meta_controllers):
+            if not set(deps).issubset(self.controllers.keys()):
+                controllers_still_waiting.append(self.waiting_meta_controllers[i])
+                rospy.logwarn('[%s] not all dependencies started, still waiting for %s...' % (controller_name, str(list(set(deps).difference(self.controllers.keys())))))
+            else:
+                dependencies = [self.controllers[dep_name] for dep_name in deps]
+                controller = kls(controller_name, dependencies)
+                
+                if controller.initialize():
+                    controller.start()
+                    self.controllers[controller_name] = controller
+                    
+        self.waiting_meta_controllers = controllers_still_waiting[:]
+
+    def start_controller(self, req):
+        port_name = req.port_name
+        package_path = req.package_path
+        module_name = req.module_name
+        class_name = req.class_name
+        controller_name = req.controller_name
+        
+        self.start_controller_lock.acquire()
+        
+        if controller_name in self.controllers:
+            self.start_controller_lock.release()
+            return StartControllerResponse(False, 'Controller [%s] already started. If you want to restart it, call restart.' % controller_name)
+            
+        try:
+            if module_name not in sys.modules:
+                # import if module not previously imported
+                package_module = __import__(package_path, globals(), locals(), [module_name], -1)
+            else:
+                # reload module if previously imported
+                package_module = reload(sys.modules[package_path])
+            controller_module = getattr(package_module, module_name)
+        except ImportError, ie:
+            self.start_controller_lock.release()
+            return StartControllerResponse(False, 'Cannot find controller module. Unable to start controller %s\n%s' % (module_name, str(ie)))
+        except SyntaxError, se:
+            self.start_controller_lock.release()
+            return StartControllerResponse(False, 'Syntax error in controller module. Unable to start controller %s\n%s' % (module_name, str(se)))
+        except Exception, e:
+            self.start_controller_lock.release()
+            return StartControllerResponse(False, 'Unknown error has occured. Unable to start controller %s\n%s' % (module_name, str(e)))
+        
+        kls = getattr(controller_module, class_name)
+        
+        if port_name == 'meta':
+            self.waiting_meta_controllers.append((controller_name,req.dependencies,kls))
+            self.check_deps()
+            self.start_controller_lock.release()
+            return StartControllerResponse(True, '')
+            
+        if port_name != 'meta' and (port_name not in self.serial_proxies):
+            self.start_controller_lock.release()
+            return StartControllerResponse(False, 'Specified port [%s] not found, available ports are %s. Unable to start controller %s' % (port_name, str(self.serial_proxies.keys()), controller_name))
+            
+        controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name)
+        
+        if controller.initialize():
+            controller.start()
+            self.controllers[controller_name] = controller
+            
+            self.check_deps()
+            self.start_controller_lock.release()
+            
+            return StartControllerResponse(True, 'Controller %s successfully started.' % controller_name)
+        else:
+            self.start_controller_lock.release()
+            return StartControllerResponse(False, 'Initialization failed. Unable to start controller %s' % controller_name)
+
+    def stop_controller(self, req):
+        controller_name = req.controller_name
+        self.stop_controller_lock.acquire()
+        
+        if controller_name in self.controllers:
+            self.controllers[controller_name].stop()
+            del self.controllers[controller_name]
+            self.stop_controller_lock.release()
+            return StopControllerResponse(True, 'controller %s successfully stopped.' % controller_name)
+        else:
+            self.self.stop_controller_lock.release()
+            return StopControllerResponse(False, 'controller %s was not running.' % controller_name)
+
+    def restart_controller(self, req):
+        response1 = self.stop_controller(StopController(req.controller_name))
+        response2 = self.start_controller(req)
+        return RestartControllerResponse(response1.success and response2.success, '%s\n%s' % (response1.reason, response2.reason))
+
+if __name__ == '__main__':
+    try:
+        manager = ControllerManager()
+        rospy.spin()
+    except rospy.ROSInterruptException: pass
+

src/dynamixel_motor/dynamixel_controllers/nodes/controller_spawner.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import sys
+import os
+from optparse import OptionParser
+
+import rospy
+
+from dynamixel_controllers.srv import StartController
+from dynamixel_controllers.srv import StopController
+from dynamixel_controllers.srv import RestartController
+
+
+parser = OptionParser()
+
+def manage_controller(controller_name, port_namespace, controller_type, command, deps, start, stop, restart):
+    try:
+        controller = rospy.get_param(controller_name + '/controller')
+        package_path = controller['package']
+        module_name = controller['module']
+        class_name = controller['type']
+    except KeyError as ke:
+        rospy.logerr('[%s] configuration error: could not find controller parameters on parameter server' % controller_name)
+        sys.exit(1)
+    except Exception as e:
+        rospy.logerr('[%s]: %s' % (controller_name, e))
+        sys.exit(1)
+        
+    if command.lower() == 'start':
+        try:
+            response = start(port_namespace, package_path, module_name, class_name, controller_name, deps)
+            if response.success: rospy.loginfo(response.reason)
+            else: rospy.logerr(response.reason)
+        except rospy.ServiceException, e:
+            rospy.logerr('Service call failed: %s' % e)
+    elif command.lower() == 'stop':
+        try:
+            response = stop(controller_name)
+            if response.success: rospy.loginfo(response.reason)
+            else: rospy.logerr(response.reason)
+        except rospy.ServiceException, e:
+            rospy.logerr('Service call failed: %s' % e)
+    elif command.lower() == 'restart':
+        try:
+            response = restart(port_namespace, package_path, module_name, class_name, controller_name, deps)
+            if response.success: rospy.loginfo(response.reason)
+            else: rospy.logerr(response.reason)
+        except rospy.ServiceException, e:
+            rospy.logerr('Service call failed: %s' % e)
+    else:
+        rospy.logerr('Invalid command.')
+        parser.print_help()
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('controller_spawner', anonymous=True)
+        
+        parser.add_option('-m', '--manager', metavar='MANAGER',
+                          help='specified serial port is managed by MANAGER')
+        parser.add_option('-p', '--port', metavar='PORT',
+                          help='motors of specified controllers are connected to PORT')
+        parser.add_option('-t', '--type', metavar='TYPE', default='simple', choices=('simple','meta'),
+                          help='type of controller to be loaded (simple|meta) [default: %default]')
+        parser.add_option('-c', '--command', metavar='COMMAND', default='start', choices=('start','stop','restart'),
+                          help='command to perform on specified controllers: start, stop or restart [default: %default]')
+                          
+        (options, args) = parser.parse_args(rospy.myargv()[1:])
+        
+        if len(args) < 1:
+            parser.error('specify at least one controller name')
+            
+        manager_namespace = options.manager
+        port_namespace = options.port
+        controller_type = options.type
+        command = options.command
+        joint_controllers = args
+        
+        if controller_type == 'meta': port_namespace = 'meta'
+        
+        start_service_name = '%s/%s/start_controller' % (manager_namespace, port_namespace)
+        stop_service_name = '%s/%s/stop_controller' % (manager_namespace, port_namespace)
+        restart_service_name = '%s/%s/restart_controller' % (manager_namespace, port_namespace)
+        
+        parent_namespace = 'global' if rospy.get_namespace() == '/' else rospy.get_namespace()
+        rospy.loginfo('%s controller_spawner: waiting for controller_manager %s to startup in %s namespace...' % (port_namespace, manager_namespace, parent_namespace))
+        
+        rospy.wait_for_service(start_service_name)
+        rospy.wait_for_service(stop_service_name)
+        rospy.wait_for_service(restart_service_name)
+        
+        start_controller = rospy.ServiceProxy(start_service_name, StartController)
+        stop_controller = rospy.ServiceProxy(stop_service_name, StopController)
+        restart_controller = rospy.ServiceProxy(restart_service_name, RestartController)
+        
+        rospy.loginfo('%s controller_spawner: All services are up, spawning controllers...' % port_namespace)
+        
+        if controller_type == 'simple':
+            for controller_name in joint_controllers:
+                manage_controller(controller_name, port_namespace, controller_type, command, [], start_controller, stop_controller, restart_controller)
+        elif controller_type == 'meta':
+            controller_name = joint_controllers[0]
+            dependencies = joint_controllers[1:]
+            manage_controller(controller_name, port_namespace, controller_type, command, dependencies, start_controller, stop_controller, restart_controller)
+    except rospy.ROSInterruptException: pass
+

src/dynamixel_motor/dynamixel_controllers/package.xml

@@ -0,0 +1,39 @@
+<package>
+  <name>dynamixel_controllers</name>
+  <version>0.4.1</version>
+  <description>
+	This package contains a configurable node, services and a spawner script
+        to start, stop and restart one or more controller plugins. Reusable
+        controller types are defined for common Dynamixel motor joints. Both speed and
+        torque can be set for each joint. This python package can be used by more
+        specific robot controllers and all configurable parameters can be loaded
+        via a yaml file.
+  </description>
+  <maintainer email="arebgun@gmail.com">Antons Rebguns</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/dynamixel_controllers</url>
+  <url type="bugtracker">https://github.com/arebgun/dynamixel_motor</url>
+
+  <author>Antons Rebguns</author>
+  <author>Cody Jorgensen</author>
+  <author>Cara Slutter</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>diagnostic_msgs</run_depend>
+  <run_depend>dynamixel_msgs</run_depend>
+  <run_depend>control_msgs</run_depend>
+  <run_depend>dynamixel_driver</run_depend>
+  
+  <export>
+
+  </export>
+</package>

src/dynamixel_motor/dynamixel_controllers/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['dynamixel_controllers'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)

src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py

@@ -0,0 +1,178 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import math
+
+import rospy
+
+from dynamixel_driver.dynamixel_const import *
+
+from dynamixel_controllers.srv import SetSpeed
+from dynamixel_controllers.srv import TorqueEnable
+from dynamixel_controllers.srv import SetComplianceSlope
+from dynamixel_controllers.srv import SetComplianceMargin
+from dynamixel_controllers.srv import SetCompliancePunch
+from dynamixel_controllers.srv import SetTorqueLimit
+
+from std_msgs.msg import Float64
+from dynamixel_msgs.msg import MotorStateList
+from dynamixel_msgs.msg import JointState
+
+class JointController:
+    def __init__(self, dxl_io, controller_namespace, port_namespace):
+        self.running = False
+        self.dxl_io = dxl_io
+        self.controller_namespace = controller_namespace
+        self.port_namespace = port_namespace
+        self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name')
+        self.joint_speed = rospy.get_param(self.controller_namespace + '/joint_speed', 1.0)
+        self.compliance_slope = rospy.get_param(self.controller_namespace + '/joint_compliance_slope', None)
+        self.compliance_margin = rospy.get_param(self.controller_namespace + '/joint_compliance_margin', None)
+        self.compliance_punch = rospy.get_param(self.controller_namespace + '/joint_compliance_punch', None)
+        self.torque_limit = rospy.get_param(self.controller_namespace + '/joint_torque_limit', None)
+        
+        self.__ensure_limits()
+        
+        self.speed_service = rospy.Service(self.controller_namespace + '/set_speed', SetSpeed, self.process_set_speed)
+        self.torque_service = rospy.Service(self.controller_namespace + '/torque_enable', TorqueEnable, self.process_torque_enable)
+        self.compliance_slope_service = rospy.Service(self.controller_namespace + '/set_compliance_slope', SetComplianceSlope, self.process_set_compliance_slope)
+        self.compliance_marigin_service = rospy.Service(self.controller_namespace + '/set_compliance_margin', SetComplianceMargin, self.process_set_compliance_margin)
+        self.compliance_punch_service = rospy.Service(self.controller_namespace + '/set_compliance_punch', SetCompliancePunch, self.process_set_compliance_punch)
+        self.torque_limit_service = rospy.Service(self.controller_namespace + '/set_torque_limit', SetTorqueLimit, self.process_set_torque_limit)
+
+    def __ensure_limits(self):
+        if self.compliance_slope is not None:
+            if self.compliance_slope < DXL_MIN_COMPLIANCE_SLOPE: self.compliance_slope = DXL_MIN_COMPLIANCE_SLOPE
+            elif self.compliance_slope > DXL_MAX_COMPLIANCE_SLOPE: self.compliance_slope = DXL_MAX_COMPLIANCE_SLOPE
+            else: self.compliance_slope = int(self.compliance_slope)
+            
+        if self.compliance_margin is not None:
+            if self.compliance_margin < DXL_MIN_COMPLIANCE_MARGIN: self.compliance_margin = DXL_MIN_COMPLIANCE_MARGIN
+            elif self.compliance_margin > DXL_MAX_COMPLIANCE_MARGIN: self.compliance_margin = DXL_MAX_COMPLIANCE_MARGIN
+            else: self.compliance_margin = int(self.compliance_margin)
+            
+        if self.compliance_punch is not None:
+            if self.compliance_punch < DXL_MIN_PUNCH: self.compliance_punch = DXL_MIN_PUNCH
+            elif self.compliance_punch > DXL_MAX_PUNCH: self.compliance_punch = DXL_MAX_PUNCH
+            else: self.compliance_punch = int(self.compliance_punch)
+            
+        if self.torque_limit is not None:
+            if self.torque_limit < 0: self.torque_limit = 0.0
+            elif self.torque_limit > 1: self.torque_limit = 1.0
+
+    def initialize(self):
+        raise NotImplementedError
+
+    def start(self):
+        self.running = True
+        self.joint_state_pub = rospy.Publisher(self.controller_namespace + '/state', JointState, queue_size=1)
+        self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', Float64, self.process_command)
+        self.motor_states_sub = rospy.Subscriber('motor_states/%s' % self.port_namespace, MotorStateList, self.process_motor_states)
+
+    def stop(self):
+        self.running = False
+        self.joint_state_pub.unregister()
+        self.motor_states_sub.unregister()
+        self.command_sub.unregister()
+        self.speed_service.shutdown('normal shutdown')
+        self.torque_service.shutdown('normal shutdown')
+        self.compliance_slope_service.shutdown('normal shutdown')
+
+    def set_torque_enable(self, torque_enable):
+        raise NotImplementedError
+
+    def set_speed(self, speed):
+        raise NotImplementedError
+
+    def set_compliance_slope(self, slope):
+        raise NotImplementedError
+
+    def set_compliance_margin(self, margin):
+        raise NotImplementedError
+
+    def set_compliance_punch(self, punch):
+        raise NotImplementedError
+
+    def set_torque_limit(self, max_torque):
+        raise NotImplementedError
+
+    def process_set_speed(self, req):
+        self.set_speed(req.speed)
+        return [] # success
+
+    def process_torque_enable(self, req):
+        self.set_torque_enable(req.torque_enable)
+        return []
+
+    def process_set_compliance_slope(self, req):
+        self.set_compliance_slope(req.slope)
+        return []
+
+    def process_set_compliance_margin(self, req):
+        self.set_compliance_margin(req.margin)
+        return []
+
+    def process_set_compliance_punch(self, req):
+        self.set_compliance_punch(req.punch)
+        return []
+
+    def process_set_torque_limit(self, req):
+        self.set_torque_limit(req.torque_limit)
+        return []
+
+    def process_motor_states(self, state_list):
+        raise NotImplementedError
+
+    def process_command(self, msg):
+        raise NotImplementedError
+
+    def rad_to_raw(self, angle, initial_position_raw, flipped, encoder_ticks_per_radian):
+        """ angle is in radians """
+        #print 'flipped = %s, angle_in = %f, init_raw = %d' % (str(flipped), angle, initial_position_raw)
+        angle_raw = angle * encoder_ticks_per_radian
+        #print 'angle = %f, val = %d' % (math.degrees(angle), int(round(initial_position_raw - angle_raw if flipped else initial_position_raw + angle_raw)))
+        return int(round(initial_position_raw - angle_raw if flipped else initial_position_raw + angle_raw))
+
+    def raw_to_rad(self, raw, initial_position_raw, flipped, radians_per_encoder_tick):
+        return (initial_position_raw - raw if flipped else raw - initial_position_raw) * radians_per_encoder_tick
+

src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

@@ -0,0 +1,187 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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 __future__ import division
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+__credits__ = 'Cara Slutter'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import rospy
+
+from dynamixel_driver.dynamixel_const import *
+from dynamixel_controllers.joint_controller import JointController
+
+from dynamixel_msgs.msg import JointState
+
+class JointPositionController(JointController):
+    def __init__(self, dxl_io, controller_namespace, port_namespace):
+        JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
+        
+        self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id')
+        self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
+        self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
+        self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
+        if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
+            self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration')
+        else:
+            self.acceleration = None
+        
+        self.flipped = self.min_angle_raw > self.max_angle_raw
+        
+        self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
+
+    def initialize(self):
+        # verify that the expected motor is connected and responding
+        available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
+        if not self.motor_id in available_ids:
+            rospy.logwarn('The specified motor id is not connected and responding.')
+            rospy.logwarn('Available ids: %s' % str(available_ids))
+            rospy.logwarn('Specified id: %d' % self.motor_id)
+            return False
+            
+        self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.motor_id))
+        self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.motor_id))
+        
+        if self.flipped:
+            self.min_angle = (self.initial_position_raw - self.min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.max_angle = (self.initial_position_raw - self.max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+        else:
+            self.min_angle = (self.min_angle_raw - self.initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.max_angle = (self.max_angle_raw - self.initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            
+        self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.motor_id))
+        self.MAX_POSITION = self.ENCODER_RESOLUTION - 1
+        self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.motor_id))
+        self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.motor_id))
+        self.MIN_VELOCITY = self.VELOCITY_PER_TICK
+        
+        if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
+        if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin)
+        if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch)
+        if self.torque_limit is not None: self.set_torque_limit(self.torque_limit)
+        if self.acceleration is not None:
+            rospy.loginfo("Setting acceleration of %d to %d" % (self.motor_id, self.acceleration))
+            self.dxl_io.set_acceleration(self.motor_id, self.acceleration)
+
+        self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY)
+        
+        if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY
+        elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY
+        
+        if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY
+        elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed
+        
+        self.set_speed(self.joint_speed)
+        
+        return True
+
+    def pos_rad_to_raw(self, pos_rad):
+        if pos_rad < self.min_angle: pos_rad = self.min_angle
+        elif pos_rad > self.max_angle: pos_rad = self.max_angle
+        return self.rad_to_raw(pos_rad, self.initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN)
+
+    def spd_rad_to_raw(self, spd_rad):
+        if spd_rad < self.MIN_VELOCITY: spd_rad = self.MIN_VELOCITY
+        elif spd_rad > self.joint_max_speed: spd_rad = self.joint_max_speed
+        # velocity of 0 means maximum, make sure that doesn't happen
+        return max(1, int(round(spd_rad / self.VELOCITY_PER_TICK)))
+
+    def set_torque_enable(self, torque_enable):
+        mcv = (self.motor_id, torque_enable)
+        self.dxl_io.set_multi_torque_enabled([mcv])
+
+    def set_speed(self, speed):
+        mcv = (self.motor_id, self.spd_rad_to_raw(speed))
+        self.dxl_io.set_multi_speed([mcv])
+
+    def set_compliance_slope(self, slope):
+        if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
+        elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
+        mcv = (self.motor_id, slope, slope)
+        self.dxl_io.set_multi_compliance_slopes([mcv])
+
+    def set_compliance_margin(self, margin):
+        if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
+        elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
+        else: margin = int(margin)
+        mcv = (self.motor_id, margin, margin)
+        self.dxl_io.set_multi_compliance_margins([mcv])
+
+    def set_compliance_punch(self, punch):
+        if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
+        elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
+        else: punch = int(punch)
+        mcv = (self.motor_id, punch)
+        self.dxl_io.set_multi_punch([mcv])
+
+    def set_torque_limit(self, max_torque):
+        if max_torque > 1: max_torque = 1.0         # use all torque motor can provide
+        elif max_torque < 0: max_torque = 0.0       # turn off motor torque
+        raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
+        mcv = (self.motor_id, raw_torque_val)
+        self.dxl_io.set_multi_torque_limit([mcv])
+
+    def set_acceleration_raw(self, acc):
+        if acc < 0: acc = 0
+        elif acc > 254: acc = 254
+        self.dxl_io.set_acceleration(self.motor_id, acc)
+
+    def process_motor_states(self, state_list):
+        if self.running:
+            state = filter(lambda state: state.id == self.motor_id, state_list.motor_states)
+            if state:
+                state = state[0]
+                self.joint_state.motor_temps = [state.temperature]
+                self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
+                self.joint_state.current_pos = self.raw_to_rad(state.position, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
+                self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK
+                self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK
+                self.joint_state.load = state.load
+                self.joint_state.is_moving = state.moving
+                self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
+                
+                self.joint_state_pub.publish(self.joint_state)
+
+    def process_command(self, msg):
+        angle = msg.data
+        mcv = (self.motor_id, self.pos_rad_to_raw(angle))
+        self.dxl_io.set_multi_position([mcv])
+

src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py

@@ -0,0 +1,205 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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 __future__ import division
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+__credits__ = 'Cara Slutter'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import rospy
+
+from dynamixel_driver.dynamixel_const import *
+from dynamixel_controllers.joint_controller import JointController
+
+from dynamixel_msgs.msg import JointState
+
+class JointPositionControllerDual(JointController):
+    def __init__(self, dxl_io, controller_namespace, port_namespace):
+        JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
+        
+        self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id')
+        self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init')
+        self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min')
+        self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max')
+        
+        self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id')
+        self.slave_offset = rospy.get_param(self.controller_namespace + '/motor_slave/calibration_offset', 0)
+        
+        self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
+        
+        self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
+
+    def initialize(self):
+        # verify that the expected motor is connected and responding
+        available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
+        if not (self.master_id in available_ids and
+                self.slave_id in available_ids):
+            rospy.logwarn('The specified motor id is not connected and responding.')
+            rospy.logwarn('Available ids: %s' % str(available_ids))
+            rospy.logwarn('Specified ids: %d %d' % (self.master_id, self.slave_id))
+            return False
+            
+        self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.master_id))
+        self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.master_id))
+        
+        if self.flipped:
+            self.master_min_angle = (self.master_initial_position_raw - self.master_min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.master_max_angle = (self.master_initial_position_raw - self.master_max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+        else:
+            self.master_min_angle = (self.master_min_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.master_max_angle = (self.master_max_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            
+        self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.master_id))
+        self.MAX_POSITION = self.ENCODER_RESOLUTION - 1
+        self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.master_id))
+        self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.master_id))
+        self.MIN_VELOCITY = self.VELOCITY_PER_TICK
+        
+        if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
+        if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin)
+        if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch)
+        if self.torque_limit is not None: self.set_torque_limit(self.torque_limit)
+        
+        self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY)
+        
+        if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY
+        elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY
+        
+        if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY
+        elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed
+        
+        self.set_speed(self.joint_speed)
+        
+        return True
+
+    def pos_rad_to_raw(self, angle):
+        if angle < self.master_min_angle: 
+            angle = self.master_min_angle
+        elif angle > self.master_max_angle: 
+            angle = self.master_max_angle
+        mcv_master = self.rad_to_raw(angle, self.master_initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN)
+        mcv_slave = self.MAX_POSITION - mcv_master + self.slave_offset
+        if mcv_slave < 0: 
+            mcv_slave = 0
+        elif mcv_slave > self.MAX_POSITION: 
+            mcv_slave = self.MAX_POSITION
+        return (mcv_master, mcv_slave)
+        
+    def spd_rad_to_raw(self, spd_rad):
+        if spd_rad < self.MIN_VELOCITY: 
+            spd_rad = self.MIN_VELOCITY
+        elif spd_rad > self.joint_max_speed: 
+            spd_rad = self.joint_max_speed
+        # velocity of 0 means maximum, make sure that doesn't happen
+        return max(1, int(round(spd_rad / self.VELOCITY_PER_TICK)))
+        
+    def set_torque_enable(self, torque_enable):
+        mcv_master = (self.master_id, torque_enable)
+        mcv_slave = (self.slave_id, torque_enable)
+        self.dxl_io.set_multi_torque_enabled([mcv_master, mcv_slave])
+
+    def set_speed(self, speed):
+        if speed < self.MIN_VELOCITY: speed = self.MIN_VELOCITY
+        elif speed > self.joint_max_speed: speed = self.joint_max_speed
+        speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
+        mcv_master = (self.master_id, speed_raw if speed_raw > 0 else 1)
+        mcv_slave = (self.slave_id, mcv_master[1])
+        self.dxl_io.set_multi_speed([mcv_master, mcv_slave])
+
+    def set_compliance_slope(self, slope):
+        if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
+        elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
+        mcv_master = (self.master_id, slope, slope)
+        mcv_slave = (self.slave_id, slope, slope)
+        self.dxl_io.set_multi_compliance_slopes([mcv_master, mcv_slave])
+
+    def set_compliance_margin(self, margin):
+        if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
+        elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
+        else: margin = int(margin)
+        mcv_master = (self.master_id, margin, margin)
+        mcv_slave = (self.slave_id, margin, margin)
+        self.dxl_io.set_multi_compliance_margins([mcv_master, mcv_slave])
+
+    def set_compliance_punch(self, punch):
+        if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
+        elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
+        else: punch = int(punch)
+        mcv_master = (self.master_id, punch)
+        mcv_slave = (self.slave_id, punch)
+        self.dxl_io.set_multi_punch([mcv_master, mcv_slave])
+
+    def set_torque_limit(self, max_torque):
+        if max_torque > 1: max_torque = 1.0
+        elif max_torque < 0: max_torque = 0.0  # turn off motor torque
+        raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
+        mcv_master = (self.master_id, raw_torque_val)
+        mcv_slave = (self.slave_id, raw_torque_val)
+        self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave])
+
+    def process_motor_states(self, state_list):
+        if self.running:
+            states = {}
+            
+            for state in state_list.motor_states:
+                if state.id in [self.master_id, self.slave_id]: states[state.id] = state
+                               
+            if self.master_id in states and self.slave_id in states:
+                state = states[self.master_id]
+                self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature]
+                self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
+                self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
+                self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK
+                self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK
+                self.joint_state.load = state.load
+                self.joint_state.is_moving = state.moving
+                self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
+                self.joint_state_pub.publish(self.joint_state)
+
+    def process_command(self, msg):
+        angle = msg.data
+        if angle < self.master_min_angle: angle = self.master_min_angle
+        elif angle > self.master_max_angle: angle = self.master_max_angle
+        mcv_master = (self.master_id, self.rad_to_raw(angle, self.master_initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN))
+        mcv_slave = [self.slave_id, self.MAX_POSITION - mcv_master[1] + self.slave_offset]
+        if mcv_slave[1] < 0: mcv_slave[1] = 0
+        elif mcv_slave[1] > self.MAX_POSITION: mcv_slave[1] = self.MAX_POSITION
+        self.dxl_io.set_multi_position([mcv_master, mcv_slave])

src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller.py

@@ -0,0 +1,170 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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 __future__ import division
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+__credits__ = 'Cody Jorgensen'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import rospy
+from dynamixel_driver.dynamixel_const import *
+from dynamixel_controllers.joint_controller import JointController
+
+from dynamixel_msgs.msg import JointState
+from std_msgs.msg import Float64
+
+class JointTorqueController(JointController):
+    def __init__(self, dxl_io, controller_namespace, port_namespace):
+        JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
+        
+        self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id')
+        self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
+        self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
+        self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
+        
+        self.flipped = self.min_angle_raw > self.max_angle_raw
+        self.last_commanded_torque = 0.0
+        
+        self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
+
+    def initialize(self):
+        # verify that the expected motor is connected and responding
+        available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
+        
+        if not self.motor_id in available_ids:
+            rospy.logwarn('The specified motor id is not connected and responding.')
+            rospy.logwarn('Available ids: %s' % str(available_ids))
+            rospy.logwarn('Specified id: %d' % self.motor_id)
+            return False
+            
+        self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.motor_id))
+        self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.motor_id))
+        
+        if self.flipped:
+            self.min_angle = (self.initial_position_raw - self.min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.max_angle = (self.initial_position_raw - self.max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+        else:
+            self.min_angle = (self.min_angle_raw - self.initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.max_angle = (self.max_angle_raw - self.initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            
+        self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.motor_id))
+        self.MAX_POSITION = self.ENCODER_RESOLUTION - 1
+        self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.motor_id))
+        self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.motor_id))
+        self.MIN_VELOCITY = self.VELOCITY_PER_TICK
+        
+        if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
+        if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin)
+        if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch)
+        if self.torque_limit is not None: self.set_torque_limit(self.torque_limit)
+        
+        self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY)
+        
+        if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY
+        elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY
+        
+        if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY
+        elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed
+        
+        self.set_speed(0.0)
+        
+        return True
+
+    def spd_rad_to_raw(self, spd_rad):
+        if spd_rad < -self.joint_max_speed: spd_rad = -self.joint_max_speed
+        elif spd_rad > self.joint_max_speed: spd_rad = self.joint_max_speed
+        self.last_commanded_torque = spd_rad
+        return int(round(spd_rad / self.VELOCITY_PER_TICK))
+
+    def set_torque_enable(self, torque_enable):
+        mcv = (self.motor_id, torque_enable)
+        self.dxl_io.set_multi_torque_enabled([mcv])
+
+    def set_speed(self, speed):
+        mcv = (self.motor_id, self.spd_rad_to_raw(speed))
+        self.dxl_io.set_multi_speed([mcv])
+
+    def set_compliance_slope(self, slope):
+        if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
+        elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
+        mcv = (self.motor_id, slope, slope)
+        self.dxl_io.set_multi_compliance_slopes([mcv])
+
+    def set_compliance_margin(self, margin):
+        if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
+        elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
+        else: margin = int(margin)
+        mcv = (self.motor_id, margin, margin)
+        self.dxl_io.set_multi_compliance_margins([mcv])
+
+    def set_compliance_punch(self, punch):
+        if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
+        elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
+        else: punch = int(punch)
+        mcv = (self.motor_id, punch)
+        self.dxl_io.set_multi_punch([mcv])
+
+    def set_torque_limit(self, max_torque):
+        if max_torque > 1: max_torque = 1.0         # use all torque motor can provide
+        elif max_torque < 0: max_torque = 0.0       # turn off motor torque
+        raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
+        mcv = (self.motor_id, raw_torque_val)
+        self.dxl_io.set_multi_torque_limit([mcv])
+
+    def process_motor_states(self, state_list):
+        if self.running:
+            state = filter(lambda state: state.id == self.motor_id, state_list.motor_states)
+            if state:
+                state = state[0]
+                self.joint_state.motor_temps = [state.temperature]
+                self.joint_state.goal_pos = self.last_commanded_torque
+                self.joint_state.current_pos = self.raw_to_rad(state.position, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
+                self.joint_state.error = 0.0
+                self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK
+                self.joint_state.load = state.load
+                self.joint_state.is_moving = state.moving
+                self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
+                
+                self.joint_state_pub.publish(self.joint_state)
+
+    def process_command(self, msg):
+        self.set_speed(msg.data)
+

src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py

@@ -0,0 +1,188 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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 __future__ import division
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import rospy
+
+from dynamixel_driver.dynamixel_const import *
+from dynamixel_controllers.joint_controller import JointController
+
+from dynamixel_msgs.msg import JointState
+
+class JointTorqueControllerDualMotor(JointController):
+    def __init__(self, dxl_io, controller_namespace, port_namespace):
+        JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
+        
+        self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id')
+        self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init')
+        self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min')
+        self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max')
+        
+        self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id')
+        
+        self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
+        self.last_commanded_torque = 0.0
+        
+        self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
+
+
+    def initialize(self):
+        # verify that the expected motor is connected and responding
+        available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
+        if not (self.master_id in available_ids and
+                self.slave_id in available_ids):
+            rospy.logwarn('The specified motor id is not connected and responding.')
+            rospy.logwarn('Available ids: %s' % str(available_ids))
+            rospy.logwarn('Specified ids: %d %d' % (self.master_id, self.slave_id))
+            return False
+            
+        self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.master_id))
+        self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.master_id))
+        
+        if self.flipped:
+            self.master_min_angle = (self.master_initial_position_raw - self.master_min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.master_max_angle = (self.master_initial_position_raw - self.master_max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
+        else:
+            self.master_min_angle = (self.master_min_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            self.master_max_angle = (self.master_max_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
+            
+        self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.master_id))
+        self.MAX_POSITION = self.ENCODER_RESOLUTION - 1
+        self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.master_id))
+        self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.master_id))
+        self.MIN_VELOCITY = self.VELOCITY_PER_TICK
+        
+        if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
+        if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin)
+        if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch)
+        if self.torque_limit is not None: self.set_torque_limit(self.torque_limit)
+        
+        self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY)
+        
+        if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY
+        elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY
+        
+        if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY
+        elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed
+        
+        self.set_speed(0.0)
+        
+        return True
+
+
+    def set_torque_enable(self, torque_enable):
+        mcv_master = (self.master_id, torque_enable)
+        mcv_slave = (self.slave_id, torque_enable)
+        self.dxl_io.set_multi_torque_enabled([mcv_master, mcv_slave])
+
+
+    def set_speed(self, speed):
+        if speed < -self.joint_max_speed: speed = -self.joint_max_speed
+        elif speed > self.joint_max_speed: speed = self.joint_max_speed
+        self.last_commanded_torque = speed
+        speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
+        mcv_master = (self.master_id, speed_raw)
+        mcv_slave = (self.slave_id, -mcv_master[1])
+        self.dxl_io.set_multi_speed([mcv_master, mcv_slave])
+
+
+    def set_compliance_slope(self, slope):
+        if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
+        elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
+        mcv_master = (self.master_id, slope, slope)
+        mcv_slave = (self.slave_id, slope, slope)
+        self.dxl_io.set_multi_compliance_slopes([mcv_master, mcv_slave])
+
+
+    def set_compliance_margin(self, margin):
+        if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
+        elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
+        else: margin = int(margin)
+        mcv_master = (self.master_id, margin, margin)
+        mcv_slave = (self.slave_id, margin, margin)
+        self.dxl_io.set_multi_compliance_margins([mcv_master, mcv_slave])
+
+
+    def set_compliance_punch(self, punch):
+        if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
+        elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
+        else: punch = int(punch)
+        mcv_master = (self.master_id, punch)
+        mcv_slave = (self.slave_id, punch)
+        self.dxl_io.set_multi_punch([mcv_master, mcv_slave])
+
+
+    def set_torque_limit(self, max_torque):
+        if max_torque > 1: max_torque = 1.0
+        elif max_torque < 0: max_torque = 0.0     # turn off motor torque
+        raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
+        mcv_master = (self.master_id, raw_torque_val)
+        mcv_slave = (self.slave_id, raw_torque_val)
+        self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave])
+
+
+    def process_motor_states(self, state_list):
+        if self.running:
+            states = {}
+            
+            for state in state_list.motor_states:
+                if state.id in [self.master_id, self.slave_id]: states[state.id] = state
+                
+            if states:
+                state = states[self.master_id]
+                self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature]
+                self.joint_state.goal_pos = self.last_commanded_torque
+                self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
+                self.joint_state.error = 0.0
+                self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK
+                self.joint_state.load = state.load
+                self.joint_state.is_moving = state.moving
+                self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
+                
+                self.joint_state_pub.publish(self.joint_state)
+
+
+    def process_command(self, msg):
+        self.set_speed(msg.data)
+

src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py

@@ -0,0 +1,360 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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 __future__ import division
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+from threading import Thread
+
+import rospy
+import actionlib
+
+from std_msgs.msg import Float64
+from trajectory_msgs.msg import JointTrajectory
+from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import FollowJointTrajectoryFeedback
+from control_msgs.msg import FollowJointTrajectoryResult
+
+
+class Segment():
+    def __init__(self, num_joints):
+        self.start_time = 0.0  # trajectory segment start time
+        self.duration = 0.0  # trajectory segment duration
+        self.positions = [0.0] * num_joints
+        self.velocities = [0.0] * num_joints
+
+class JointTrajectoryActionController():
+    def __init__(self, controller_namespace, controllers):
+        self.update_rate = 1000
+        self.state_update_rate = 50
+        self.trajectory = []
+        
+        self.controller_namespace = controller_namespace
+        self.joint_names = [c.joint_name for c in controllers]
+        
+        self.joint_to_controller = {}
+        for c in controllers:
+            self.joint_to_controller[c.joint_name] = c
+            
+        self.port_to_joints = {}
+        for c in controllers:
+            if c.port_namespace not in self.port_to_joints: self.port_to_joints[c.port_namespace] = []
+            self.port_to_joints[c.port_namespace].append(c.joint_name)
+            
+        self.port_to_io = {}
+        for c in controllers:
+            if c.port_namespace in self.port_to_io: continue
+            self.port_to_io[c.port_namespace] = c.dxl_io
+            
+        self.joint_states = dict(zip(self.joint_names, [c.joint_state for c in controllers]))
+        self.num_joints = len(self.joint_names)
+        self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
+
+    def initialize(self):
+        ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
+        self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
+        self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01)
+        self.goal_constraints = []
+        self.trajectory_constraints = []
+        self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1)
+        
+        for joint in self.joint_names:
+            self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0))
+            self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
+            
+        # Message containing current state for all controlled joints
+        self.msg = FollowJointTrajectoryFeedback()
+        self.msg.joint_names = self.joint_names
+        self.msg.desired.positions = [0.0] * self.num_joints
+        self.msg.desired.velocities = [0.0] * self.num_joints
+        self.msg.desired.accelerations = [0.0] * self.num_joints
+        self.msg.actual.positions = [0.0] * self.num_joints
+        self.msg.actual.velocities = [0.0] * self.num_joints
+        self.msg.error.positions = [0.0] * self.num_joints
+        self.msg.error.velocities = [0.0] * self.num_joints
+        
+        return True
+
+
+    def start(self):
+        self.running = True
+        
+        self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', JointTrajectory, self.process_command)
+        self.state_pub = rospy.Publisher(self.controller_namespace + '/state', FollowJointTrajectoryFeedback, queue_size=1)
+        self.action_server = actionlib.SimpleActionServer(self.controller_namespace + '/follow_joint_trajectory',
+                                                          FollowJointTrajectoryAction,
+                                                          execute_cb=self.process_follow_trajectory,
+                                                          auto_start=False)
+        self.action_server.start()
+        Thread(target=self.update_state).start()
+
+    def stop(self):
+        self.running = False
+
+    def process_command(self, msg):
+        if self.action_server.is_active(): self.action_server.set_preempted()
+        
+        while self.action_server.is_active():
+            rospy.sleep(0.01)
+            
+        self.process_trajectory(msg)
+
+    def process_follow_trajectory(self, goal):
+        self.process_trajectory(goal.trajectory)
+
+    def process_trajectory(self, traj):
+        num_points = len(traj.points)
+        
+        # make sure the joints in the goal match the joints of the controller
+        if set(self.joint_names) != set(traj.joint_names):
+            res = FollowJointTrajectoryResult()
+            res.error_code=FollowJointTrajectoryResult.INVALID_JOINTS
+            msg = 'Incoming trajectory joints do not match the joints of the controller'
+            rospy.logerr(msg)
+            rospy.logerr(' self.joint_names={%s}' % (set(self.joint_names)))
+            rospy.logerr(' traj.joint_names={%s}' % (set(traj.joint_names)))
+            self.action_server.set_aborted(result=res, text=msg)
+            return
+            
+        # make sure trajectory is not empty
+        if num_points == 0:
+            msg = 'Incoming trajectory is empty'
+            rospy.logerr(msg)
+            self.action_server.set_aborted(text=msg)
+            return
+            
+        # correlate the joints we're commanding to the joints in the message
+        # map from an index of joint in the controller to an index in the trajectory
+        lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
+        durations = [0.0] * num_points
+        
+        # find out the duration of each segment in the trajectory
+        durations[0] = traj.points[0].time_from_start.to_sec()
+        
+        for i in range(1, num_points):
+            durations[i] = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec()
+            
+        if not traj.points[0].positions:
+            res = FollowJointTrajectoryResult()
+            res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
+            msg = 'First point of trajectory has no positions'
+            rospy.logerr(msg)
+            self.action_server.set_aborted(result=res, text=msg)
+            return
+            
+        trajectory = []
+        time = rospy.Time.now() + rospy.Duration(0.01)
+        
+        for i in range(num_points):
+            seg = Segment(self.num_joints)
+            
+            if traj.header.stamp == rospy.Time(0.0):
+                seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
+            else:
+                seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
+                
+            seg.duration = durations[i]
+            
+            # Checks that the incoming segment has the right number of elements.
+            if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
+                res = FollowJointTrajectoryResult()
+                res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
+                msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
+                rospy.logerr(msg)
+                self.action_server.set_aborted(result=res, text=msg)
+                return
+                
+            if len(traj.points[i].positions) != self.num_joints:
+                res = FollowJointTrajectoryResult()
+                res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
+                msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
+                rospy.logerr(msg)
+                self.action_server.set_aborted(result=res, text=msg)
+                return
+                
+            for j in range(self.num_joints):
+                if traj.points[i].velocities:
+                    seg.velocities[j] = traj.points[i].velocities[lookup[j]]
+                if traj.points[i].positions:
+                    seg.positions[j] = traj.points[i].positions[lookup[j]]
+                    
+            trajectory.append(seg)
+            
+        rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
+        rate = rospy.Rate(self.update_rate)
+        
+        while traj.header.stamp > time:
+            time = rospy.Time.now()
+            rate.sleep()
+            
+        end_time = traj.header.stamp + rospy.Duration(sum(durations))
+        seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
+        
+        rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
+        
+        self.trajectory = trajectory
+        traj_start_time = rospy.Time.now()
+        
+        for seg in range(len(trajectory)):
+            rospy.logdebug('current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
+            rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
+            
+            # first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
+            if durations[seg] == 0:
+                rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
+                continue
+                
+            multi_packet = {}
+            
+            for port, joints in self.port_to_joints.items():
+                vals = []
+                
+                for joint in joints:
+                    j = self.joint_names.index(joint)
+                    
+                    start_position = self.joint_states[joint].current_pos
+                    if seg != 0: start_position = trajectory[seg - 1].positions[j]
+                        
+                    desired_position = trajectory[seg].positions[j]
+                    desired_velocity = max(self.min_velocity, abs(desired_position - start_position) / durations[seg])
+                    
+                    self.msg.desired.positions[j] = desired_position
+                    self.msg.desired.velocities[j] = desired_velocity
+                    
+                    # probably need a more elegant way of figuring out if we are dealing with a dual controller
+                    if hasattr(self.joint_to_controller[joint], "master_id"):
+                        master_id = self.joint_to_controller[joint].master_id
+                        slave_id = self.joint_to_controller[joint].slave_id
+                        master_pos, slave_pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
+                        spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
+                        vals.append((master_id, master_pos, spd))
+                        vals.append((slave_id, slave_pos, spd))
+                    else:
+                        motor_id = self.joint_to_controller[joint].motor_id
+                        pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
+                        spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
+                        vals.append((motor_id, pos, spd))
+                    
+                multi_packet[port] = vals
+                
+            for port, vals in multi_packet.items():
+                self.port_to_io[port].set_multi_position_and_speed(vals)
+                
+            while time < seg_end_times[seg]:
+                # check if new trajectory was received, if so abort current trajectory execution
+                # by setting the goal to the current position
+                if self.action_server.is_preempt_requested():
+                    msg = 'New trajectory received. Aborting old trajectory.'
+                    multi_packet = {}
+                    
+                    for port, joints in self.port_to_joints.items():
+                        vals = []
+                        
+                        for joint in joints:
+                            cur_pos = self.joint_states[joint].current_pos
+                            
+                            motor_id = self.joint_to_controller[joint].motor_id
+                            pos = self.joint_to_controller[joint].pos_rad_to_raw(cur_pos)
+                            
+                            vals.append((motor_id, pos))
+                            
+                        multi_packet[port] = vals
+                        
+                    for port, vals in multi_packet.items():
+                        self.port_to_io[port].set_multi_position(vals)
+                        
+                    self.action_server.set_preempted(text=msg)
+                    rospy.logwarn(msg)
+                    return
+                    
+                rate.sleep()
+                time = rospy.Time.now()
+                
+            # Verifies trajectory constraints
+            for j, joint in enumerate(self.joint_names):
+                if self.trajectory_constraints[j] > 0 and self.msg.error.positions[j] > self.trajectory_constraints[j]:
+                    res = FollowJointTrajectoryResult()
+                    res.error_code=FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
+                    msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
+                           (joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
+                    rospy.logwarn(msg)
+                    self.action_server.set_aborted(result=res, text=msg)
+                    return
+                    
+        # let motors roll for specified amount of time
+        rospy.sleep(self.goal_time_constraint)
+        
+        for i, j in enumerate(self.joint_names):
+            rospy.logdebug('desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i]))
+            
+        # Checks that we have ended inside the goal constraints
+        for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
+            if pos_constraint > 0 and abs(pos_error) > pos_constraint:
+                res = FollowJointTrajectoryResult()
+                res.error_code=FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
+                msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
+                      (joint, pos_error, pos_constraint)
+                rospy.logwarn(msg)
+                self.action_server.set_aborted(result=res, text=msg)
+                break
+        else:
+	    msg = 'Trajectory execution successfully completed'
+	    rospy.loginfo(msg)
+	    res = FollowJointTrajectoryResult()  
+	    res.error_code=FollowJointTrajectoryResult.SUCCESSFUL
+            self.action_server.set_succeeded(result=res, text=msg)
+
+    def update_state(self):
+        rate = rospy.Rate(self.state_update_rate)
+        while self.running and not rospy.is_shutdown():
+            self.msg.header.stamp = rospy.Time.now()
+            
+            # Publish current joint state
+            for i, joint in enumerate(self.joint_names):
+                state = self.joint_states[joint]
+                self.msg.actual.positions[i] = state.current_pos
+                self.msg.actual.velocities[i] = abs(state.velocity)
+                self.msg.error.positions[i] = self.msg.actual.positions[i] - self.msg.desired.positions[i]
+                self.msg.error.velocities[i] = self.msg.actual.velocities[i] - self.msg.desired.velocities[i]
+                
+            self.state_pub.publish(self.msg)
+            rate.sleep()

src/dynamixel_motor/dynamixel_controllers/srv/RestartController.srv

@@ -0,0 +1,9 @@
+string port_name            # serial port that this controller is connected to
+string package_path         # path to ROS package containing controller module
+string module_name          # name of the controller module within that package
+string class_name           # controller class name within that module
+string controller_name      # path to controller config parameters on param server
+string[] dependencies       # optional, list names of all controllers this controller depends on
+---
+bool success
+string reason

src/dynamixel_motor/dynamixel_controllers/srv/SetComplianceMargin.srv

@@ -0,0 +1,4 @@
+# Range is 0 to 255, larger value = more error
+
+uint8 margin
+---

src/dynamixel_motor/dynamixel_controllers/srv/SetCompliancePunch.srv

@@ -0,0 +1,4 @@
+# See Dynamixel documentation for details
+
+uint8 punch
+---

src/dynamixel_motor/dynamixel_controllers/srv/SetComplianceSlope.srv

@@ -0,0 +1,4 @@
+# See Dynamixel documentation for details
+
+uint8 slope
+---

src/dynamixel_motor/dynamixel_controllers/srv/SetSpeed.srv

@@ -0,0 +1,3 @@
+float64 speed
+---
+

src/dynamixel_motor/dynamixel_controllers/srv/SetTorqueLimit.srv

@@ -0,0 +1,5 @@
+# Set maximum motor torque in percenatge (0 - no torque to 1 - maximum torque)
+
+float64 torque_limit
+---
+

src/dynamixel_motor/dynamixel_controllers/srv/StartController.srv

@@ -0,0 +1,9 @@
+string port_name            # serial port that this controller is connected to
+string package_path         # path to ROS package containing controller module
+string module_name          # name of the controller module within that package
+string class_name           # controller class name within that module
+string controller_name      # path to controller config parameters on param server
+string[] dependencies       # optional, list names of all controllers this controller depends on
+---
+bool success
+string reason

src/dynamixel_motor/dynamixel_controllers/srv/StopController.srv

@@ -0,0 +1,4 @@
+string controller_name
+---
+bool success
+string reason

src/dynamixel_motor/dynamixel_controllers/srv/TorqueEnable.srv

@@ -0,0 +1,3 @@
+bool torque_enable
+---
+

src/dynamixel_motor/dynamixel_driver/CHANGELOG.rst

@@ -0,0 +1,34 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package dynamixel_driver
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.1 (2017-01-19)
+------------------
+* Don't set return delay time if value is invalid
+* Fix typo valie -> valid
+* Add EX-106, rename EX-106+ accordingly (Fixes `#60 <https://github.com/arebgun/dynamixel_motor/issues/60>`_)
+* Merge pull request `#49 <https://github.com/arebgun/dynamixel_motor/issues/49>`_ from anuragmakineni/master
+  remove Serial set/get functions from dynamixel_io
+* Update dynamixel_io.py
+  In set_p_gain, set_i_gain, set_d_gain, small spelling mistake. Instead of 'slope', it should be p/i/d_gain, since slope is not defined for the given function.
+* remove deprecated functions from dynamixel_io
+* bug fixes for issue `#33 <https://github.com/arebgun/dynamixel_motor/issues/33>`_ and warnnings on queue_size
+* Adds couple of methods for LED status fetching and changing.
+* Catch occurancies when error_code is parsed as float
+* added in a queue size as none to remove warning messages
+* Readback echo for simple one-wire (TTL) converters
+* Support for reading current, setting acceleration, torque control mode (mainly MX series). Available features specified for each model.
+* New model MX-12W
+* fix typo ;; min -> max
+* Fix bug in velocity conversion
+  The factor for the velocity conversion from raw to radsec and the other way
+  round is model-specific and does not depend on the maximum motor speed.
+  Therefore, a conversion factor is added to the motor data and employed
+  for all velocity conversions. The set motor velocity and the motor velocity
+  in the servo state does now correspond with the real servo speed in
+  radians per second.
+* Contributors: Andreas Wachaja, Antons Rebguns, Anurag Makineni, Gabrielius Mickevicius, Nicolas Alt, Russell Toris, Stefan Kohlbrecher, Yam Geva, Zilvinas, nozawa, parijat10, pazeshun
+
+0.4.0 (2013-07-26)
+------------------
+* stack is now catkin compatible

src/dynamixel_motor/dynamixel_driver/CMakeLists.txt

@@ -0,0 +1,19 @@
+# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamixel_driver)
+
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
+
+catkin_package(
+    CATKIN_DEPENDS rospy diagnostic_msgs
+)
+
+install(PROGRAMS
+  scripts/change_id.py
+  scripts/info_dump.py
+  scripts/set_servo_config.py
+  scripts/set_torque.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/dynamixel_motor/dynamixel_driver/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ax12_driver_core is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

src/dynamixel_motor/dynamixel_driver/package.xml

@@ -0,0 +1,32 @@
+<package>
+  <name>dynamixel_driver</name>
+  <version>0.4.1</version>
+  <description>
+    This package provides low level IO for Robotis Dynamixel servos.
+    Fully supports and was tested with AX-12, AX-18, RX-24, RX-28,
+    MX-28, RX-64, EX-106 models. Hardware specific constants are
+    defined for reading and writing information from/to Dynamixel
+    servos. This low level package won't be used directly by most
+    ROS users. The higher level dynamixel_controllers and specific
+    robot joint controllers make use of this package.
+  </description>
+  <maintainer email="arebgun@gmail.com">Antons Rebguns</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/dynamixel_driver</url>
+  <url type="bugtracker">https://github.com/arebgun/dynamixel_motor</url>
+
+  <author>Antons Rebguns</author>
+  <author>Cody Jorgensen</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>diagnostic_msgs</run_depend>
+  <run_depend>python-serial</run_depend>
+
+  <export>
+
+  </export>
+</package>

src/dynamixel_motor/dynamixel_driver/scripts/README

@@ -0,0 +1,9 @@
+This directory contains scripts that are useful when working with Dynamixel servos.
+
+Current Contents:
+
+    - set_torque.py -- use to turn motor torque on/off
+    - change_id.py -- use to change the id of a single motor
+    - set_servo_config.py -- use to change various servo parameters, like baud rate, return delay time, angle limits, etc.
+    - info_dump.py -- use to print out information for connected motors
+

src/dynamixel_motor/dynamixel_driver/scripts/change_id.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import sys
+from optparse import OptionParser
+
+import roslib
+roslib.load_manifest('dynamixel_driver')
+
+from dynamixel_driver import dynamixel_io
+
+if __name__ == '__main__':
+    parser = OptionParser(usage='Usage: %prog [options] OLD_ID NEW_ID', description='Changes the unique ID of a Dynamixel servo motor.')
+    parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
+                      help='motors of specified controllers are connected to PORT [default: %default]')
+    parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000,
+                      help='connection to serial port will be established at BAUD bps [default: %default]')
+                      
+    (options, args) = parser.parse_args(sys.argv)
+    
+    if len(args) < 3:
+        parser.print_help()
+        exit(1)
+        
+    port = options.port
+    baudrate = options.baud
+    old_id = int(args[1])
+    new_id = int(args[2])
+    
+    try:
+        dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
+    except dynamixel_io.SerialOpenError, soe:
+        print 'ERROR:', soe
+    else:
+        print 'Changing motor id from %d to %d...' %(old_id, new_id),
+        if dxl_io.ping(old_id):
+            dxl_io.set_id(old_id, new_id)
+            print ' done'
+            print 'Verifying new id...' ,
+            if dxl_io.ping(new_id):
+                print ' done'
+            else:
+                print 'ERROR: The motor did not respond to a ping to its new id.'
+        else:
+            print 'ERROR: The specified motor did not respond to id %d. Make sure to specify the correct baudrate.' % old_id

src/dynamixel_motor/dynamixel_driver/scripts/info_dump.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Cody Jorgensen, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Cody Jorgensen, Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Cody Jorgensen, Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import sys
+from optparse import OptionParser
+
+import roslib
+roslib.load_manifest('dynamixel_driver')
+
+from dynamixel_driver import dynamixel_io
+from dynamixel_driver.dynamixel_const import *
+
+def print_data(values):
+    ''' Takes a dictionary with all the motor values and does a formatted print.
+    '''
+    if values['freespin']:
+        print '''\
+    Motor %(id)d is connected:
+        Freespin: True
+        Model ------------------- %(model)s
+        Current Speed ----------- %(speed)d
+        Current Temperature ----- %(temperature)d%(degree_symbol)sC
+        Current Voltage --------- %(voltage).1fv
+        Current Load ------------ %(load)d
+        Moving ------------------ %(moving)s
+''' %values
+    else:
+        print '''\
+    Motor %(id)d is connected:
+        Freespin: False
+        Model ------------------- %(model)s
+        Min Angle --------------- %(min)d
+        Max Angle --------------- %(max)d
+        Current Position -------- %(position)d
+        Current Speed ----------- %(speed)d
+        Current Temperature ----- %(temperature)d%(degree_symbol)sC
+        Current Voltage --------- %(voltage).1fv
+        Current Load ------------ %(load)d
+        Moving ------------------ %(moving)s
+''' %values
+
+if __name__ == '__main__':
+    usage_msg = 'Usage: %prog [options] IDs'
+    desc_msg = 'Prints the current status of specified Dynamixel servo motors.'
+    epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 2 3 4 5' % sys.argv[0]
+    
+    parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
+    parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
+                      help='motors of specified controllers are connected to PORT [default: %default]')
+    parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000,
+                      help='connection to serial port will be established at BAUD bps [default: %default]')
+                      
+    (options, args) = parser.parse_args(sys.argv)
+    
+    if len(args) < 2:
+        parser.print_help()
+        exit(1)
+        
+    port = options.port
+    baudrate = options.baud
+    motor_ids = args[1:]
+    
+    try:
+        dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
+    except dynamixel_io.SerialOpenError, soe:
+        print 'ERROR:', soe
+    else:
+        responses = 0
+        print 'Pinging motors:'
+        for motor_id in motor_ids:
+            motor_id = int(motor_id)
+            print '%d ...' % motor_id,
+            p = dxl_io.ping(motor_id)
+            if p:
+                responses += 1
+                values = dxl_io.get_feedback(motor_id)
+                angles = dxl_io.get_angle_limits(motor_id)
+                model = dxl_io.get_model_number(motor_id)
+                firmware = dxl_io.get_firmware_version(motor_id)
+                values['model'] = '%s (firmware version: %d)' % (DXL_MODEL_TO_PARAMS[model]['name'], firmware)
+                values['degree_symbol'] = u"\u00B0"
+                values['min'] = angles['min']
+                values['max'] = angles['max']
+                values['voltage'] = values['voltage']
+                values['moving'] = str(values['moving'])
+                print 'done'
+                if angles['max'] == 0 and angles['min'] == 0:
+                    values['freespin'] = True
+                else:
+                    values['freespin'] = False
+                print_data(values)
+            else:
+                print 'error'
+        if responses == 0:
+            print 'ERROR: None of the specified motors responded. Make sure to specify the correct baudrate.'
+

src/dynamixel_motor/dynamixel_driver/scripts/set_servo_config.py

@@ -0,0 +1,144 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import sys
+from optparse import OptionParser
+
+import roslib
+roslib.load_manifest('dynamixel_driver')
+
+from dynamixel_driver import dynamixel_io
+
+if __name__ == '__main__':
+    usage_msg = 'Usage: %prog [options] MOTOR_IDs'
+    desc_msg = 'Sets various configuration options of specified Dynamixel servo motor.'
+    epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 --baud-rate=1 --return-delay=1 5 9 23' % sys.argv[0]
+    
+    parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
+    parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
+                      help='motors of specified controllers are connected to PORT [default: %default]')
+    parser.add_option('-b', '--baud', metavar='BAUD', type='int', default=1000000,
+                      help='connection to serial port will be established at BAUD bps [default: %default]')
+    parser.add_option('-r', '--baud-rate', type='int', metavar='RATE', dest='baud_rate',
+                      help='set servo motor communication speed')
+    parser.add_option('-d', '--return-delay', type='int', metavar='DELAY', dest='return_delay',
+                      help='set servo motor return packet delay time')
+    parser.add_option('--cw-angle-limit', type='int', metavar='CW_ANGLE', dest='cw_angle_limit',
+                      help='set servo motor CW angle limit')
+    parser.add_option('--ccw-angle-limit', type='int', metavar='CCW_ANGLE', dest='ccw_angle_limit',
+                      help='set servo motor CCW angle limit')
+    parser.add_option('--min-voltage-limit', type='int', metavar='MIN_VOLTAGE', dest='min_voltage_limit',
+                      help='set servo motor minimum voltage limit')
+    parser.add_option('--max-voltage-limit', type='int', metavar='MAX_VOLTAGE', dest='max_voltage_limit',
+                      help='set servo motor maximum voltage limit')
+                      
+    (options, args) = parser.parse_args(sys.argv)
+    print options
+    
+    if len(args) < 2:
+        parser.print_help()
+        exit(1)
+        
+    port = options.port
+    baudrate = options.baud
+    motor_ids = args[1:]
+    
+    try:
+        dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
+    except dynamixel_io.SerialOpenError, soe:
+        print 'ERROR:', soe
+    else:
+        for motor_id in motor_ids:
+            motor_id = int(motor_id)
+            print 'Configuring Dynamixel motor with ID %d' % motor_id
+            if dxl_io.ping(motor_id):
+                # check if baud rate needs to be changed
+                if options.baud_rate:
+                    valid_rates = (1,3,4,7,9,16,34,103,207,250,251,252)
+                    
+                    if options.baud_rate not in valid_rates:
+                        print 'Requested baud rate is invalid, please use one of the following: %s' % str(valid_rates)
+                        
+                    if options.baud_rate <= 207:
+                        print 'Setting baud rate to %d bps' % int(2000000.0/(options.baud_rate + 1))
+                    elif options.baud_rate == 250:
+                        print 'Setting baud rate to %d bps' % 2250000
+                    elif options.baud_rate == 251:
+                        print 'Setting baud rate to %d bps' % 2500000
+                    elif options.baud_rate == 252:
+                        print 'Setting baud rate to %d bps' % 3000000
+                        
+                    dxl_io.set_baud_rate(motor_id, options.baud_rate)
+                    
+                # check if return delay time needs to be changed
+                if options.return_delay is not None:
+                    if options.return_delay < 0 or options.return_delay > 254:
+                        print 'Requested return delay time is out of valid range (0 - 254)'
+                    else:
+                        print 'Setting return delay time to %d us' % (options.return_delay * 2)
+                        dxl_io.set_return_delay_time(motor_id, options.return_delay)
+                        
+                # check if CW angle limit needs to be changed
+                if options.cw_angle_limit is not None:
+                    print 'Setting CW angle limit to %d' % options.cw_angle_limit
+                    dxl_io.set_angle_limit_cw(motor_id, options.cw_angle_limit)
+                    
+                # check if CCW angle limit needs to be changed
+                if options.ccw_angle_limit is not None:
+                    print 'Setting CCW angle limit to %d' % options.ccw_angle_limit
+                    dxl_io.set_angle_limit_ccw(motor_id, options.ccw_angle_limit)
+                    
+                # check if minimum voltage limit needs to be changed
+                if options.min_voltage_limit:
+                    print 'Setting minimum voltage limit to %d' % options.min_voltage_limit
+                    dxl_io.set_voltage_limit_min(motor_id, options.min_voltage_limit)
+                    
+                # check if maximum voltage limit needs to be changed
+                if options.max_voltage_limit:
+                    print 'Setting maximum voltage limit to %d' % options.max_voltage_limit
+                    dxl_io.set_voltage_limit_max(motor_id, options.max_voltage_limit)
+                    
+                print 'done'
+            else:
+                print 'Unable to connect to Dynamixel motor with ID %d' % motor_id

src/dynamixel_motor/dynamixel_driver/scripts/set_torque.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import sys
+from optparse import OptionParser
+
+import roslib
+roslib.load_manifest('dynamixel_driver')
+
+from dynamixel_driver import dynamixel_io
+
+if __name__ == '__main__':
+    usage_msg = 'Usage: %prog [options] ID [On|Off]'
+    desc_msg = 'Turns the torque of specified Dynamixel servo motor on or off.'
+    epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 Off' % sys.argv[0]
+    
+    parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
+    parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
+                      help='motors of specified controllers are connected to PORT [default: %default]')
+    parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000,
+                      help='connection to serial port will be established at BAUD bps [default: %default]')
+                      
+    (options, args) = parser.parse_args(sys.argv)
+    
+    if len(args) < 3:
+        parser.print_help()
+        exit(1)
+        
+    port = options.port
+    baudrate = options.baud
+    motor_id = int(args[1])
+    torque_on = args[2]
+
+    try:
+        dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
+    except dynamixel_io.SerialOpenError, soe:
+        print 'ERROR:', soe
+    else:
+        print 'Turning torque %s for motor %d' % (torque_on, motor_id)
+        if dxl_io.ping(motor_id):
+            if torque_on.lower() == 'off':
+                torque_on = False
+            elif torque_on.lower() == 'on':
+                torque_on = True
+            else:
+                parser.print_help()
+                exit(1)
+            dxl_io.set_torque_enabled(motor_id, torque_on)
+            print 'done'
+        else:
+            print 'ERROR: motor %d did not respond. Make sure to specify the correct baudrate.' % motor_id
+

src/dynamixel_motor/dynamixel_driver/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['dynamixel_driver'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)

src/dynamixel_motor/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py

@@ -0,0 +1,287 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Cody Jorgensen, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Cody Jorgensen, Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Cody Jorgensen, Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+"""
+Dynamixel Constants
+"""
+# Control Table Constants
+DXL_MODEL_NUMBER_L = 0
+DXL_MODEL_NUMBER_H = 1
+DXL_VERSION = 2
+DXL_ID = 3
+DXL_BAUD_RATE = 4
+DXL_RETURN_DELAY_TIME = 5
+DXL_CW_ANGLE_LIMIT_L = 6
+DXL_CW_ANGLE_LIMIT_H = 7
+DXL_CCW_ANGLE_LIMIT_L = 8
+DXL_CCW_ANGLE_LIMIT_H = 9
+DXL_DRIVE_MODE = 10
+DXL_LIMIT_TEMPERATURE = 11
+DXL_DOWN_LIMIT_VOLTAGE = 12
+DXL_UP_LIMIT_VOLTAGE = 13
+DXL_MAX_TORQUE_L = 14
+DXL_MAX_TORQUE_H = 15
+DXL_RETURN_LEVEL = 16
+DXL_ALARM_LED = 17
+DXL_ALARM_SHUTDOWN = 18
+DXL_OPERATING_MODE = 19
+DXL_DOWN_CALIBRATION_L = 20
+DXL_DOWN_CALIBRATION_H = 21
+DXL_UP_CALIBRATION_L = 22
+DXL_UP_CALIBRATION_H = 23
+DXL_TORQUE_ENABLE = 24
+DXL_LED = 25
+DXL_CW_COMPLIANCE_MARGIN = 26
+DXL_CCW_COMPLIANCE_MARGIN = 27
+DXL_CW_COMPLIANCE_SLOPE = 28
+DXL_CCW_COMPLIANCE_SLOPE = 29
+DXL_D_GAIN = 26
+DXL_I_GAIN = 27
+DXL_P_GAIN = 28
+DXL_GOAL_POSITION_L = 30
+DXL_GOAL_POSITION_H = 31
+DXL_GOAL_SPEED_L = 32
+DXL_GOAL_SPEED_H = 33
+DXL_TORQUE_LIMIT_L = 34
+DXL_TORQUE_LIMIT_H = 35
+DXL_PRESENT_POSITION_L = 36
+DXL_PRESENT_POSITION_H = 37
+DXL_PRESENT_SPEED_L = 38
+DXL_PRESENT_SPEED_H = 39
+DXL_PRESENT_LOAD_L = 40
+DXL_PRESENT_LOAD_H = 41
+DXL_PRESENT_VOLTAGE = 42
+DXL_PRESENT_TEMPERATURE = 43
+DXL_REGISTERED_INSTRUCTION = 44
+DXL_PAUSE_TIME = 45
+DXL_MOVING = 46
+DXL_LOCK = 47
+DXL_PUNCH_L = 48
+DXL_PUNCH_H = 49
+DXL_SENSED_CURRENT_L = 56   # For EX-106
+DXL_SENSED_CURRENT_H = 57
+DXL_CURRENT_L = 68    # For MX-64 and up; different unit than EX-106
+DXL_CURRENT_H = 69
+DXL_TORQUE_CONTROL_MODE = 70
+DXL_GOAL_TORQUE_L = 71
+DXL_GOAL_TORQUE_H = 72
+DXL_GOAL_ACCELERATION = 73
+
+# Status Return Levels
+DXL_RETURN_NONE = 0
+DXL_RETURN_READ = 1
+DXL_RETURN_ALL = 2
+
+# Instruction Set
+DXL_PING = 1
+DXL_READ_DATA = 2
+DXL_WRITE_DATA = 3
+DXL_REG_WRITE = 4
+DXL_ACTION = 5
+DXL_RESET = 6
+DXL_SYNC_WRITE = 131
+
+# Broadcast Constant
+DXL_BROADCAST = 254
+
+# Error Codes
+DXL_INSTRUCTION_ERROR = 64
+DXL_OVERLOAD_ERROR = 32
+DXL_CHECKSUM_ERROR = 16
+DXL_RANGE_ERROR = 8
+DXL_OVERHEATING_ERROR = 4
+DXL_ANGLE_LIMIT_ERROR = 2
+DXL_INPUT_VOLTAGE_ERROR = 1
+DXL_NO_ERROR = 0
+
+# Static parameters
+DXL_MIN_COMPLIANCE_MARGIN = 0
+DXL_MAX_COMPLIANCE_MARGIN = 255
+
+DXL_MIN_COMPLIANCE_SLOPE = 1
+DXL_MAX_COMPLIANCE_SLOPE = 254
+
+# These are guesses as Dynamixel documentation doesn't have any info about it
+DXL_MIN_PUNCH = 0
+DXL_MAX_PUNCH = 255
+
+DXL_MAX_SPEED_TICK = 1023                   # maximum speed in encoder units
+DXL_MAX_TORQUE_TICK = 1023                  # maximum torque in encoder units
+
+KGCM_TO_NM = 0.0980665                      # 1 kg-cm is that many N-m
+RPM_TO_RADSEC = 0.104719755                 # 1 RPM is that many rad/sec
+
+# maximum holding torque is in N-m per volt
+# maximum velocity is in rad/sec per volt
+DXL_MODEL_TO_PARAMS = \
+{
+    113: { 'name':               'DX-113',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    1.0 / 12.0,                       #  1.0 NM @ 12V
+           'velocity_per_volt':  (54 * RPM_TO_RADSEC) / 12.0,      #  54 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+    116: { 'name':               'DX-116',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    2.1 / 12.0,                       #  2.1 NM @ 12V
+           'velocity_per_volt':  (78 * RPM_TO_RADSEC) / 12.0,      #  78 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+    117: { 'name':               'DX-117',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    3.7 / 18.5,                       #  3.7 NM @ 18.5V
+           'velocity_per_volt':  (85 * RPM_TO_RADSEC) / 18.5,      #  85 RPM @ 18.5V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+     12: { 'name':               'AX-12',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    1.5 / 12.0,                       #  1.5 NM @ 12V
+           'velocity_per_volt':  (59 * RPM_TO_RADSEC) / 12.0,      #  59 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+    300: { 'name':               'AX-12W',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    0.2 / 12.0,                       # 0.2 NM @ 12V
+           'velocity_per_volt':  (470 * RPM_TO_RADSEC) / 12.0,     # 470 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+     18: { 'name':               'AX-18',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    1.8 / 12.0,                       #  1.8 NM @ 12V
+           'velocity_per_volt':  (97 * RPM_TO_RADSEC) / 12.0,      #  97 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+     10: { 'name':               'RX-10',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    1.3 / 12.0,                       #  1.3 NM @ 12V
+           'velocity_per_volt':  (54 * RPM_TO_RADSEC) / 12.0,      #  54 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+     24: { 'name':               'RX-24',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    2.6 / 12.0,                       #  2.6 NM @ 12V
+           'velocity_per_volt':  (126 * RPM_TO_RADSEC) / 12.0,     # 126 RPM @ 12V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+     28: { 'name':               'RX-28',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    3.7 / 18.5,                       #  3.7 NM @ 18.5V
+           'velocity_per_volt':  (85 * RPM_TO_RADSEC) / 18.5,      #  85 RPM @ 18.5V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+     64: { 'name':               'RX-64',
+           'encoder_resolution': 1024,
+           'range_degrees':      300.0,
+           'torque_per_volt':    5.3 / 18.5,                       #  5.3 NM @ 18.5V
+           'velocity_per_volt':  (64 * RPM_TO_RADSEC) / 18.5,      #  64 RPM @ 18.5V
+           'rpm_per_tick':       0.111,
+           'features':           []
+         },
+    106: { 'name':               'EX-106',
+           'encoder_resolution': 4096,
+           'range_degrees':      250.92,
+           'torque_per_volt':    10.9 / 18.5,                      # 10.9 NM @ 18.5V
+           'velocity_per_volt':  (91 * RPM_TO_RADSEC) / 18.5,      #  91 RPM @ 18.5V
+           'rpm_per_tick':       0.111,
+           'features':           [DXL_SENSED_CURRENT_L]
+         },     
+    107: { 'name':               'EX-106+',
+           'encoder_resolution': 4096,
+           'range_degrees':      250.92,
+           'torque_per_volt':    10.9 / 18.5,                      # 10.9 NM @ 18.5V
+           'velocity_per_volt':  (91 * RPM_TO_RADSEC) / 18.5,      #  91 RPM @ 18.5V
+           'rpm_per_tick':       0.111,
+           'features':           [DXL_SENSED_CURRENT_L]
+         },
+    360: { 'name':               'MX-12W',
+           'encoder_resolution': 4096,
+           'range_degrees':      360.0,
+           'torque_per_volt':    0.2 / 12.0,                        #  torque not specified!
+           'velocity_per_volt':  (470 * RPM_TO_RADSEC) / 12.0,      #  470 RPM @ 12.0V
+           'rpm_per_tick':       0.114,
+           'features':           [DXL_GOAL_ACCELERATION]
+         },
+     29: { 'name':               'MX-28',
+           'encoder_resolution': 4096,
+           'range_degrees':      360.0,
+           'torque_per_volt':    2.5 / 12.0,                       #  2.5 NM @ 12V
+           'velocity_per_volt':  (55 * RPM_TO_RADSEC) / 12.0,      #  54 RPM @ 12.0V
+           'rpm_per_tick':       0.114,
+           'features':           [DXL_GOAL_ACCELERATION]
+         },
+    310: { 'name':               'MX-64',
+           'encoder_resolution': 4096,
+           'range_degrees':      360.0,
+           'torque_per_volt':    6.0 / 12.0,                       #  6 NM @ 12V
+           'velocity_per_volt':  (63 * RPM_TO_RADSEC) / 12.0,      #  63 RPM @ 12.0V
+           'rpm_per_tick':       0.114,
+           'features':           [DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION]
+         },
+    320: { 'name':               'MX-106',
+           'encoder_resolution': 4096,
+           'range_degrees':      360.0,
+           'torque_per_volt':    8.4 / 12.0,                       #  8.4 NM @ 12V
+           'velocity_per_volt':  (45 * RPM_TO_RADSEC) / 12.0,      #  45 RPM @ 12.0V
+           'rpm_per_tick':       0.114,
+           'features':           [DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION]
+         },
+}
+

src/dynamixel_motor/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py

@@ -0,0 +1,1067 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Cody Jorgensen, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Cody Jorgensen, Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Cody Jorgensen, Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import time
+import serial
+from array import array
+from binascii import b2a_hex
+from threading import Lock
+
+from dynamixel_const import *
+
+exception = None
+
+class DynamixelIO(object):
+    """ Provides low level IO with the Dynamixel servos through pyserial. Has the
+    ability to write instruction packets, request and read register value
+    packets, send and receive a response to a ping packet, and send a SYNC WRITE
+    multi-servo instruction packet.
+    """
+
+    def __init__(self, port, baudrate, readback_echo=False):
+        """ Constructor takes serial port and baudrate as arguments. """
+        try:
+            self.serial_mutex = Lock()
+            self.ser = None
+            self.ser = serial.Serial(port, baudrate, timeout=0.015)
+            self.port_name = port
+            self.readback_echo = readback_echo
+        except SerialOpenError:
+           raise SerialOpenError(port, baudrate)
+
+    def __del__(self):
+        """ Destructor calls DynamixelIO.close """
+        self.close()
+
+    def close(self):
+        """
+        Be nice, close the serial port.
+        """
+        if self.ser:
+            self.ser.flushInput()
+            self.ser.flushOutput()
+            self.ser.close()
+
+    def __write_serial(self, data):
+        self.ser.flushInput()
+        self.ser.flushOutput()
+        self.ser.write(data)
+        if self.readback_echo:
+            self.ser.read(len(data))
+
+    def __read_response(self, servo_id):
+        data = []
+
+        try:
+            data.extend(self.ser.read(4))
+            if not data[0:2] == ['\xff', '\xff']: raise Exception('Wrong packet prefix %s' % data[0:2])
+            data.extend(self.ser.read(ord(data[3])))
+            data = array('B', ''.join(data)).tolist() # [int(b2a_hex(byte), 16) for byte in data]
+        except Exception, e:
+            raise DroppedPacketError('Invalid response received from motor %d. %s' % (servo_id, e))
+
+        # verify checksum
+        checksum = 255 - sum(data[2:-1]) % 256
+        if not checksum == data[-1]: raise ChecksumError(servo_id, data, checksum)
+
+        return data
+
+    def read(self, servo_id, address, size):
+        """ Read "size" bytes of data from servo with "servo_id" starting at the
+        register with "address". "address" is an integer between 0 and 57. It is
+        recommended to use the constants in module dynamixel_const for readability.
+
+        To read the position from servo with id 1, the method should be called
+        like:
+            read(1, DXL_GOAL_POSITION_L, 2)
+        """
+        # Number of bytes following standard header (0xFF, 0xFF, id, length)
+        length = 4  # instruction, address, size, checksum
+
+        # directly from AX-12 manual:
+        # Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
+        # If the calculated value is > 255, the lower byte is the check sum.
+        checksum = 255 - ( (servo_id + length + DXL_READ_DATA + address + size) % 256 )
+
+        # packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
+        packet = [0xFF, 0xFF, servo_id, length, DXL_READ_DATA, address, size, checksum]
+        packetStr = array('B', packet).tostring() # same as: packetStr = ''.join([chr(byte) for byte in packet])
+
+        with self.serial_mutex:
+            self.__write_serial(packetStr)
+
+            # wait for response packet from the motor
+            timestamp = time.time()
+            time.sleep(0.0013)#0.00235)
+
+            # read response
+            data = self.__read_response(servo_id)
+            data.append(timestamp)
+
+        return data
+
+    def write(self, servo_id, address, data):
+        """ Write the values from the "data" list to the servo with "servo_id"
+        starting with data[0] at "address", continuing through data[n-1] at
+        "address" + (n-1), where n = len(data). "address" is an integer between
+        0 and 49. It is recommended to use the constants in module dynamixel_const
+        for readability. "data" is a list/tuple of integers.
+
+        To set servo with id 1 to position 276, the method should be called
+        like:
+            write(1, DXL_GOAL_POSITION_L, (20, 1))
+        """
+        # Number of bytes following standard header (0xFF, 0xFF, id, length)
+        length = 3 + len(data)  # instruction, address, len(data), checksum
+
+        # directly from AX-12 manual:
+        # Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
+        # If the calculated value is > 255, the lower byte is the check sum.
+        checksum = 255 - ((servo_id + length + DXL_WRITE_DATA + address + sum(data)) % 256)
+
+        # packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
+        packet = [0xFF, 0xFF, servo_id, length, DXL_WRITE_DATA, address]
+        packet.extend(data)
+        packet.append(checksum)
+
+        packetStr = array('B', packet).tostring() # packetStr = ''.join([chr(byte) for byte in packet])
+
+        with self.serial_mutex:
+            self.__write_serial(packetStr)
+
+            # wait for response packet from the motor
+            timestamp = time.time()
+            time.sleep(0.0013)
+
+            # read response
+            data = self.__read_response(servo_id)
+            data.append(timestamp)
+
+        return data
+
+    def sync_write(self, address, data):
+        """ Use Broadcast message to send multiple servos instructions at the
+        same time. No "status packet" will be returned from any servos.
+        "address" is an integer between 0 and 49. It is recommended to use the
+        constants in module dynamixel_const for readability. "data" is a tuple of
+        tuples. Each tuple in "data" must contain the servo id followed by the
+        data that should be written from the starting address. The amount of
+        data can be as long as needed.
+
+        To set servo with id 1 to position 276 and servo with id 2 to position
+        550, the method should be called like:
+            sync_write(DXL_GOAL_POSITION_L, ( (1, 20, 1), (2 ,38, 2) ))
+        """
+        # Calculate length and sum of all data
+        flattened = [value for servo in data for value in servo]
+
+        # Number of bytes following standard header (0xFF, 0xFF, id, length) plus data
+        length = 4 + len(flattened)
+
+        checksum = 255 - ((DXL_BROADCAST + length + \
+                          DXL_SYNC_WRITE + address + len(data[0][1:]) + \
+                          sum(flattened)) % 256)
+
+        # packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
+        packet = [0xFF, 0xFF, DXL_BROADCAST, length, DXL_SYNC_WRITE, address, len(data[0][1:])]
+        packet.extend(flattened)
+        packet.append(checksum)
+
+        packetStr = array('B', packet).tostring() # packetStr = ''.join([chr(byte) for byte in packet])
+
+        with self.serial_mutex:
+            self.__write_serial(packetStr)
+
+    def ping(self, servo_id):
+        """ Ping the servo with "servo_id". This causes the servo to return a
+        "status packet". This can tell us if the servo is attached and powered,
+        and if so, if there are any errors.
+        """
+        # Number of bytes following standard header (0xFF, 0xFF, id, length)
+        length = 2  # instruction, checksum
+
+        # directly from AX-12 manual:
+        # Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
+        # If the calculated value is > 255, the lower byte is the check sum.
+        checksum = 255 - ((servo_id + length + DXL_PING) % 256)
+
+        # packet: FF  FF  ID LENGTH INSTRUCTION CHECKSUM
+        packet = [0xFF, 0xFF, servo_id, length, DXL_PING, checksum]
+        packetStr = array('B', packet).tostring()
+
+        with self.serial_mutex:
+            self.__write_serial(packetStr)
+
+            # wait for response packet from the motor
+            timestamp = time.time()
+            time.sleep(0.0013)
+
+            # read response
+            try:
+                response = self.__read_response(servo_id)
+                response.append(timestamp)
+            except Exception, e:
+                response = []
+
+        if response:
+            self.exception_on_error(response[4], servo_id, 'ping')
+        return response
+
+    def test_bit(self, number, offset):
+        mask = 1 << offset
+        return (number & mask)
+
+
+    ######################################################################
+    # These function modify EEPROM data which persists after power cycle #
+    ######################################################################
+
+    def set_id(self, old_id, new_id):
+        """
+        Sets a new unique number to identify a motor. The range from 1 to 253
+        (0xFD) can be used.
+        """
+        response = self.write(old_id, DXL_ID, [new_id])
+        if response:
+            self.exception_on_error(response[4], old_id, 'setting id to %d' % new_id)
+        return response
+
+    def set_baud_rate(self, servo_id, baud_rate):
+        """
+        Sets servo communication speed. The range from 0 to 254.
+        """
+        response = self.write(servo_id, DXL_BAUD_RATE, [baud_rate])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting baud rate to %d' % baud_rate)
+        return response
+
+    def set_return_delay_time(self, servo_id, delay):
+        """
+        Sets the delay time from the transmission of Instruction Packet until
+        the return of Status Packet. 0 to 254 (0xFE) can be used, and the delay
+        time per data value is 2 usec.
+        """
+        response = self.write(servo_id, DXL_RETURN_DELAY_TIME, [delay])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting return delay time to %d' % delay)
+        return response
+
+    def set_angle_limit_cw(self, servo_id, angle_cw):
+        """
+        Set the min (CW) angle of rotation limit.
+        """
+        loVal = int(angle_cw % 256)
+        hiVal = int(angle_cw >> 8)
+
+        response = self.write(servo_id, DXL_CW_ANGLE_LIMIT_L, (loVal, hiVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CW angle limits to %d' % angle_cw)
+        return response
+
+    def set_angle_limit_ccw(self, servo_id, angle_ccw):
+        """
+        Set the max (CCW) angle of rotation limit.
+        """
+        loVal = int(angle_ccw % 256)
+        hiVal = int(angle_ccw >> 8)
+
+        response = self.write(servo_id, DXL_CCW_ANGLE_LIMIT_L, (loVal, hiVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CCW angle limits to %d' % angle_ccw)
+        return response
+
+    def set_angle_limits(self, servo_id, min_angle, max_angle):
+        """
+        Set the min (CW) and max (CCW) angle of rotation limits.
+        """
+        loMinVal = int(min_angle % 256)
+        hiMinVal = int(min_angle >> 8)
+        loMaxVal = int(max_angle % 256)
+        hiMaxVal = int(max_angle >> 8)
+
+        # set 4 register values with low and high bytes for min and max angles
+        response = self.write(servo_id, DXL_CW_ANGLE_LIMIT_L, (loMinVal, hiMinVal, loMaxVal, hiMaxVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CW and CCW angle limits to %d and %d' %(min_angle, max_angle))
+        return response
+
+    def set_drive_mode(self, servo_id, is_slave=False, is_reverse=False):
+        """
+        Sets the drive mode for EX-106 motors
+        """
+        drive_mode = (is_slave << 1) + is_reverse
+
+        response = self.write(servo_id, DXL_DRIVE_MODE, [drive_mode])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting drive mode to %d' % drive_mode)
+        return response
+
+    def set_voltage_limit_min(self, servo_id, min_voltage):
+        """
+        Set the minimum voltage limit.
+        NOTE: the absolute min is 5v
+        """
+
+        if min_voltage < 5: min_voltage = 5
+        minVal = int(min_voltage * 10)
+
+        response = self.write(servo_id, DXL_DOWN_LIMIT_VOLTAGE, [minVal])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting minimum voltage level to %d' % min_voltage)
+        return response
+
+    def set_voltage_limit_max(self, servo_id, max_voltage):
+        """
+        Set the maximum voltage limit.
+        NOTE: the absolute min is 25v
+        """
+
+        if max_voltage > 25: max_voltage = 25
+        maxVal = int(max_voltage * 10)
+
+        response = self.write(servo_id, DXL_UP_LIMIT_VOLTAGE, [maxVal])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting maximum voltage level to %d' % max_voltage)
+        return response
+
+    def set_voltage_limits(self, servo_id, min_voltage, max_voltage):
+        """
+        Set the min and max voltage limits.
+        NOTE: the absolute min is 5v and the absolute max is 25v
+        """
+
+        if min_voltage < 5: min_voltage = 5
+        if max_voltage > 25: max_voltage = 25
+
+        minVal = int(min_voltage * 10)
+        maxVal = int(max_voltage * 10)
+
+        response = self.write(servo_id, DXL_DOWN_LIMIT_VOLTAGE, (minVal, maxVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting min and max voltage levels to %d and %d' %(min_voltage, max_voltage))
+        return response
+
+
+    ###############################################################
+    # These functions can send a single command to a single servo #
+    ###############################################################
+
+    def set_torque_enabled(self, servo_id, enabled):
+        """
+        Sets the value of the torque enabled register to 1 or 0. When the
+        torque is disabled the servo can be moved manually while the motor is
+        still powered.
+        """
+        response = self.write(servo_id, DXL_TORQUE_ENABLE, [enabled])
+        if response:
+            self.exception_on_error(response[4], servo_id, '%sabling torque' % 'en' if enabled else 'dis')
+        return response
+
+    def set_compliance_margin_cw(self, servo_id, margin):
+        """
+        The error between goal position and present position in CW direction.
+        The range of the value is 0 to 255, and the unit is the same as Goal Position.
+        The greater the value, the more difference occurs.
+        """
+        response = self.write(servo_id, DXL_CW_COMPLIANCE_MARGIN, [margin])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CW compliance margin to %d' % margin)
+        return response
+
+    def set_compliance_margin_ccw(self, servo_id, margin):
+        """
+        The error between goal position and present position in CCW direction.
+        The range of the value is 0 to 255, and the unit is the same as Goal Position.
+        The greater the value, the more difference occurs.
+        """
+        response = self.write(servo_id, DXL_CCW_COMPLIANCE_MARGIN, [margin])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CCW compliance margin to %d' % margin)
+        return response
+
+    def set_compliance_margins(self, servo_id, margin_cw, margin_ccw):
+        """
+        The error between goal position and present position in CCW direction.
+        The range of the value is 0 to 255, and the unit is the same as Goal Position.
+        The greater the value, the more difference occurs.
+        """
+        response = self.write(servo_id, DXL_CW_COMPLIANCE_MARGIN, (margin_cw, margin_ccw))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CW and CCW compliance margins to values %d and %d' %(margin_cw, margin_ccw))
+        return response
+
+    def set_compliance_slope_cw(self, servo_id, slope):
+        """
+        Sets the level of Torque near the goal position in CW direction.
+        Compliance Slope is set in 7 steps, the higher the value, the more flexibility is obtained.
+        """
+        response = self.write(servo_id, DXL_CW_COMPLIANCE_SLOPE, [slope])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CW compliance slope to %d' %  slope)
+        return response
+
+    def set_compliance_slope_ccw(self, servo_id, slope):
+        """
+        Sets the level of Torque near the goal position in CCW direction.
+        Compliance Slope is set in 7 steps, the higher the value, the more flexibility is obtained.
+        """
+        response = self.write(servo_id, DXL_CCW_COMPLIANCE_SLOPE, [slope])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CCW compliance slope to %d' % slope)
+        return response
+
+    def set_compliance_slopes(self, servo_id, slope_cw, slope_ccw):
+        """
+        Sets the level of Torque near the goal position in CW/CCW direction.
+        Compliance Slope is set in 7 steps, the higher the value, the more flexibility is obtained.
+        """
+        response = self.write(servo_id, DXL_CW_COMPLIANCE_SLOPE, (slope_cw, slope_ccw))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting CW and CCW compliance slopes to %d and %d' %(slope_cw, slope_ccw))
+        return response
+
+    def set_d_gain(self, servo_id, d_gain):
+        """
+        Sets the value of derivative action of PID controller.
+        Gain value is in range 0 to 254.
+        """
+        response = self.write(servo_id, DXL_D_GAIN, [d_gain])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting D gain value of PID controller to %d' % d_gain)
+        return response
+
+    def set_i_gain(self, servo_id, i_gain):
+        """
+        Sets the value of integral action of PID controller.
+        Gain value is in range 0 to 254.
+        """
+        response = self.write(servo_id, DXL_I_GAIN, [i_gain])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting I gain value of PID controller to %d' % i_gain)
+        return response
+
+    def set_p_gain(self, servo_id, p_gain):
+        """
+        Sets the value of proportional action of PID controller.
+        Gain value is in range 0 to 254.
+        """
+        response = self.write(servo_id, DXL_P_GAIN, [p_gain])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting P gain value of PID controller to %d' % p_gain)
+        return response
+
+    def set_punch(self, servo_id, punch):
+        """
+        Sets the limit value of torque being reduced when the output torque is
+        decreased in the Compliance Slope area. In other words, it is the mimimum
+        torque. The initial value is 32 (0x20) and can be extended up to 1023
+        (0x3FF). (Refer to Compliance margin & Slope)
+        """
+        loVal = int(punch % 256)
+        hiVal = int(punch >> 8)
+        response = self.write(servo_id, DXL_PUNCH_L, (loVal, hiVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting punch to %d' % punch)
+        return response
+
+    def set_acceleration(self, servo_id, acceleration):
+        """
+        Sets the acceleration. The unit is 8.583 Degree / sec^2.
+        0 - acceleration control disabled, 1-254 - valid range for acceleration.
+        """
+
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_GOAL_ACCELERATION)
+
+        if DXL_GOAL_ACCELERATION in DXL_MODEL_TO_PARAMS[model]['features']:
+            response = self.write(servo_id, DXL_GOAL_ACCELERATION, (acceleration, ))
+            if response:
+                self.exception_on_error(response[4], servo_id, 'setting acceleration to %d' % acceleration)
+            return response
+        else:
+            raise UnsupportedFeatureError(model, DXL_GOAL_ACCELERATION)
+
+    def set_position(self, servo_id, position):
+        """
+        Set the servo with servo_id to the specified goal position.
+        Position value must be positive.
+        """
+        loVal = int(position % 256)
+        hiVal = int(position >> 8)
+
+        response = self.write(servo_id, DXL_GOAL_POSITION_L, (loVal, hiVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting goal position to %d' % position)
+        return response
+
+    def set_speed(self, servo_id, speed):
+        """
+        Set the servo with servo_id to the specified goal speed.
+        Speed can be negative only if the dynamixel is in "freespin" mode.
+        """
+        # split speed into 2 bytes
+        if speed >= 0:
+            loVal = int(speed % 256)
+            hiVal = int(speed >> 8)
+        else:
+            loVal = int((1023 - speed) % 256)
+            hiVal = int((1023 - speed) >> 8)
+
+        # set two register values with low and high byte for the speed
+        response = self.write(servo_id, DXL_GOAL_SPEED_L, (loVal, hiVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting moving speed to %d' % speed)
+        return response
+
+    def set_torque_limit(self, servo_id, torque):
+        """
+        Sets the value of the maximum torque limit for servo with id servo_id.
+        Valid values are 0 to 1023 (0x3FF), and the unit is about 0.1%.
+        For example, if the value is 512 only 50% of the maximum torque will be used.
+        If the power is turned on, the value of Max Torque (Address 14, 15) is used as the initial value.
+        """
+        loVal = int(torque % 256)
+        hiVal = int(torque >> 8)
+
+        response = self.write(servo_id, DXL_TORQUE_LIMIT_L, (loVal, hiVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting torque limit to %d' % torque)
+        return response
+
+    def set_goal_torque(self, servo_id, torque):
+        """
+        Set the servo to torque control mode (similar to wheel mode, but controlling the torque)
+        Valid values are from -1023 to 1023.
+        Anything outside this range or 'None' disables torque control.
+        """
+
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_TORQUE_CONTROL_MODE)
+
+        valid_torque = torque is not None and torque >= -1023 and torque <= 1023
+        if torque is not None and torque < 0:
+            torque = 1024 - torque
+
+        if DXL_TORQUE_CONTROL_MODE in DXL_MODEL_TO_PARAMS[model]['features']:
+            if valid_torque:
+                loVal = int(torque % 256); hiVal = int(torque >> 8);
+                response = self.write(servo_id, DXL_GOAL_TORQUE_L, (loVal, hiVal))
+                if response:
+                    self.exception_on_error(response[4], servo_id, 'setting goal torque to %d' % torque)
+            response = self.write(servo_id, DXL_TORQUE_CONTROL_MODE, (int(valid_torque), ))
+            if response:
+                self.exception_on_error(response[4], servo_id, 'enabling torque mode')
+            return response
+        else:
+            raise UnsupportedFeatureError(model, DXL_TORQUE_CONTROL_MODE)
+
+    def set_position_and_speed(self, servo_id, position, speed):
+        """
+        Set the servo with servo_id to specified position and speed.
+        Speed can be negative only if the dynamixel is in "freespin" mode.
+        """
+        # split speed into 2 bytes
+        if speed >= 0:
+            loSpeedVal = int(speed % 256)
+            hiSpeedVal = int(speed >> 8)
+        else:
+            loSpeedVal = int((1023 - speed) % 256)
+            hiSpeedVal = int((1023 - speed) >> 8)
+
+        # split position into 2 bytes
+        loPositionVal = int(position % 256)
+        hiPositionVal = int(position >> 8)
+
+        response = self.write(servo_id, DXL_GOAL_POSITION_L, (loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting goal position to %d and moving speed to %d' %(position, speed))
+        return response
+
+    def set_led(self, servo_id, led_state):
+        """
+        Turn the LED of servo motor on/off.
+        Possible boolean state values:
+            True - turn the LED on,
+            False - turn the LED off.
+        """
+        response = self.write(servo_id, DXL_LED, [led_state])
+        if response:
+            self.exception_on_error(response[4], servo_id,
+                    'setting a LED to %s' % led_state)
+        return response
+
+
+    #################################################################
+    # These functions can send multiple commands to multiple servos #
+    # These commands are used in ROS wrapper as they don't send a   #
+    # response packet, ROS wrapper gets motor states at a set rate  #
+    #################################################################
+
+    def set_multi_torque_enabled(self, valueTuples):
+        """
+        Method to set multiple servos torque enabled.
+        Should be called as such:
+        set_multi_servos_to_torque_enabled( (id1, True), (id2, True), (id3, True) )
+        """
+        # use sync write to broadcast multi servo message
+        self.sync_write(DXL_TORQUE_ENABLE, tuple(valueTuples))
+
+    def set_multi_compliance_margin_cw(self, valueTuples):
+        """
+        Set different CW compliance margin for multiple servos.
+        Should be called as such:
+        set_multi_compliance_margin_cw( ( (id1, margin1), (id2, margin2), (id3, margin3) ) )
+        """
+        self.sync_write(DXL_CW_COMPLIANCE_MARGIN, tuple(valueTuples))
+
+    def set_multi_compliance_margin_ccw(self, valueTuples):
+        """
+        Set different CCW compliance margin for multiple servos.
+        Should be called as such:
+        set_multi_compliance_margin_ccw( ( (id1, margin1), (id2, margin2), (id3, margin3) ) )
+        """
+        self.sync_write(DXL_CCW_COMPLIANCE_MARGIN, tuple(valueTuples))
+
+    def set_multi_compliance_margins(self, valueTuples):
+        """
+        Set different CW and CCW compliance margins for multiple servos.
+        Should be called as such:
+        set_multi_compliance_margins( ( (id1, cw_margin1, ccw_margin1), (id2, cw_margin2, ccw_margin2) ) )
+        """
+        self.sync_write(DXL_CW_COMPLIANCE_MARGIN, tuple(valueTuples))
+
+    def set_multi_compliance_slope_cw(self, valueTuples):
+        """
+        Set different CW compliance slope for multiple servos.
+        Should be called as such:
+        set_multi_compliance_slope_cw( ( (id1, slope1), (id2, slope2), (id3, slope3) ) )
+        """
+        self.sync_write(DXL_CW_COMPLIANCE_SLOPE, tuple(valueTuples))
+
+    def set_multi_compliance_slope_ccw(self, valueTuples):
+        """
+        Set different CCW compliance slope for multiple servos.
+        Should be called as such:
+        set_multi_compliance_slope_ccw( ( (id1, slope1), (id2, slope2), (id3, slope3) ) )
+        """
+        self.sync_write(DXL_CCW_COMPLIANCE_SLOPE, tuple(valueTuples))
+
+    def set_multi_compliance_slopes(self, valueTuples):
+        """
+        Set different CW and CCW compliance slopes for multiple servos.
+        Should be called as such:
+        set_multi_compliance_slopes( ( (id1, cw_slope1, ccw_slope1), (id2, cw_slope2, ccw_slope2) ) )
+        """
+        self.sync_write(DXL_CW_COMPLIANCE_SLOPE, tuple(valueTuples))
+
+    def set_multi_punch(self, valueTuples):
+        """
+        Set different punch values for multiple servos.
+        NOTE: according to documentation, currently this value is not being used.
+        Should be called as such:
+        set_multi_punch( ( (id1, punch1), (id2, punch2), (id3, punch3) ) )
+        """
+        # prepare value tuples for call to syncwrite
+        writeableVals = []
+
+        for sid,punch in valueTuples:
+            # split punch into 2 bytes
+            loVal = int(punch % 256)
+            hiVal = int(punch >> 8)
+            writeableVals.append( (sid, loVal, hiVal) )
+
+        # use sync write to broadcast multi servo message
+        self.sync_write(DXL_PUNCH_L, writeableVals)
+
+    def set_multi_position(self, valueTuples):
+        """
+        Set different positions for multiple servos.
+        Should be called as such:
+        set_multi_position( ( (id1, position1), (id2, position2), (id3, position3) ) )
+        """
+        # prepare value tuples for call to syncwrite
+        writeableVals = []
+
+        for vals in valueTuples:
+            sid = vals[0]
+            position = vals[1]
+            # split position into 2 bytes
+            loVal = int(position % 256)
+            hiVal = int(position >> 8)
+            writeableVals.append( (sid, loVal, hiVal) )
+
+        # use sync write to broadcast multi servo message
+        self.sync_write(DXL_GOAL_POSITION_L, writeableVals)
+
+    def set_multi_speed(self, valueTuples):
+        """
+        Set different speeds for multiple servos.
+        Should be called as such:
+        set_multi_speed( ( (id1, speed1), (id2, speed2), (id3, speed3) ) )
+        """
+        # prepare value tuples for call to syncwrite
+        writeableVals = []
+
+        for vals in valueTuples:
+            sid = vals[0]
+            speed = vals[1]
+
+            # split speed into 2 bytes
+            if speed >= 0:
+                loVal = int(speed % 256)
+                hiVal = int(speed >> 8)
+            else:
+                loVal = int((1023 - speed) % 256)
+                hiVal = int((1023 - speed) >> 8)
+
+            writeableVals.append( (sid, loVal, hiVal) )
+
+        # use sync write to broadcast multi servo message
+        self.sync_write(DXL_GOAL_SPEED_L, writeableVals)
+
+    def set_multi_torque_limit(self, valueTuples):
+        """
+        Set different torque limits for multiple servos.
+        Should be called as such:
+        set_multi_torque_limit( ( (id1, torque1), (id2, torque2), (id3, torque3) ) )
+        """
+        # prepare value tuples for call to syncwrite
+        writeableVals = []
+
+        for sid,torque in valueTuples:
+            # split torque into 2 bytes
+            loVal = int(torque % 256)
+            hiVal = int(torque >> 8)
+            writeableVals.append( (sid, loVal, hiVal) )
+
+        # use sync write to broadcast multi servo message
+        self.sync_write(DXL_TORQUE_LIMIT_L, writeableVals)
+
+    def set_multi_position_and_speed(self, valueTuples):
+        """
+        Set different positions and speeds for multiple servos.
+        Should be called as such:
+        set_multi_position_and_speed( ( (id1, position1, speed1), (id2, position2, speed2), (id3, position3, speed3) ) )
+        """
+        # prepare value tuples for call to syncwrite
+        writeableVals = []
+
+        for vals in valueTuples:
+            sid = vals[0]
+            position = vals[1]
+            speed = vals[2]
+
+            # split speed into 2 bytes
+            if speed >= 0:
+                loSpeedVal = int(speed % 256)
+                hiSpeedVal = int(speed >> 8)
+            else:
+                loSpeedVal = int((1023 - speed) % 256)
+                hiSpeedVal = int((1023 - speed) >> 8)
+
+            # split position into 2 bytes
+            loPositionVal = int(position % 256)
+            hiPositionVal = int(position >> 8)
+            writeableVals.append( (sid, loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal) )
+
+        # use sync write to broadcast multi servo message
+        self.sync_write(DXL_GOAL_POSITION_L, tuple(writeableVals))
+
+
+    #################################
+    # Servo status access functions #
+    #################################
+
+    def get_model_number(self, servo_id):
+        """ Reads the servo's model number (e.g. 12 for AX-12+). """
+        response = self.read(servo_id, DXL_MODEL_NUMBER_L, 2)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching model number')
+        return response[5] + (response[6] << 8)
+
+    def get_firmware_version(self, servo_id):
+        """ Reads the servo's firmware version. """
+        response = self.read(servo_id, DXL_VERSION, 1)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching firmware version')
+        return response[5]
+
+    def get_return_delay_time(self, servo_id):
+        """ Reads the servo's return delay time. """
+        response = self.read(servo_id, DXL_RETURN_DELAY_TIME, 1)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching return delay time')
+        return response[5]
+
+    def get_angle_limits(self, servo_id):
+        """
+        Returns the min and max angle limits from the specified servo.
+        """
+        # read in 4 consecutive bytes starting with low value of clockwise angle limit
+        response = self.read(servo_id, DXL_CW_ANGLE_LIMIT_L, 4)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching CW/CCW angle limits')
+        # extract data valus from the raw data
+        cwLimit = response[5] + (response[6] << 8)
+        ccwLimit = response[7] + (response[8] << 8)
+
+        # return the data in a dictionary
+        return {'min':cwLimit, 'max':ccwLimit}
+
+    def get_drive_mode(self, servo_id):
+        """ Reads the servo's drive mode. """
+        response = self.read(servo_id, DXL_DRIVE_MODE, 1)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching drive mode')
+        return response[5]
+
+    def get_voltage_limits(self, servo_id):
+        """
+        Returns the min and max voltage limits from the specified servo.
+        """
+        response = self.read(servo_id, DXL_DOWN_LIMIT_VOLTAGE, 2)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching voltage limits')
+        # extract data valus from the raw data
+        min_voltage = response[5] / 10.0
+        max_voltage = response[6] / 10.0
+
+        # return the data in a dictionary
+        return {'min':min_voltage, 'max':max_voltage}
+
+    def get_position(self, servo_id):
+        """ Reads the servo's position value from its registers. """
+        response = self.read(servo_id, DXL_PRESENT_POSITION_L, 2)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching present position')
+        position = response[5] + (response[6] << 8)
+        return position
+
+    def get_speed(self, servo_id):
+        """ Reads the servo's speed value from its registers. """
+        response = self.read(servo_id, DXL_PRESENT_SPEED_L, 2)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching present speed')
+        speed = response[5] + (response[6] << 8)
+        if speed > 1023:
+            return 1023 - speed
+        return speed
+
+    def get_voltage(self, servo_id):
+        """ Reads the servo's voltage. """
+        response = self.read(servo_id, DXL_PRESENT_VOLTAGE, 1)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching supplied voltage')
+        return response[5] / 10.0
+
+    def get_current(self, servo_id):
+        """ Reads the servo's current consumption (if supported by model) """
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_CURRENT_L)
+
+        if DXL_CURRENT_L in DXL_MODEL_TO_PARAMS[model]['features']:
+            response = self.read(servo_id, DXL_CURRENT_L, 2)
+            if response:
+                self.exception_on_error(response[4], servo_id, 'fetching sensed current')
+            current = response[5] + (response[6] << 8)
+            return 0.0045 * (current - 2048)
+
+        if DXL_SENSED_CURRENT_L in DXL_MODEL_TO_PARAMS[model]['features']:
+            response = self.read(servo_id, DXL_SENSED_CURRENT_L, 2)
+            if response:
+                self.exception_on_error(response[4], servo_id, 'fetching sensed current')
+            current = response[5] + (response[6] << 8)
+            return 0.01 * (current - 512)
+
+        else:
+            raise UnsupportedFeatureError(model, DXL_CURRENT_L)
+
+
+    def get_feedback(self, servo_id):
+        """
+        Returns the id, goal, position, error, speed, load, voltage, temperature
+        and moving values from the specified servo.
+        """
+        # read in 17 consecutive bytes starting with low value for goal position
+        response = self.read(servo_id, DXL_GOAL_POSITION_L, 17)
+
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching full servo status')
+        if len(response) == 24:
+            # extract data values from the raw data
+            goal = response[5] + (response[6] << 8)
+            position = response[11] + (response[12] << 8)
+            error = position - goal
+            speed = response[13] + ( response[14] << 8)
+            if speed > 1023: speed = 1023 - speed
+            load_raw = response[15] + (response[16] << 8)
+            load_direction = 1 if self.test_bit(load_raw, 10) else 0
+            load = (load_raw & int('1111111111', 2)) / 1024.0
+            if load_direction == 1: load = -load
+            voltage = response[17] / 10.0
+            temperature = response[18]
+            moving = response[21]
+            timestamp = response[-1]
+
+            # return the data in a dictionary
+            return { 'timestamp': timestamp,
+                     'id': servo_id,
+                     'goal': goal,
+                     'position': position,
+                     'error': error,
+                     'speed': speed,
+                     'load': load,
+                     'voltage': voltage,
+                     'temperature': temperature,
+                     'moving': bool(moving) }
+
+    def get_led(self, servo_id):
+        """
+        Get status of the LED. Boolean return values:
+            True - LED is on,
+            False - LED is off.
+        """
+        response = self.read(servo_id, DXL_LED, 1)
+        if response:
+            self.exception_on_error(response[4], servo_id,
+                'fetching LED status')
+
+        return bool(response[5])
+
+
+    def exception_on_error(self, error_code, servo_id, command_failed):
+        global exception
+        exception = None
+        ex_message = '[servo #%d on %s@%sbps]: %s failed' % (servo_id, self.ser.port, self.ser.baudrate, command_failed)
+
+        if not isinstance(error_code, int):
+            msg = 'Communcation Error ' + ex_message
+            exception = NonfatalErrorCodeError(msg, 0)
+            return
+        if not error_code & DXL_OVERHEATING_ERROR == 0:
+            msg = 'Overheating Error ' + ex_message
+            exception = FatalErrorCodeError(msg, error_code)
+        if not error_code & DXL_OVERLOAD_ERROR == 0:
+            msg = 'Overload Error ' + ex_message
+            exception = FatalErrorCodeError(msg, error_code)
+        if not error_code & DXL_INPUT_VOLTAGE_ERROR == 0:
+            msg = 'Input Voltage Error ' + ex_message
+            exception = NonfatalErrorCodeError(msg, error_code)
+        if not error_code & DXL_ANGLE_LIMIT_ERROR == 0:
+            msg = 'Angle Limit Error ' + ex_message
+            exception = NonfatalErrorCodeError(msg, error_code)
+        if not error_code & DXL_RANGE_ERROR == 0:
+            msg = 'Range Error ' + ex_message
+            exception = NonfatalErrorCodeError(msg, error_code)
+        if not error_code & DXL_CHECKSUM_ERROR == 0:
+            msg = 'Checksum Error ' + ex_message
+            exception = NonfatalErrorCodeError(msg, error_code)
+        if not error_code & DXL_INSTRUCTION_ERROR == 0:
+            msg = 'Instruction Error ' + ex_message
+            exception = NonfatalErrorCodeError(msg, error_code)
+
+class SerialOpenError(Exception):
+    def __init__(self, port, baud):
+        Exception.__init__(self)
+        self.message = "Cannot open port '%s' at %d bps" %(port, baud)
+        self.port = port
+        self.baud = baud
+    def __str__(self):
+        return self.message
+
+class ChecksumError(Exception):
+    def __init__(self, servo_id, response, checksum):
+        Exception.__init__(self)
+        self.message = 'Checksum received from motor %d does not match the expected one (%d != %d)' \
+                       %(servo_id, response[-1], checksum)
+        self.response_data = response
+        self.expected_checksum = checksum
+    def __str__(self):
+        return self.message
+
+class FatalErrorCodeError(Exception):
+    def __init__(self, message, ec_const):
+        Exception.__init__(self)
+        self.message = message
+        self.error_code = ec_const
+    def __str__(self):
+        return self.message
+
+class NonfatalErrorCodeError(Exception):
+    def __init__(self, message, ec_const):
+        Exception.__init__(self)
+        self.message = message
+        self.error_code = ec_const
+    def __str__(self):
+        return self.message
+
+class ErrorCodeError(Exception):
+    def __init__(self, message, ec_const):
+        Exception.__init__(self)
+        self.message = message
+        self.error_code = ec_const
+    def __str__(self):
+        return self.message
+
+class DroppedPacketError(Exception):
+    def __init__(self, message):
+        Exception.__init__(self)
+        self.message = message
+    def __str__(self):
+        return self.message
+
+class UnsupportedFeatureError(Exception):
+    def __init__(self, model_id, feature_id):
+        Exception.__init__(self)
+        if model_id in DXL_MODEL_TO_PARAMS:
+            model = DXL_MODEL_TO_PARAMS[model_id]['name']
+        else:
+            model = 'Unknown'
+        self.message = "Feature %d not supported by model %d (%s)" %(feature_id, model_id, model)
+    def __str__(self):
+        return self.message
+

src/dynamixel_motor/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py

@@ -0,0 +1,334 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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 OWNER 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.
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+__credits__ = 'Cody Jorgensen, Cara Slutter'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+import math
+import sys
+import errno
+from collections import deque
+from threading import Thread
+from collections import defaultdict
+
+import roslib
+roslib.load_manifest('dynamixel_driver')
+
+import rospy
+import dynamixel_io
+from dynamixel_driver.dynamixel_const import *
+
+from diagnostic_msgs.msg import DiagnosticArray
+from diagnostic_msgs.msg import DiagnosticStatus
+from diagnostic_msgs.msg import KeyValue
+
+from dynamixel_msgs.msg import MotorState
+from dynamixel_msgs.msg import MotorStateList
+
+class SerialProxy():
+    def __init__(self,
+                 port_name='/dev/ttyUSB0',
+                 port_namespace='ttyUSB0',
+                 baud_rate='1000000',
+                 min_motor_id=1,
+                 max_motor_id=25,
+                 update_rate=5,
+                 diagnostics_rate=1,
+                 error_level_temp=75,
+                 warn_level_temp=70,
+                 readback_echo=False):
+        self.port_name = port_name
+        self.port_namespace = port_namespace
+        self.baud_rate = baud_rate
+        self.min_motor_id = min_motor_id
+        self.max_motor_id = max_motor_id
+        self.update_rate = update_rate
+        self.diagnostics_rate = diagnostics_rate
+        self.error_level_temp = error_level_temp
+        self.warn_level_temp = warn_level_temp
+        self.readback_echo = readback_echo
+        
+        self.actual_rate = update_rate
+        self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
+        self.current_state = MotorStateList()
+        self.num_ping_retries = 5
+        
+        self.motor_states_pub = rospy.Publisher('motor_states/%s' % self.port_namespace, MotorStateList, queue_size=1)
+        self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
+
+    def connect(self):
+        try:
+            self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo)
+            self.__find_motors()
+        except dynamixel_io.SerialOpenError, e:
+            rospy.logfatal(e.message)
+            sys.exit(1)
+            
+        self.running = True
+        if self.update_rate > 0: Thread(target=self.__update_motor_states).start()
+        if self.diagnostics_rate > 0: Thread(target=self.__publish_diagnostic_information).start()
+
+    def disconnect(self):
+        self.running = False
+
+    def __fill_motor_parameters(self, motor_id, model_number):
+        """
+        Stores some extra information about each motor on the parameter server.
+        Some of these paramters are used in joint controller implementation.
+        """
+        angles = self.dxl_io.get_angle_limits(motor_id)
+        voltage = self.dxl_io.get_voltage(motor_id)
+        voltages = self.dxl_io.get_voltage_limits(motor_id)
+        
+        rospy.set_param('dynamixel/%s/%d/model_number' %(self.port_namespace, motor_id), model_number)
+        rospy.set_param('dynamixel/%s/%d/model_name' %(self.port_namespace, motor_id), DXL_MODEL_TO_PARAMS[model_number]['name'])
+        rospy.set_param('dynamixel/%s/%d/min_angle' %(self.port_namespace, motor_id), angles['min'])
+        rospy.set_param('dynamixel/%s/%d/max_angle' %(self.port_namespace, motor_id), angles['max'])
+        
+        torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt']
+        rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt)
+        rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage)
+        
+        velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt']
+        rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick']
+        rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt)
+        rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage)
+        rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC)
+        
+        encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution']
+        range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees']
+        range_radians = math.radians(range_degrees)
+        rospy.set_param('dynamixel/%s/%d/encoder_resolution' %(self.port_namespace, motor_id), encoder_resolution)
+        rospy.set_param('dynamixel/%s/%d/range_degrees' %(self.port_namespace, motor_id), range_degrees)
+        rospy.set_param('dynamixel/%s/%d/range_radians' %(self.port_namespace, motor_id), range_radians)
+        rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees)
+        rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians)
+        rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution)
+        rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution)
+        
+        # keep some parameters around for diagnostics
+        self.motor_static_info[motor_id] = {}
+        self.motor_static_info[motor_id]['model'] = DXL_MODEL_TO_PARAMS[model_number]['name']
+        self.motor_static_info[motor_id]['firmware'] = self.dxl_io.get_firmware_version(motor_id)
+        self.motor_static_info[motor_id]['delay'] = self.dxl_io.get_return_delay_time(motor_id)
+        self.motor_static_info[motor_id]['min_angle'] = angles['min']
+        self.motor_static_info[motor_id]['max_angle'] = angles['max']
+        self.motor_static_info[motor_id]['min_voltage'] = voltages['min']
+        self.motor_static_info[motor_id]['max_voltage'] = voltages['max']
+
+    def __find_motors(self):
+        rospy.loginfo('%s: Pinging motor IDs %d through %d...' % (self.port_namespace, self.min_motor_id, self.max_motor_id))
+        self.motors = []
+        self.motor_static_info = {}
+        
+        for motor_id in range(self.min_motor_id, self.max_motor_id + 1):
+            for trial in range(self.num_ping_retries):
+                try:
+                    result = self.dxl_io.ping(motor_id)
+                except Exception as ex:
+                    rospy.logerr('Exception thrown while pinging motor %d - %s' % (motor_id, ex))
+                    continue
+                    
+                if result:
+                    self.motors.append(motor_id)
+                    break
+                    
+        if not self.motors:
+            rospy.logfatal('%s: No motors found.' % self.port_namespace)
+            sys.exit(1)
+            
+        counts = defaultdict(int)
+        
+        to_delete_if_error = []
+        for motor_id in self.motors:
+            for trial in range(self.num_ping_retries):
+                try:
+                    model_number = self.dxl_io.get_model_number(motor_id)
+                    self.__fill_motor_parameters(motor_id, model_number)
+                except Exception as ex:
+                    rospy.logerr('Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex))
+                    if trial == self.num_ping_retries - 1: to_delete_if_error.append(motor_id)
+                    continue
+                    
+                counts[model_number] += 1
+                break
+                
+        for motor_id in to_delete_if_error:
+            self.motors.remove(motor_id)
+            
+        rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors)
+        
+        status_str = '%s: Found %d motors - ' % (self.port_namespace, len(self.motors))
+        for model_number,count in counts.items():
+            if count:
+                model_name = DXL_MODEL_TO_PARAMS[model_number]['name']
+                status_str += '%d %s [' % (count, model_name)
+                
+                for motor_id in self.motors:
+                    if self.motor_static_info[motor_id]['model'] == model_name:
+                        status_str += '%d, ' % motor_id
+                        
+                status_str = status_str[:-2] + '], '
+                
+        rospy.loginfo('%s, initialization complete.' % status_str[:-2])
+
+    def __update_motor_states(self):
+        num_events = 50
+        rates = deque([float(self.update_rate)]*num_events, maxlen=num_events)
+        last_time = rospy.Time.now()
+        
+        rate = rospy.Rate(self.update_rate)
+        while not rospy.is_shutdown() and self.running:
+            # get current state of all motors and publish to motor_states topic
+            motor_states = []
+            for motor_id in self.motors:
+                try:
+                    state = self.dxl_io.get_feedback(motor_id)
+                    if state:
+                        motor_states.append(MotorState(**state))
+                        if dynamixel_io.exception: raise dynamixel_io.exception
+                except dynamixel_io.FatalErrorCodeError, fece:
+                    rospy.logerr(fece)
+                except dynamixel_io.NonfatalErrorCodeError, nfece:
+                    self.error_counts['non_fatal'] += 1
+                    rospy.logdebug(nfece)
+                except dynamixel_io.ChecksumError, cse:
+                    self.error_counts['checksum'] += 1
+                    rospy.logdebug(cse)
+                except dynamixel_io.DroppedPacketError, dpe:
+                    self.error_counts['dropped'] += 1
+                    rospy.logdebug(dpe.message)
+                except OSError, ose:
+                    if ose.errno != errno.EAGAIN:
+                        rospy.logfatal(errno.errorcode[ose.errno])
+                        rospy.signal_shutdown(errno.errorcode[ose.errno])
+                        
+            if motor_states:
+                msl = MotorStateList()
+                msl.motor_states = motor_states
+                self.motor_states_pub.publish(msl)
+                
+                self.current_state = msl
+                
+                # calculate actual update rate
+                current_time = rospy.Time.now()
+                rates.append(1.0 / (current_time - last_time).to_sec())
+                self.actual_rate = round(sum(rates)/num_events, 2)
+                last_time = current_time
+                
+            rate.sleep()
+
+    def __publish_diagnostic_information(self):
+        diag_msg = DiagnosticArray()
+        
+        rate = rospy.Rate(self.diagnostics_rate)
+        while not rospy.is_shutdown() and self.running:
+            diag_msg.status = []
+            diag_msg.header.stamp = rospy.Time.now()
+            
+            status = DiagnosticStatus()
+            
+            status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace
+            status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name
+            status.values.append(KeyValue('Baud Rate', str(self.baud_rate)))
+            status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id)))
+            status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id)))
+            status.values.append(KeyValue('Desired Update Rate', str(self.update_rate)))
+            status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate)))
+            status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal'])))
+            status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum'])))
+            status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped'])))
+            status.level = DiagnosticStatus.OK
+            status.message = 'OK'
+            
+            if self.actual_rate - self.update_rate < -5:
+                status.level = DiagnosticStatus.WARN
+                status.message = 'Actual update rate is lower than desired'
+                
+            diag_msg.status.append(status)
+            
+            for motor_state in self.current_state.motor_states:
+                mid = motor_state.id
+                
+                status = DiagnosticStatus()
+                
+                status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace)
+                status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace)
+                status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model'])))
+                status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware'])))
+                status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay'])))
+                status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage'])))
+                status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage'])))
+                status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle'])))
+                status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle'])))
+                
+                status.values.append(KeyValue('Goal', str(motor_state.goal)))
+                status.values.append(KeyValue('Position', str(motor_state.position)))
+                status.values.append(KeyValue('Error', str(motor_state.error)))
+                status.values.append(KeyValue('Velocity', str(motor_state.speed)))
+                status.values.append(KeyValue('Load', str(motor_state.load)))
+                status.values.append(KeyValue('Voltage', str(motor_state.voltage)))
+                status.values.append(KeyValue('Temperature', str(motor_state.temperature)))
+                status.values.append(KeyValue('Moving', str(motor_state.moving)))
+                
+                if motor_state.temperature >= self.error_level_temp:
+                    status.level = DiagnosticStatus.ERROR
+                    status.message = 'OVERHEATING'
+                elif motor_state.temperature >= self.warn_level_temp:
+                    status.level = DiagnosticStatus.WARN
+                    status.message = 'VERY HOT'
+                else:
+                    status.level = DiagnosticStatus.OK
+                    status.message = 'OK'
+                    
+                diag_msg.status.append(status)
+                
+            self.diagnostics_pub.publish(diag_msg)
+            rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        serial_proxy = SerialProxy()
+        serial_proxy.connect()
+        rospy.spin()
+        serial_proxy.disconnect()
+    except rospy.ROSInterruptException: pass
+

src/dynamixel_motor/dynamixel_motor/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package dynamixel_motor
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.1 (2017-01-19)
+------------------
+
+0.4.0 (2013-07-26)
+------------------
+* stack is now catkin compatible