Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

urdfEditor: add euler -> quaternion conversions (otherwise orientations get silently ignored)

Browse Source 
urdfEditor: move combine code into UrdfEditor.joinUrdf
urdfEditor: Fix geom_length
This commit is contained in:
erwincoumans
2018-05-10 15:08:00 -07:00
parent 3116cd82da
commit 80bab22b9c
2 changed files with 79 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
View File

@@ -13,7 +13,7 @@ p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
husky = p1.loadURDF("husky/husky.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
kuka = p0.loadURDF("kuka_iiwa/model_free_base.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
kuka = p0.loadURDF("kuka_iiwa/model.urdf")
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(husky, p1._client)
@@ -23,67 +23,15 @@ ed1.initializeFromBulletBody(kuka, p0._client)
parentLinkIndex = 0
childLinkIndex = len(ed0.urdfLinks)
insertJointIndex = len(ed0.urdfJoints)
#combine all links, and add a joint
for link in ed1.urdfLinks:
ed0.linkNameToIndex[link.link_name]=len(ed0.urdfLinks)
ed0.urdfLinks.append(link)
for joint in ed1.urdfJoints:
ed0.urdfJoints.append(joint)
#add a new joint between a particular
jointPivotXYZInParent = [0.1,0,0.1]
jointPivotXYZInParent = [0,0,0]
jointPivotRPYInParent = [0,0,0]
jointPivotXYZInChild = [0,0,0]
jointPivotRPYInChild = [0,0,0]
jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
#apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
#inertial
pos, orn = p0.multiplyTransforms(ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p0.getQuaternionFromEuler(ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p0.getEulerFromQuaternion(orn)
#all visual
for v in ed0.urdfLinks[childLinkIndex].urdf_visual_shapes:
pos, orn = p0.multiplyTransforms(v.origin_xyz,p0.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
v.origin_xyz = pos
v.origin_rpy = p0.getEulerFromQuaternion(orn)
#all collision
for c in ed0.urdfLinks[childLinkIndex].urdf_collision_shapes:
pos, orn = p0.multiplyTransforms(c.origin_xyz,p0.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
c.origin_xyz = pos
c.origin_rpy = p0.getEulerFromQuaternion(orn)
childLink = ed0.urdfLinks[childLinkIndex]
parentLink = ed0.urdfLinks[parentLinkIndex]
joint = ed.UrdfJoint()
joint.link = childLink
joint.joint_name = "joint_dummy1"
joint.joint_type = p0.JOINT_REVOLUTE
joint.joint_lower_limit = 0
joint.joint_upper_limit = -1
joint.parent_name = parentLink.link_name
joint.child_name = childLink.link_name
joint.joint_origin_xyz = jointPivotXYZInParent
joint.joint_origin_rpy = jointPivotRPYInParent
joint.joint_axis_xyz = [0,0,1]
#the following commented line would crash PyBullet, it messes up the joint indexing/ordering
#ed0.urdfJoints.append(joint)
#so make sure to insert the joint in the right place, to links/joints match
ed0.urdfJoints.insert(insertJointIndex,joint)
newjoint = ed0.joinUrdf(ed1, parentLinkIndex , jointPivotXYZInParent, jointPivotRPYInParent, jointPivotXYZInChild, jointPivotRPYInChild, p0._client, p1._client)
ed0.saveUrdf("combined.urdf")
@@ -94,7 +42,8 @@ print("p1.getNumBodies()=",p1.getNumBodies())
pgui = bc.BulletClient(connection_mode=pybullet.GUI)
ed0.createMultiBody([0,0,0],pgui._client)
orn=[0,0,0,1]
ed0.createMultiBody([0,0,0],orn, pgui._client)
pgui.setRealTimeSimulation(1)
while (pgui.isConnected()):

81
examples/pybullet/gym/pybullet_utils/urdfEditor.py
View File

@@ -29,7 +29,7 @@ class UrdfVisual(object):
self.geom_type = p.GEOM_BOX
self.geom_radius = 1
self.geom_extents = [7,8,9]
self.geom_length=[10]
self.geom_length=10
self.geom_meshfilename = "meshfile"
self.geom_meshscale=[1,1,1]
self.material_rgba = [1,0,0,1]
@@ -339,7 +339,71 @@ class UrdfEditor(object):
#def addLink(...)
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
def joinUrdf(self, childEditor, parentLinkIndex=0, jointPivotXYZInParent=[0,0,0], jointPivotRPYInParent=[0,0,0], jointPivotXYZInChild=[0,0,0], jointPivotRPYInChild=[0,0,0], parentPhysicsClientId=0, childPhysicsClientId=0):
childLinkIndex = len(self.urdfLinks)
insertJointIndex = len(self.urdfJoints)
#combine all links, and add a joint
for link in childEditor.urdfLinks:
self.linkNameToIndex[link.link_name]=len(self.urdfLinks)
self.urdfLinks.append(link)
for joint in childEditor.urdfJoints:
self.urdfJoints.append(joint)
#add a new joint between a particular
jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
#apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
#inertial
pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p.getQuaternionFromEuler(self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild, physicsClientId=parentPhysicsClientId)
self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn)
#all visual
for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes:
pos, orn = p.multiplyTransforms(v.origin_xyz,p.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
v.origin_xyz = pos
v.origin_rpy = p.getEulerFromQuaternion(orn)
#all collision
for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes:
pos, orn = p.multiplyTransforms(c.origin_xyz,p.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
c.origin_xyz = pos
c.origin_rpy = p.getEulerFromQuaternion(orn)
childLink = self.urdfLinks[childLinkIndex]
parentLink = self.urdfLinks[parentLinkIndex]
joint = UrdfJoint()
joint.link = childLink
joint.joint_name = "joint_dummy1"
joint.joint_type = p.JOINT_REVOLUTE
joint.joint_lower_limit = 0
joint.joint_upper_limit = -1
joint.parent_name = parentLink.link_name
joint.child_name = childLink.link_name
joint.joint_origin_xyz = jointPivotXYZInParent
joint.joint_origin_rpy = jointPivotRPYInParent
joint.joint_axis_xyz = [0,0,1]
#the following commented line would crash PyBullet, it messes up the joint indexing/ordering
#self.urdfJoints.append(joint)
#so make sure to insert the joint in the right place, to links/joints match
self.urdfJoints.insert(insertJointIndex,joint)
return joint
def createMultiBody(self, basePosition=[0,0,0],baseOrientation=[0,0,0,1], physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
@@ -392,12 +456,12 @@ class UrdfEditor(object):
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length[0] for v in urdfVisuals]
lengths=[v.geom_length for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals]
visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
baseVisualShapeIndex = -1
if (len(shapeTypes)):
@@ -477,13 +541,13 @@ class UrdfEditor(object):
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length[0] for v in urdfVisuals]
lengths=[v.geom_length for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals]
visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
if (len(shapeTypes)):
print("fileNames=",fileNames)
@@ -508,8 +572,9 @@ class UrdfEditor(object):
baseCollisionShapeIndex=baseCollisionShapeIndex,
baseVisualShapeIndex=baseVisualShapeIndex,
basePosition=basePosition,
baseOrientation=baseOrientation,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy),
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,