joints.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2010-2011 Vanadium Labs LLC. 
00004 # All right reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #   * Redistributions of source code must retain the above copyright
00010 #     notice, this list of conditions and the following disclaimer.
00011 #   * Redistributions in binary form must reproduce the above copyright
00012 #     notice, this list of conditions and the following disclaimer in the
00013 #     documentation and/or other materials provided with the distribution.
00014 #   * Neither the name of Vanadium Labs LLC nor the names of its 
00015 #     contributors may be used to endorse or promote products derived 
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 # DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 ## @file joints.py Base class and support functions for joints.
00030 
00031 ## @brief Joints hold current values.
00032 class Joint:
00033 
00034     ## @brief Constructs a Joint instance.
00035     ##
00036     ## @param device The arbotix instance.
00037     ## 
00038     ## @param name The joint name.
00039     def __init__(self, device, name):
00040         self.device = device
00041         self.name = name
00042         self.controller = None
00043 
00044         self.position = 0.0
00045         self.velocity = 0.0
00046         self.last = rospy.Time.now()
00047 
00048     ## @brief Get new output, in raw data format.
00049     ##
00050     ## @param frame The frame length in seconds to interpolate forward.
00051     ##
00052     ## @return The new output, in raw data format. 
00053     def interpolate(self, frame):
00054         return None
00055 
00056     ## @brief Set the current position from feedback data.
00057     ##
00058     ## @param raw_data The current feedback.
00059     ##
00060     ## @return The current position, in radians/meters. 
00061     def setCurrentFeedback(self, raw_data):
00062         return None
00063 
00064     ## @brief Set the goal position.
00065     ##
00066     ## @param position The goal position, in radians/meters. 
00067     ##
00068     ## @return The output position, in raw data format. 
00069     def setControlOutput(self, position):
00070         return None
00071 
00072     ## @brief Get a diagnostics message for this joint.
00073     ##
00074     ## @return Diagnostics message. 
00075     def getDiagnostics(self):
00076         return None
00077 
00078 
00079 import rospy
00080 import xml.dom.minidom
00081 
00082 from math import pi, radians
00083 
00084 ## @brief Get joint parameters from URDF
00085 def getJointsFromURDF():
00086     try:
00087         description = rospy.get_param("robot_description")
00088         robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
00089         joints = {}
00090         # Find all non-fixed joints
00091         for child in robot.childNodes:
00092             if child.nodeType is child.TEXT_NODE:
00093                 continue
00094             if child.localName == 'joint':
00095                 jtype = child.getAttribute('type')
00096                 if jtype == 'fixed':
00097                   continue
00098                 name = child.getAttribute('name')
00099                 if jtype == 'continuous':
00100                     minval = -pi
00101                     maxval = pi
00102                 else:
00103                     limit = child.getElementsByTagName('limit')[0]
00104                     minval = float(limit.getAttribute('lower'))
00105                     maxval = float(limit.getAttribute('upper'))
00106 
00107                 if minval > 0 or maxval < 0:
00108                     zeroval = (maxval + minval)/2
00109                 else:
00110                     zeroval = 0
00111 
00112                 joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
00113                 joints[name] = joint
00114         return joints
00115     except:
00116         rospy.loginfo('No URDF defined, proceeding with defaults')
00117         return dict()
00118 
00119 
00120 ## @brief Get limits of servo, from YAML, then URDF, then defaults if neither is defined.
00121 def getJointLimits(name, joint_defaults, default_min=-150, default_max=150):
00122     min_angle = radians(default_min)
00123     max_angle = radians(default_max)
00124     
00125     try: 
00126         min_angle = joint_defaults[name]['min']
00127     except:
00128         pass
00129     try: 
00130         min_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/min_angle"))
00131     except:
00132         pass
00133     try: 
00134         min_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/min_angle"))
00135     except:
00136         pass
00137     try: 
00138         min_angle = rospy.get_param("/arbotix/joints/"+name+"/min_position")
00139     except:
00140         pass
00141 
00142     try: 
00143         max_angle = joint_defaults[name]['max']
00144     except:
00145         pass
00146     try: 
00147         max_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/max_angle"))
00148     except:
00149         pass
00150     try: 
00151         max_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/max_angle"))
00152     except:
00153         pass
00154     try: 
00155         max_angle = rospy.get_param("/arbotix/joints/"+name+"/max_position")
00156     except:
00157         pass
00158 
00159     return (min_angle, max_angle)
00160 


arbotix_python
Author(s): Michael Ferguson
autogenerated on Sat Dec 28 2013 16:46:13