Updates to simmechanics_to_urdf and resource_retriever
This commit is contained in:
davidlu
parent
4f07e9d19a
commit
3970d34724
|
|
@ -0,0 +1,36 @@
|
|||
import roslib; roslib.load_manifest('resource_retriever')
|
||||
import subprocess
|
||||
import urlgrabber, string
|
||||
|
||||
PACKAGE_PREFIX = 'package://'
|
||||
|
||||
def rospack_find(package):
|
||||
process = subprocess.Popen(['rospack', 'find', package], shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
|
||||
(stdout, stderr) = process.communicate()
|
||||
if len(stderr) > 0:
|
||||
raise Exception(stderr)
|
||||
else:
|
||||
return string.strip(stdout)
|
||||
|
||||
def get_filename(url, use_protocol=True ):
|
||||
mod_url = url
|
||||
if url.find(PACKAGE_PREFIX) == 0:
|
||||
mod_url = url[len(PACKAGE_PREFIX):]
|
||||
pos = mod_url.find('/')
|
||||
if pos == -1:
|
||||
raise Exception("Could not parse package:// format into file:// format for "+url)
|
||||
|
||||
package = mod_url[0:pos]
|
||||
mod_url = mod_url[pos:]
|
||||
package_path = rospack_find(package)
|
||||
|
||||
if use_protocol:
|
||||
protocol = "file://"
|
||||
else:
|
||||
protocol = ""
|
||||
mod_url = protocol + package_path + mod_url;
|
||||
return mod_url
|
||||
|
||||
def get(url):
|
||||
return urlgrabber.urlopen(get_filename(url))
|
||||
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
import roslib; roslib.load_manifest('resource_retriever2')
|
||||
import rospy
|
||||
import subprocess
|
||||
import urlgrabber, string
|
||||
|
||||
PACKAGE_PREFIX = 'package://'
|
||||
|
||||
def rospack_find(package):
|
||||
process = subprocess.Popen(['rospack', 'find', package], shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
|
||||
(stdout, stderr) = process.communicate()
|
||||
if len(stderr) > 0:
|
||||
raise Exception(stderr)
|
||||
else:
|
||||
return string.strip(stdout)
|
||||
|
||||
def get(url):
|
||||
mod_url = url
|
||||
if url.find(PACKAGE_PREFIX) == 0:
|
||||
mod_url = url[len(PACKAGE_PREFIX):]
|
||||
pos = mod_url.find('/')
|
||||
if pos == -1:
|
||||
raise Exception("Could not parse package:// format into file:// format for "+url)
|
||||
|
||||
package = mod_url[0:pos]
|
||||
mod_url = mod_url[pos:]
|
||||
package_path = rospack_find(package)
|
||||
|
||||
mod_url = "file://" + package_path + mod_url;
|
||||
|
||||
return urlgrabber.urlopen(mod_url)
|
||||
|
||||
|
|
@ -5,12 +5,13 @@ import rospy
|
|||
import sys
|
||||
import tf
|
||||
from tf.transformations import euler_from_quaternion, quaternion_from_matrix
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
import xml.dom.minidom
|
||||
from xml.dom.minidom import Document
|
||||
import threading
|
||||
import math
|
||||
import numpy
|
||||
import numpy #for creating matrix to convert to quaternion
|
||||
import yaml
|
||||
from urdf_python.urdf import *
|
||||
|
||||
# Conversion Factors
|
||||
INCH2METER = 0.0254
|
||||
|
|
@ -22,10 +23,10 @@ MM2M = .001
|
|||
WORLD = "WORLD"
|
||||
|
||||
# Arbitrary List of colors to give pieces different looks
|
||||
COLORS =[("green", "0 1 0 1"), ("black", "0 0 0 1"), ("red", "1 0 0 1"),
|
||||
("blue", "0 0 1 1"), ("yellow", "1 1 0 1"), ("pink", "1 0 1 1"),
|
||||
("cyan", "0 1 1 1"), ("green", "0 1 0 1"), ("white", "1 1 1 1"),
|
||||
("dblue", "0 0 .8 1"), ("dgreen", ".1 .8 .1 1"), ("gray", ".5 .5 .5 1")]
|
||||
COLORS =[("green", (0, 1, 0, 1)), ("black", (0, 0, 0, 1)), ("red", (1, 0, 0, 1)),
|
||||
("blue", (0, 0, 1, 1)), ("yellow", (1, 1, 0, 1)), ("pink", (1, 0, 1, 1)),
|
||||
("cyan", (0, 1, 1, 1)), ("green", (0, 1, 0, 1)), ("white", (1, 1, 1, 1)),
|
||||
("dblue", (0, 0, .8, 1)), ("dgreen", (.1, .8, .1, 1)), ("gray", (.5, .5, .5, 1))]
|
||||
|
||||
class Converter:
|
||||
def __init__(self):
|
||||
|
|
@ -40,7 +41,6 @@ class Converter:
|
|||
|
||||
# Start the Transform Manager
|
||||
self.tfman = TransformManager()
|
||||
self.tfman.start()
|
||||
|
||||
# Extra Transforms for Debugging
|
||||
self.tfman.add([0,0,0], [0.70682518,0,0,0.70682518], "ROOT", WORLD) # rotate so Z axis is up
|
||||
|
|
@ -56,12 +56,11 @@ class Converter:
|
|||
self.buildTree(self.root)
|
||||
|
||||
# Create the output
|
||||
self.result = Document()
|
||||
self.output(self.root)
|
||||
|
||||
# output the output
|
||||
if mode == "xml":
|
||||
print self.result.toprettyxml()
|
||||
print self.result.to_xml()
|
||||
if mode == "graph":
|
||||
print self.graph()
|
||||
if mode == "groups":
|
||||
|
|
@ -140,8 +139,7 @@ class Converter:
|
|||
colorelement = linkdict['MaterialProp'][1]
|
||||
color = colorelement.childNodes[0].data
|
||||
linkdict['MaterialProp'] = None
|
||||
linkdict['color'] = color.replace(",", " ") + " 1"
|
||||
|
||||
linkdict['color'] = map(float, color.split(",")) + [1.0]
|
||||
|
||||
self.links[uid] = linkdict
|
||||
self.parseFrames(frames, uid)
|
||||
|
|
@ -302,17 +300,14 @@ class Converter:
|
|||
def output(self, rootid):
|
||||
"""Creates the URDF from the parsed document.
|
||||
Makes the document and starts the recursive build process"""
|
||||
doc = self.result;
|
||||
self.root = doc.createElement("robot")
|
||||
doc.appendChild(self.root)
|
||||
self.root.setAttribute("name", self.name)
|
||||
self.result = URDF(self.name)
|
||||
self.outputLink(rootid)
|
||||
self.processLink(rootid)
|
||||
|
||||
def processLink(self, id):
|
||||
""" Creates the output for the specified node's
|
||||
child links, the connecting joints, then
|
||||
recursively processes each child """
|
||||
child links, the connecting joints, then
|
||||
recursively processes each child """
|
||||
link = self.links[id]
|
||||
for cid in link['children']:
|
||||
jid = link['jointmap'][cid]
|
||||
|
|
@ -323,102 +318,78 @@ class Converter:
|
|||
|
||||
def outputLink(self, id):
|
||||
""" Creates the URDF output for a single link """
|
||||
doc = self.result
|
||||
|
||||
linkdict = self.links[id]
|
||||
if linkdict['name'] == "RootPart":
|
||||
return
|
||||
|
||||
link = doc.createElement("link")
|
||||
link.setAttribute("name", id)
|
||||
|
||||
visual = doc.createElement("visual")
|
||||
inertial = doc.createElement("inertial")
|
||||
collision = doc.createElement("collision")
|
||||
link.appendChild(visual)
|
||||
link.appendChild(inertial)
|
||||
link.appendChild(collision)
|
||||
visual = Visual()
|
||||
inertial = Inertial()
|
||||
collision = Collision()
|
||||
|
||||
# Define Geometry
|
||||
geometry = doc.createElement("geometry")
|
||||
mesh = doc.createElement("mesh")
|
||||
filename = linkdict['geometryFileName']
|
||||
if self.forcelowercase:
|
||||
filename = filename.lower()
|
||||
|
||||
mesh.setAttribute("filename", self.filenameformat % filename)
|
||||
if self.scale != None:
|
||||
mesh.setAttribute("scale", self.scale)
|
||||
geometry.appendChild(mesh)
|
||||
visual.appendChild(geometry)
|
||||
collision.appendChild(geometry.cloneNode(1))
|
||||
filename = self.filenameformat % filename
|
||||
|
||||
visual.geometry = Mesh(filename, self.scale)
|
||||
collision.geometry = visual.geometry
|
||||
|
||||
# Define Inertial Frame
|
||||
mass = doc.createElement("mass")
|
||||
units = linkdict['massUnits']
|
||||
massval = convert(float(linkdict['mass']), units)
|
||||
mass.setAttribute("value", str(massval))
|
||||
inertial.appendChild(mass)
|
||||
inertial.mass = massval
|
||||
|
||||
inertia = doc.createElement("inertia")
|
||||
matrix = getlist(linkdict["inertia"])
|
||||
|
||||
units = linkdict['inertiaUnits']
|
||||
|
||||
for i in range(0,len(matrix)):
|
||||
matrix[i] = convert(matrix[i], units)
|
||||
|
||||
inertia.setAttribute("ixx", str(matrix[0]))
|
||||
inertia.setAttribute("ixy", str(matrix[1]))
|
||||
inertia.setAttribute("ixz", str(matrix[2]))
|
||||
inertia.setAttribute("iyy", str(matrix[4]))
|
||||
inertia.setAttribute("iyz", str(matrix[5]))
|
||||
inertia.setAttribute("izz", str(matrix[8]))
|
||||
inertial.appendChild(inertia)
|
||||
inertial.matrix['ixx'] = matrix[0]
|
||||
inertial.matrix['ixy'] = matrix[1]
|
||||
inertial.matrix['ixz'] = matrix[2]
|
||||
inertial.matrix['iyy'] = matrix[4]
|
||||
inertial.matrix['iyz'] = matrix[5]
|
||||
inertial.matrix['izz'] = matrix[8]
|
||||
|
||||
# Inertial origin is the center of gravity
|
||||
iorigin = doc.createElement("origin")
|
||||
(off, rot) = self.tfman.get("X" + id, id+"CG")
|
||||
rpy = list(euler_from_quaternion(rot))
|
||||
iorigin.setAttribute("xyz", " ".join(map(str,zero(off))))
|
||||
iorigin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
|
||||
inertial.appendChild(iorigin)
|
||||
inertial.origin = Pose(zero(off), zero(rpy))
|
||||
|
||||
# Create link's origin
|
||||
origin = doc.createElement("origin")
|
||||
|
||||
# Visual offset is difference between origin and CS1
|
||||
(off, rot) = self.tfman.get("X" + id, id+"CS1")
|
||||
rpy = list(euler_from_quaternion(rot))
|
||||
origin.setAttribute("xyz", " ".join(map(str,zero(off))))
|
||||
origin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
|
||||
visual.appendChild(origin)
|
||||
collision.appendChild(origin.cloneNode(1))
|
||||
|
||||
# Define Material
|
||||
material = doc.createElement("material")
|
||||
visual.origin = Pose(zero(off), zero(rpy))
|
||||
collision.origin = visual.origin
|
||||
|
||||
# Define Material
|
||||
visual.material = Material()
|
||||
# Use specified color, if exists. Otherwise, get random color
|
||||
if 'color' in linkdict:
|
||||
cname = "%s_color"%id
|
||||
rgba = linkdict['color']
|
||||
(r,g,b,a) = linkdict['color']
|
||||
else:
|
||||
(cname, rgba) = self.getColor(linkdict['name'])
|
||||
material.setAttribute("name", cname)
|
||||
visual.appendChild(material)
|
||||
(cname, (r,g,b,a)) = self.getColor(linkdict['name'])
|
||||
|
||||
visual.material.name = cname
|
||||
|
||||
# If color has already been output, only output name
|
||||
if not cname in self.usedcolors:
|
||||
color = doc.createElement("color")
|
||||
color.setAttribute("rgba", rgba)
|
||||
material.appendChild(color)
|
||||
visual.material.color = Color(r,g,b,a)
|
||||
self.usedcolors[cname] = True
|
||||
|
||||
self.root.appendChild(link)
|
||||
|
||||
link = Link(id, visual, inertial, collision)
|
||||
self.result.add_link(link)
|
||||
|
||||
def getColor(self, s):
|
||||
""" Gets a two element list containing a color name,
|
||||
and it's rgba. The color selected is based on the mesh name.
|
||||
If already seen, returns the saved color
|
||||
Otherwise, returns the next available color"""
|
||||
and it's rgba. The color selected is based on the mesh name.
|
||||
If already seen, returns the saved color
|
||||
Otherwise, returns the next available color"""
|
||||
if s in self.colormap:
|
||||
return self.colormap[s]
|
||||
color = COLORS[self.colorindex]
|
||||
|
|
@ -428,14 +399,11 @@ class Converter:
|
|||
|
||||
def outputJoint(self, id, parentname):
|
||||
""" Outputs URDF for a single joint """
|
||||
doc = self.result
|
||||
jointdict = self.joints[id]
|
||||
|
||||
if "Root" in jointdict['name']:
|
||||
return
|
||||
|
||||
joint = doc.createElement("joint")
|
||||
joint.setAttribute('name', id)
|
||||
|
||||
# Define the parent and child
|
||||
pid = self.getLinkNameByFrame(jointdict['parent'])
|
||||
|
|
@ -446,39 +414,28 @@ class Converter:
|
|||
if parentname != pid:
|
||||
cid = pid
|
||||
pid = parentname
|
||||
|
||||
parent = doc.createElement("parent")
|
||||
child = doc.createElement("child")
|
||||
parent.setAttribute('link', pid)
|
||||
child.setAttribute('link', cid)
|
||||
joint.appendChild(parent)
|
||||
joint.appendChild(child)
|
||||
|
||||
# Define joint type
|
||||
jtype = jointdict['type']
|
||||
joint.setAttribute('type', jtype)
|
||||
|
||||
limits = None
|
||||
axis = None
|
||||
|
||||
if 'limits' in jointdict:
|
||||
limit = doc.createElement("limit")
|
||||
limits = JointLimit(None, None)
|
||||
for (k,v) in jointdict['limits'].items():
|
||||
limit.setAttribute(str(k), str(v))
|
||||
joint.appendChild(limit)
|
||||
setattr(limits, k, v)
|
||||
|
||||
if 'axis' in jointdict and jtype != 'fixed':
|
||||
axis = doc.createElement("axis")
|
||||
xyz = jointdict['axis'].replace(',', ' ')
|
||||
axis.setAttribute('xyz', xyz)
|
||||
joint.appendChild(axis)
|
||||
axis = jointdict['axis'].replace(',', ' ')
|
||||
|
||||
# Define the origin
|
||||
origin = doc.createElement("origin")
|
||||
(off, rot) = self.tfman.get("X" + pid, "X" + cid)
|
||||
rpy = list(euler_from_quaternion(rot))
|
||||
off = zero(off)
|
||||
rpy = zero(rpy)
|
||||
origin.setAttribute("xyz", " ".join(map(str,off)))
|
||||
origin.setAttribute("rpy", " ".join(map(str,rpy)))
|
||||
joint.appendChild(origin)
|
||||
origin = Pose(zero(off), zero(rpy))
|
||||
|
||||
self.root.appendChild(joint)
|
||||
joint = Joint(id, pid, cid, jtype, limits=limits, axis=axis, origin=origin)
|
||||
self.result.add_joint(joint)
|
||||
|
||||
def getName(self, basename):
|
||||
"""Return a unique name of the format
|
||||
|
|
@ -540,7 +497,7 @@ class Converter:
|
|||
|
||||
def groups(self, root):
|
||||
""" For planning purposes, print out lists of
|
||||
all the links between the different joints"""
|
||||
all the links between the different joints"""
|
||||
self.groups = {}
|
||||
self.makeGroup(root, "BASE")
|
||||
s = ""
|
||||
|
|
@ -554,7 +511,7 @@ class Converter:
|
|||
|
||||
def makeGroup(self, id, gid):
|
||||
""" Helper function for recursively gathering
|
||||
groups of joints. """
|
||||
groups of joints. """
|
||||
if gid in self.groups:
|
||||
idlist = self.groups[gid]
|
||||
idlist.append(id)
|
||||
|
|
@ -624,9 +581,9 @@ def matrixToQuaternion(matrix):
|
|||
(R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix
|
||||
# Build 4x4 matrix
|
||||
M = [[R11, R21, R31, 0],
|
||||
[R12, R22, R32, 0],
|
||||
[R13, R23, R33, 0],
|
||||
[0, 0, 0, 1]]
|
||||
[R12, R22, R32, 0],
|
||||
[R13, R23, R33, 0],
|
||||
[0, 0, 0, 1]]
|
||||
A = numpy.array(M)
|
||||
[w,x,y,z] = quaternion_from_matrix(A)
|
||||
return [w,x,y,z]
|
||||
|
|
@ -646,48 +603,28 @@ def zero(arr):
|
|||
return arr
|
||||
|
||||
|
||||
class TransformManager(threading.Thread):
|
||||
"""Bridge to the ROS tf package. Constantly broadcasts
|
||||
all tfs, and retrieves them on demand."""
|
||||
|
||||
class TransformManager:
|
||||
def __init__(self):
|
||||
threading.Thread.__init__(self)
|
||||
self.running = True
|
||||
self.tfs = []
|
||||
self.listener = tf.TransformListener()
|
||||
self.tf = tf.Transformer(True, rospy.Duration(10.0))
|
||||
|
||||
def run(self):
|
||||
"""Broadcasts tfs until shutdown"""
|
||||
rate = rospy.Rate(10.0)
|
||||
br = tf.TransformBroadcaster()
|
||||
while self.running:
|
||||
for transform in self.tfs:
|
||||
br.sendTransform(transform['trans'],
|
||||
transform['rot'],
|
||||
rospy.Time.now(),
|
||||
transform['child'],
|
||||
transform['parent'])
|
||||
rate.sleep()
|
||||
|
||||
def add(self, trans, rot, parent, child):
|
||||
def add(self, offset, angle, parent, child):
|
||||
"""Adds a new transform to the set"""
|
||||
tf = {'trans':trans, 'rot':rot, 'child':child, 'parent':parent}
|
||||
self.tfs.append(tf)
|
||||
m = TransformStamped()
|
||||
m.header.frame_id = parent
|
||||
m.child_frame_id = child
|
||||
m.transform.translation.x = offset[0]
|
||||
m.transform.translation.y = offset[1]
|
||||
m.transform.translation.z = offset[2]
|
||||
m.transform.rotation.x = angle[0]
|
||||
m.transform.rotation.y = angle[1]
|
||||
m.transform.rotation.z = angle[2]
|
||||
m.transform.rotation.w = angle[3]
|
||||
self.tf.setTransform(m)
|
||||
|
||||
def get(self, parent, child):
|
||||
"""Attempts to retrieve a transform"""
|
||||
attempts = 0
|
||||
rate = rospy.Rate(10.0)
|
||||
while attempts < 50:
|
||||
try:
|
||||
(off,rpy) = self.listener.lookupTransform(parent, child, rospy.Time(0))
|
||||
return [list(off), list(rpy)]
|
||||
except (tf.LookupException, tf.ConnectivityException):
|
||||
attempts+=1
|
||||
rate.sleep()
|
||||
continue
|
||||
|
||||
raise Exception("Attempt to transform %s exceeded attempt limit" % (parent + "/" + child))
|
||||
"""Retrieves a transform"""
|
||||
(off,rpy) = self.tf.lookupTransform(parent, child, rospy.Time(0))
|
||||
return [list(off), list(rpy)]
|
||||
|
||||
def printTransform(self, parent, child):
|
||||
"""Attempts to print the specified transform"""
|
||||
|
|
@ -725,8 +662,6 @@ if __name__ == '__main__':
|
|||
None
|
||||
except Exception:
|
||||
raise
|
||||
finally:
|
||||
con.tfman.kill()
|
||||
|
||||
except rospy.ROSInterruptException: pass
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
<depend package="rospy"/>
|
||||
<depend package="sensor_msgs"/>
|
||||
<depend package="tf"/>
|
||||
<depend package="urdf_python"/>
|
||||
<rosdep name="python-yaml"/>
|
||||
</package>
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue