joints.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2010-2011 Vanadium Labs LLC.
4 # All right reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright
10 # notice, this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above copyright
12 # notice, this list of conditions and the following disclaimer in the
13 # documentation and/or other materials provided with the distribution.
14 # * Neither the name of Vanadium Labs LLC nor the names of its
15 # contributors may be used to endorse or promote products derived
16 # from this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 # DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
22 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
24 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
25 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
26 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
27 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 
29 
30 
31 
32 class Joint:
33 
34 
39  def __init__(self, device, name):
40  self.device = device
41  self.name = name
42  self.controller = None
43 
44  self.position = 0.0
45  self.velocity = 0.0
46  self.last = rospy.Time.now()
47 
48 
53  def interpolate(self, frame):
54  return None
55 
56 
61  def setCurrentFeedback(self, raw_data):
62  return None
63 
64 
69  def setControlOutput(self, position):
70  return None
71 
72 
75  def getDiagnostics(self):
76  return None
77 
78 
79 import rospy
80 import xml.dom.minidom
81 
82 from math import pi, radians
83 
84 
86  try:
87  description = rospy.get_param("robot_description")
88  robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
89  joints = {}
90  # Find all non-fixed joints
91  for child in robot.childNodes:
92  if child.nodeType is child.TEXT_NODE:
93  continue
94  if child.localName == 'joint':
95  jtype = child.getAttribute('type')
96  if jtype == 'fixed':
97  continue
98  name = child.getAttribute('name')
99  if jtype == 'continuous':
100  minval = -pi
101  maxval = pi
102  else:
103  limit = child.getElementsByTagName('limit')[0]
104  minval = float(limit.getAttribute('lower'))
105  maxval = float(limit.getAttribute('upper'))
106 
107  if minval > 0 or maxval < 0:
108  zeroval = (maxval + minval)/2
109  else:
110  zeroval = 0
111 
112  joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
113  joints[name] = joint
114  return joints
115  except:
116  rospy.loginfo('No URDF defined, proceeding with defaults')
117  return dict()
118 
119 
120 
121 def getJointLimits(name, joint_defaults, default_min=-150, default_max=150):
122  min_angle = radians(default_min)
123  max_angle = radians(default_max)
124 
125  try:
126  min_angle = joint_defaults[name]['min']
127  except:
128  pass
129  try:
130  min_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/min_angle"))
131  except:
132  pass
133  try:
134  min_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/min_angle"))
135  except:
136  pass
137  try:
138  min_angle = rospy.get_param("/arbotix/joints/"+name+"/min_position")
139  except:
140  pass
141 
142  try:
143  max_angle = joint_defaults[name]['max']
144  except:
145  pass
146  try:
147  max_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/max_angle"))
148  except:
149  pass
150  try:
151  max_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/max_angle"))
152  except:
153  pass
154  try:
155  max_angle = rospy.get_param("/arbotix/joints/"+name+"/max_position")
156  except:
157  pass
158 
159  return (min_angle, max_angle)
160 
Joints hold current values.
Definition: joints.py:32
def getJointsFromURDF()
Get joint parameters from URDF.
Definition: joints.py:85
def getJointLimits(name, joint_defaults, default_min=-150, default_max=150)
Get limits of servo, from YAML, then URDF, then defaults if neither is defined.
Definition: joints.py:121
def __init__(self, device, name)
Constructs a Joint instance.
Definition: joints.py:39
def interpolate(self, frame)
Get new output, in raw data format.
Definition: joints.py:53
def getDiagnostics(self)
Get a diagnostics message for this joint.
Definition: joints.py:75
def setCurrentFeedback(self, raw_data)
Set the current position from feedback data.
Definition: joints.py:61
def setControlOutput(self, position)
Set the goal position.
Definition: joints.py:69


arbotix_python
Author(s): Michael Ferguson
autogenerated on Mon Feb 28 2022 21:37:42