From 092e39a9e5842819594a7398e3d81ab7bd7e5683 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 22 Jun 2018 09:18:55 -0700 Subject: [PATCH] PyBullet urdfEditor example, use the pybullet_utils version (more up-to-date) --- examples/pybullet/examples/urdfEditor.py | 509 +---------------------- 1 file changed, 3 insertions(+), 506 deletions(-) diff --git a/examples/pybullet/examples/urdfEditor.py b/examples/pybullet/examples/urdfEditor.py index f2dfb8cf1..7d728d054 100644 --- a/examples/pybullet/examples/urdfEditor.py +++ b/examples/pybullet/examples/urdfEditor.py @@ -1,512 +1,9 @@ import pybullet as p import time +from pybullet_utils import urdfEditor -class UrdfInertial(object): - def __init__(self): - self.mass = 1 - self.inertia_xxyyzz=[7,8,9] - self.origin_rpy=[1,2,3] - self.origin_xyz=[4,5,6] - -class UrdfContact(object): - def __init__(self): - self.lateral_friction = 1 - self.rolling_friction = 0 - self.spinning_friction = 0 - -class UrdfLink(object): - def __init__(self): - self.link_name = "dummy" - self.urdf_inertial = UrdfInertial() - self.urdf_visual_shapes=[] - self.urdf_collision_shapes=[] - -class UrdfVisual(object): - def __init__(self): - self.origin_rpy = [1,2,3] - self.origin_xyz = [4,5,6] - self.geom_type = p.GEOM_BOX - self.geom_radius = 1 - self.geom_extents = [7,8,9] - self.geom_length=[10] - self.geom_meshfilename = "meshfile" - self.geom_meshscale=[1,1,1] - self.material_rgba = [1,0,0,1] - self.material_name = "" - -class UrdfCollision(object): - def __init__(self): - self.origin_rpy = [1,2,3] - self.origin_xyz = [4,5,6] - self.geom_type = p.GEOM_BOX - self.geom_radius = 1 - self.geom_length = 2 - self.geom_extents = [7,8,9] - self.geom_meshfilename = "meshfile" - self.geom_meshscale = [1,1,1] -class UrdfJoint(object): - def __init__(self): - self.link = UrdfLink() - self.joint_name = "joint_dummy" - self.joint_type = p.JOINT_REVOLUTE - self.joint_lower_limit = 0 - self.joint_upper_limit = -1 - self.parent_name = "parentName" - self.child_name = "childName" - self.joint_origin_xyz = [1,2,3] - self.joint_origin_rpy = [1,2,3] - self.joint_axis_xyz = [1,2,3] - -class UrdfEditor(object): - def __init__(self): - self.initialize() - - def initialize(self): - self.urdfLinks=[] - self.urdfJoints=[] - self.robotName = "" - self.linkNameToIndex={} - self.jointTypeToName={p.JOINT_FIXED:"JOINT_FIXED" ,\ - p.JOINT_REVOLUTE:"JOINT_REVOLUTE",\ - p.JOINT_PRISMATIC:"JOINT_PRISMATIC" } - - - def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId): - dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId) - urdfLink.urdf_inertial.mass = dyn[0] - urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2] - #todo - urdfLink.urdf_inertial.origin_xyz = dyn[3] - rpy = p.getEulerFromQuaternion(dyn[4]) - urdfLink.urdf_inertial.origin_rpy = rpy - - visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId) - matIndex = 0 - for v in visualShapes: - if (v[1]==linkIndex): - #print("visualShape base:",v) - urdfVisual = UrdfVisual() - urdfVisual.geom_type = v[2] - if (v[2]==p.GEOM_BOX): - urdfVisual.geom_extents = v[3] - if (v[2]==p.GEOM_SPHERE): - urdfVisual.geom_radius = v[3][0] - if (v[2]==p.GEOM_MESH): - urdfVisual.geom_meshfilename = v[4].decode("utf-8") - urdfVisual.geom_meshscale = v[3] - if (v[2]==p.GEOM_CYLINDER): - urdfVisual.geom_length=v[3][0] - urdfVisual.geom_radius=v[3][1] - if (v[2]==p.GEOM_CAPSULE): - urdfVisual.geom_length=v[3][0] - urdfVisual.geom_radius=v[3][1] - #print("Capsule length=",urdfVisual.geom_length) - #print("Capsule radius=",urdfVisual.geom_radius) - - urdfVisual.origin_xyz = v[5] - urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6]) - urdfVisual.material_rgba = v[7] - name = 'mat_{}_{}'.format(linkIndex,matIndex) - urdfVisual.material_name = name - - urdfLink.urdf_visual_shapes.append(urdfVisual) - matIndex=matIndex+1 - - collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId) - for v in collisionShapes: - #print("collisionShape base:",v) - urdfCollision = UrdfCollision() - print("geom type=",v[0]) - urdfCollision.geom_type = v[2] - if (v[2]==p.GEOM_BOX): - urdfCollision.geom_extents = v[3] - if (v[2]==p.GEOM_SPHERE): - urdfCollision.geom_radius = v[3][0] - if (v[2]==p.GEOM_MESH): - urdfCollision.geom_meshfilename = v[4].decode("utf-8") - urdfCollision.geom_meshscale = v[3] - print("p.GEOM_MESH filename=",urdfCollision.geom_meshfilename) - if (v[2]==p.GEOM_CYLINDER): - urdfCollision.geom_length=v[3][0] - urdfCollision.geom_radius=v[3][1] - if (v[2]==p.GEOM_CAPSULE): - urdfCollision.geom_length=v[3][0] - urdfCollision.geom_radius=v[3][1] - print("Capsule length=",urdfCollision.geom_length) - print("Capsule radius=",urdfCollision.geom_radius) - pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\ - v[5], v[6]) - urdfCollision.origin_xyz = pos - urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn) - urdfLink.urdf_collision_shapes.append(urdfCollision) - - def initializeFromBulletBody(self, bodyUid, physicsClientId): - self.initialize() - - #always create a base link - baseLink = UrdfLink() - baseLinkIndex = -1 - self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId) - baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8") - self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks) - self.urdfLinks.append(baseLink) - - - #print(visualShapes) - #optionally create child links and joints - for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)): - jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId) - urdfLink = UrdfLink() - self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId) - urdfLink.link_name = jointInfo[12].decode("utf-8") - self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks) - self.urdfLinks.append(urdfLink) - - urdfJoint = UrdfJoint() - urdfJoint.link = urdfLink - urdfJoint.joint_name = jointInfo[1].decode("utf-8") - urdfJoint.joint_type = jointInfo[2] - urdfJoint.joint_axis_xyz = jointInfo[13] - orgParentIndex = jointInfo[16] - if (orgParentIndex<0): - urdfJoint.parent_name = baseLink.link_name - else: - parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId) - urdfJoint.parent_name = parentJointInfo[12].decode("utf-8") - urdfJoint.child_name = urdfLink.link_name - - #todo, compensate for inertia/link frame offset - - dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId) - childInertiaPos = dynChild[3] - childInertiaOrn = dynChild[4] - parentCom2JointPos=jointInfo[14] - parentCom2JointOrn=jointInfo[15] - tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn) - tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn) - dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId) - parentInertiaPos = dynParent[3] - parentInertiaOrn = dynParent[4] - - pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv) - pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1]) - - urdfJoint.joint_origin_xyz = pos - urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn) - - self.urdfJoints.append(urdfJoint) - - def writeInertial(self,file,urdfInertial, precision=5): - file.write("\t\t\n") - str = '\t\t\t\n'.format(\ - urdfInertial.origin_rpy[0],urdfInertial.origin_rpy[1],urdfInertial.origin_rpy[2],\ - urdfInertial.origin_xyz[0],urdfInertial.origin_xyz[1],urdfInertial.origin_xyz[2], prec=precision) - file.write(str) - str = '\t\t\t\n'.format(urdfInertial.mass,prec=precision) - file.write(str) - str = '\t\t\t\n'.format(\ - urdfInertial.inertia_xxyyzz[0],\ - urdfInertial.inertia_xxyyzz[1],\ - urdfInertial.inertia_xxyyzz[2],prec=precision) - file.write(str) - file.write("\t\t\n") - - def writeVisualShape(self,file,urdfVisual, precision=5): - file.write("\t\t\n") - str = '\t\t\t\n'.format(\ - urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2], - urdfVisual.origin_xyz[0],urdfVisual.origin_xyz[1],urdfVisual.origin_xyz[2], prec=precision) - file.write(str) - file.write("\t\t\t\n") - if urdfVisual.geom_type == p.GEOM_BOX: - str = '\t\t\t\t\n'.format(urdfVisual.geom_extents[0],\ - urdfVisual.geom_extents[1],urdfVisual.geom_extents[2], prec=precision) - file.write(str) - if urdfVisual.geom_type == p.GEOM_SPHERE: - str = '\t\t\t\t\n'.format(urdfVisual.geom_radius,\ - prec=precision) - file.write(str) - if urdfVisual.geom_type == p.GEOM_MESH: - str = '\t\t\t\t\n'.format(urdfVisual.geom_meshfilename,\ - prec=precision) - file.write(str) - if urdfVisual.geom_type == p.GEOM_CYLINDER: - str = '\t\t\t\t\n'.format(\ - urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision) - file.write(str) - if urdfVisual.geom_type == p.GEOM_CAPSULE: - str = '\t\t\t\t\n'.format(\ - urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision) - file.write(str) - - file.write("\t\t\t\n") - str = '\t\t\t\n'.format(urdfVisual.material_name) - file.write(str) - str = '\t\t\t\t\n'.format(urdfVisual.material_rgba[0],\ - urdfVisual.material_rgba[1],urdfVisual.material_rgba[2],urdfVisual.material_rgba[3],prec=precision) - file.write(str) - file.write("\t\t\t\n") - file.write("\t\t\n") - - def writeCollisionShape(self,file,urdfCollision, precision=5): - file.write("\t\t\n") - str = '\t\t\t\n'.format(\ - urdfCollision.origin_rpy[0],urdfCollision.origin_rpy[1],urdfCollision.origin_rpy[2], - urdfCollision.origin_xyz[0],urdfCollision.origin_xyz[1],urdfCollision.origin_xyz[2], prec=precision) - file.write(str) - file.write("\t\t\t\n") - if urdfCollision.geom_type == p.GEOM_BOX: - str = '\t\t\t\t\n'.format(urdfCollision.geom_extents[0],\ - urdfCollision.geom_extents[1],urdfCollision.geom_extents[2], prec=precision) - file.write(str) - if urdfCollision.geom_type == p.GEOM_SPHERE: - str = '\t\t\t\t\n'.format(urdfCollision.geom_radius,\ - prec=precision) - file.write(str) - if urdfCollision.geom_type == p.GEOM_MESH: - str = '\t\t\t\t\n'.format(urdfCollision.geom_meshfilename,\ - prec=precision) - file.write(str) - if urdfCollision.geom_type == p.GEOM_CYLINDER: - str = '\t\t\t\t\n'.format(\ - urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision) - file.write(str) - if urdfCollision.geom_type == p.GEOM_CAPSULE: - str = '\t\t\t\t\n'.format(\ - urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision) - file.write(str) - file.write("\t\t\t\n") - file.write("\t\t\n") - - - def writeLink(self, file, urdfLink): - file.write("\t\n") - - self.writeInertial(file,urdfLink.urdf_inertial) - for v in urdfLink.urdf_visual_shapes: - self.writeVisualShape(file,v) - for c in urdfLink.urdf_collision_shapes: - self.writeCollisionShape(file,c) - file.write("\t\n") - - def writeJoint(self, file, urdfJoint, precision=5): - jointTypeStr = "invalid" - if urdfJoint.joint_type == p.JOINT_REVOLUTE: - if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit: - jointTypeStr = "continuous" - else: - jointTypeStr = "revolute" - if urdfJoint.joint_type == p.JOINT_FIXED: - jointTypeStr = "fixed" - if urdfJoint.joint_type == p.JOINT_PRISMATIC: - jointTypeStr = "prismatic" - str = '\t\n'.format(urdfJoint.joint_name, jointTypeStr) - file.write(str) - str = '\t\t\n'.format(urdfJoint.parent_name) - file.write(str) - str = '\t\t\n'.format(urdfJoint.child_name) - file.write(str) - - if urdfJoint.joint_type == p.JOINT_PRISMATIC: - #todo: handle limits - lowerLimit=-0.5 - upperLimit=0.5 - str=''.format(lowerLimit,upperLimit,prec=precision) - file.write(str) - - file.write("\t\t\n") - str = '\t\t\n'.format(urdfJoint.joint_origin_xyz[0],\ - urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision) - file.write(str) - str = '\t\t\n'.format(urdfJoint.joint_axis_xyz[0],\ - urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision) - file.write(str) - file.write("\t\n") - - def saveUrdf(self, fileName): - file = open(fileName,"w") - file.write("\n") - file.write("\n") - - for link in self.urdfLinks: - self.writeLink(file,link) - - for joint in self.urdfJoints: - self.writeJoint(file,joint) - - file.write("\n") - file.close() - - #def addLink(...) - - def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0): - #assume link[0] is base - if (len(self.urdfLinks)==0): - return -1 - - base = self.urdfLinks[0] - - #v.tmp_collision_shape_ids=[] - baseMass = base.urdf_inertial.mass - print("baseMass=",baseMass) - baseCollisionShapeIndex = -1 - - baseShapeTypeArray=[] - baseRadiusArray=[] - baseHalfExtentsArray=[] - lengthsArray=[] - fileNameArray=[] - meshScaleArray=[] - basePositionsArray=[] - baseOrientationsArray=[] - - for v in base.urdf_collision_shapes: - print("base v.origin_xyz=",v.origin_xyz) - print("base v.origin_rpy=",v.origin_rpy) - shapeType = v.geom_type - baseShapeTypeArray.append(shapeType) - baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) - baseRadiusArray.append(v.geom_radius) - lengthsArray.append(v.geom_length) - fileNameArray.append(v.geom_meshfilename) - meshScaleArray.append(v.geom_meshscale) - basePositionsArray.append(v.origin_xyz) - print("v.origin_rpy=",v.origin_rpy) - orn=p.getQuaternionFromEuler(v.origin_rpy) - baseOrientationsArray.append(orn) - - print("baseHalfExtentsArray=",baseHalfExtentsArray) - if (len(baseShapeTypeArray)): - baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, - radii=baseRadiusArray, - halfExtents=baseHalfExtentsArray, - lengths=lengthsArray, - fileNames=fileNameArray, - meshScales=meshScaleArray, - collisionFramePositions=basePositionsArray, - collisionFrameOrientations=baseOrientationsArray, - physicsClientId=physicsClientId) - - - print("baseCollisionShapeIndex=",baseCollisionShapeIndex) - - - linkMasses=[] - linkCollisionShapeIndices=[] - linkVisualShapeIndices=[] - linkPositions=[] - linkOrientations=[] - linkMeshScaleArray=[] - linkInertialFramePositions=[] - linkInertialFrameOrientations=[] - linkParentIndices=[] - linkJointTypes=[] - linkJointAxis=[] - - for joint in self.urdfJoints: - link = joint.link - linkMass = link.urdf_inertial.mass - - print("linkMass=",linkMass) - - linkCollisionShapeIndex=-1 - linkVisualShapeIndex=-1 - linkPosition=[0,0,0] - linkOrientation=[0,0,0] - linkInertialFramePosition=[0,0,0] - linkInertialFrameOrientation=[0,0,0] - linkParentIndex=self.linkNameToIndex[joint.parent_name] - print("parentId=",linkParentIndex) - - linkJointType=joint.joint_type - print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType] ) - - linkJointAx = joint.joint_axis_xyz - - linkShapeTypeArray=[] - linkRadiusArray=[] - linkHalfExtentsArray=[] - lengthsArray=[] - fileNameArray=[] - linkPositionsArray=[] - linkOrientationsArray=[] - - for v in link.urdf_collision_shapes: - print("link v.origin_xyz=",v.origin_xyz) - print("link v.origin_rpy=",v.origin_rpy) - shapeType = v.geom_type - linkShapeTypeArray.append(shapeType) - linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) - linkRadiusArray.append(v.geom_radius) - lengthsArray.append(v.geom_length) - fileNameArray.append(v.geom_meshfilename) - linkMeshScaleArray.append(v.geom_meshscale) - linkPositionsArray.append(v.origin_xyz) - linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) - - print("linkHalfExtentsArray=",linkHalfExtentsArray) - if (len(linkShapeTypeArray)): - linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, - radii=linkRadiusArray, - halfExtents=linkHalfExtentsArray, - lengths=lengthsArray, - fileNames=fileNameArray, - meshScales=linkMeshScaleArray, - collisionFramePositions=linkPositionsArray, - collisionFrameOrientations=linkOrientationsArray, - physicsClientId=physicsClientId) - - linkMasses.append(linkMass) - linkCollisionShapeIndices.append(linkCollisionShapeIndex) - linkVisualShapeIndices.append(linkVisualShapeIndex) - linkPositions.append(joint.joint_origin_xyz) - linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) - linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) - linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) - linkParentIndices.append(linkParentIndex) - linkJointTypes.append(joint.joint_type) - linkJointAxis.append(joint.joint_axis_xyz) - print("\n\n\nlinkMasses=",linkMasses) - obUid = p.createMultiBody(baseMass,\ - baseCollisionShapeIndex= baseCollisionShapeIndex, - basePosition=basePosition, - baseInertialFramePosition=base.urdf_inertial.origin_xyz, - baseInertialFrameOrientation=base.urdf_inertial.origin_rpy, - linkMasses=linkMasses, - linkCollisionShapeIndices=linkCollisionShapeIndices, - linkVisualShapeIndices=linkVisualShapeIndices, - linkPositions=linkPositions, - linkOrientations=linkOrientations, - linkInertialFramePositions=linkInertialFramePositions, - linkInertialFrameOrientations=linkInertialFrameOrientations, - linkParentIndices=linkParentIndices, - linkJointTypes=linkJointTypes, - linkJointAxis=linkJointAxis, - physicsClientId=physicsClientId) - return obUid - #create visual and collision shapes - #for link in self.urdfLinks: - # for v in link.urdf_visual_shapes: - # v.tmp_visual_shape_ids=[] - # shapeType = v[0] - # if shapeType==p.GEOM_BOX: - # p.createVisualShape(physicsClientId=physId) - - - #for joint in self.urdfJoints: - # self.writeJoint(file,joint) - - - - def __del__(self): - pass - ########################################## @@ -535,7 +32,7 @@ for i in range(p.getNumJoints(mb,physicsClientId=org)): # print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org)) # print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org)) -parser = UrdfEditor() +parser = urdfEditor.UrdfEditor() parser.initializeFromBulletBody(mb,physicsClientId=org) parser.saveUrdf("test.urdf") @@ -544,7 +41,7 @@ if (1): obUid = parser.createMultiBody(physicsClientId=gui) - parser2 = UrdfEditor() + parser2 = urdfEditor.UrdfEditor() print("\n###########################################\n") parser2.initializeFromBulletBody(obUid,physicsClientId=gui)