删掉跟机械臂相关的控制代码,在5DOF_ARM分支里有该部分代码

src/arbotix_ros/.gitignore

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

src/arbotix_ros/README.md

@@ -1,18 +0,0 @@
-## 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

@@ -1,27 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

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

src/arbotix_ros/arbotix/package.xml

@@ -1,23 +0,0 @@
-<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

@@ -1,33 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,14 +0,0 @@
-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

@@ -1,239 +0,0 @@
-#!/usr/bin/env python
-
-"""
-  gripper_controller - action based controller for grippers.
-  Copyright (c) 2011-2014 Vanadium Labs LLC.  All right reserved.
-"""
-
-import rospy, actionlib
-import thread
-
-from parallel_convert import ParallelConvert
-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
-
-
-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.
-    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 == '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

@@ -1,212 +0,0 @@
-#!/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_convert.py

@@ -1,45 +0,0 @@
-#!/usr/bin/env python
-
-"""
-  ParallelConvert:  For Parallel/Prismatic joint, convert Angle to Width
-  Copyright (c) 2014 Vanadium Labs LLC.  All right reserved.
-"""
-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_controllers/bin/parallel_gripper_controller.py

@@ -1,91 +0,0 @@
-#!/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

@@ -1,133 +0,0 @@
-#!/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

@@ -1,17 +0,0 @@
-<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

@@ -1,25 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

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

src/arbotix_ros/arbotix_firmware/package.xml

@@ -1,13 +0,0 @@
-<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

@@ -1,148 +0,0 @@
-/* 
-  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

@@ -1,76 +0,0 @@
-/* 
-  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

@@ -1,548 +0,0 @@
-/* 
-  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

@@ -1,22 +0,0 @@
-/*
- * 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

@@ -1,28 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,20 +0,0 @@
-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

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

src/arbotix_ros/arbotix_msgs/msg/Digital.msg

@@ -1,14 +0,0 @@
-# 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

@@ -1,21 +0,0 @@
-<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

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

src/arbotix_ros/arbotix_msgs/srv/Relax.srv

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

src/arbotix_ros/arbotix_msgs/srv/SetSpeed.srv

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

src/arbotix_ros/arbotix_msgs/srv/SetupChannel.srv

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

src/arbotix_ros/arbotix_python/CHANGELOG.rst

@@ -1,36 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,15 +0,0 @@
-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

@@ -1,181 +0,0 @@
-#!/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

@@ -1,127 +0,0 @@
-#!/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, 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('ARM_Controller_GUI')
-    app = wx.App(False)
-    frame = controllerGUI(None, True)
-    app.MainLoop()
-

src/arbotix_ros/arbotix_python/bin/arbotix_terminal

@@ -1,189 +0,0 @@
-#!/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

@@ -1,13 +0,0 @@
-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

@@ -1,11 +0,0 @@
-/**
-\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

@@ -1,26 +0,0 @@
-<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

@@ -1,12 +0,0 @@
-#!/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

@@ -1,517 +0,0 @@
-#!/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

@@ -1,104 +0,0 @@
-#!/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

@@ -1,75 +0,0 @@
-#!/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

@@ -1,261 +0,0 @@
-#!/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

@@ -1,165 +0,0 @@
-#!/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

@@ -1,87 +0,0 @@
-#!/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

@@ -1,160 +0,0 @@
-#!/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

@@ -1,285 +0,0 @@
-#!/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

@@ -1,67 +0,0 @@
-#!/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

@@ -1,92 +0,0 @@
-#!/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

@@ -1,479 +0,0 @@
-#!/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

@@ -1,27 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,10 +0,0 @@
-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

@@ -1,82 +0,0 @@
-#!/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

@@ -1,75 +0,0 @@
-#!/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

@@ -1,15 +0,0 @@
-<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

@@ -1,11 +0,0 @@
-#!/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

@@ -1,86 +0,0 @@
-#!/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

@@ -1,35 +0,0 @@
-*.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

@@ -1,30 +0,0 @@
-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

@@ -1,29 +0,0 @@
-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

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

src/dynamixel_motor/dynamixel_controllers/CHANGELOG.rst

@@ -1,38 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,43 +0,0 @@
-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

@@ -1,39 +0,0 @@
-<!-- -*- 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

@@ -1,26 +0,0 @@
-/**
-\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

@@ -1,269 +0,0 @@
-#!/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

@@ -1,147 +0,0 @@
-#!/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

@@ -1,39 +0,0 @@
-<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

@@ -1,11 +0,0 @@
-#!/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

@@ -1,178 +0,0 @@
-# -*- 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

@@ -1,187 +0,0 @@
-# -*- 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

@@ -1,205 +0,0 @@
-# -*- 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

@@ -1,170 +0,0 @@
-# -*- 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

@@ -1,188 +0,0 @@
-# -*- 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

@@ -1,360 +0,0 @@
-# -*- 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

@@ -1,9 +0,0 @@
-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

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

src/dynamixel_motor/dynamixel_controllers/srv/SetCompliancePunch.srv

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

src/dynamixel_motor/dynamixel_controllers/srv/SetComplianceSlope.srv

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

src/dynamixel_motor/dynamixel_controllers/srv/SetSpeed.srv

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

src/dynamixel_motor/dynamixel_controllers/srv/SetTorqueLimit.srv

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

src/dynamixel_motor/dynamixel_controllers/srv/StartController.srv

@@ -1,9 +0,0 @@
-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

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

src/dynamixel_motor/dynamixel_controllers/srv/TorqueEnable.srv

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

src/dynamixel_motor/dynamixel_driver/CHANGELOG.rst

@@ -1,34 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-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

@@ -1,19 +0,0 @@
-# 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

@@ -1,26 +0,0 @@
-/**
-\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

@@ -1,32 +0,0 @@
-<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

@@ -1,9 +0,0 @@
-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

@@ -1,86 +0,0 @@
-#!/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

@@ -1,137 +0,0 @@
-#!/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

@@ -1,144 +0,0 @@
-#!/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

@@ -1,93 +0,0 @@
-#!/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

@@ -1,11 +0,0 @@
-#!/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

@@ -1,287 +0,0 @@
-# -*- 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

@@ -1,1067 +0,0 @@
-#!/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

@@ -1,334 +0,0 @@
-# -*- 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
-