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

tweaks in urdfEditor.py and combineUrdf.py, put KUKA on top of Husky as test.

Browse Source
This commit is contained in:
Erwin Coumans
2018-05-08 23:14:39 -07:00
parent 2d1689cba5
commit 1e51e2b859
5 changed files with 83 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,6 +2,7 @@ import pybullet_utils.bullet_client as bc
import pybullet_utils.urdfEditor as ed import pybullet_utils.urdfEditor as ed
import pybullet import pybullet
import pybullet_data import pybullet_data
import time
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT) p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath()) p0.setAdditionalSearchPath(pybullet_data.getDataPath())
@@ -10,18 +11,20 @@ p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath()) p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER #can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
#pgui = bc.BulletClient(connection_mode=pybullet.GUI)
frame = p1.loadURDF("frame1.urdf") husky = p1.loadURDF("husky/husky.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
door = p0.loadURDF("door1.urdf") kuka = p0.loadURDF("kuka_iiwa/model_free_base.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
ed0 = ed.UrdfEditor() ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(frame, p1._client) ed0.initializeFromBulletBody(husky, p1._client)
ed1 = ed.UrdfEditor() ed1 = ed.UrdfEditor()
ed1.initializeFromBulletBody(door, p0._client) ed1.initializeFromBulletBody(kuka, p0._client)
#ed1.saveUrdf("combined.urdf")
parentLinkIndex = 0 parentLinkIndex = 0
childLinkIndex = len(ed0.urdfLinks) childLinkIndex = len(ed0.urdfLinks)
insertJointIndex = len(ed0.urdfJoints)
#combine all links, and add a joint #combine all links, and add a joint
for link in ed1.urdfLinks: for link in ed1.urdfLinks:
@@ -35,7 +38,7 @@ for joint in ed1.urdfJoints:
jointPivotXYZInParent = [0.1,0,0.1] jointPivotXYZInParent = [0.1,0,0.1]
jointPivotRPYInParent = [0,0,0] jointPivotRPYInParent = [0,0,0]
jointPivotXYZInChild = [-0.4,0,-0.4] jointPivotXYZInChild = [0,0,0]
jointPivotRPYInChild = [0,0,0] jointPivotRPYInChild = [0,0,0]
jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild) jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild) invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
@@ -76,7 +79,11 @@ joint.joint_origin_rpy = jointPivotRPYInParent
joint.joint_axis_xyz = [0,0,1] joint.joint_axis_xyz = [0,0,1]
ed0.urdfJoints.append(joint) #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)
ed0.saveUrdf("combined.urdf") ed0.saveUrdf("combined.urdf")
@@ -85,3 +92,10 @@ print(p1._client)
print("p0.getNumBodies()=",p0.getNumBodies()) print("p0.getNumBodies()=",p0.getNumBodies())
print("p1.getNumBodies()=",p1.getNumBodies()) print("p1.getNumBodies()=",p1.getNumBodies())
pgui = bc.BulletClient(connection_mode=pybullet.GUI)
ed0.createMultiBody([0,0,0],pgui._client)
pgui.setRealTimeSimulation(1)
while (pgui.isConnected()):
time.sleep(1./240.)

9
examples/pybullet/gym/pybullet_utils/examples/combined.py
View File

@@ -1,9 +1,12 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("combined.urdf", useFixedBase=True) p.loadURDF("combined.urdf", useFixedBase=True)
for j in range (p.getNumJoints(0)):
p.setJointMotorControl2(0,j,p.VELOCITY_CONTROL,targetVelocity=0.1)
#for j in range (p.getNumJoints(0)):
# p.setJointMotorControl2(0,j,p.VELOCITY_CONTROL,targetVelocity=0.1)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while (1): while (p.isConnected()):
import time import time
time.sleep(1./240.) time.sleep(1./240.)

29
examples/pybullet/gym/pybullet_utils/examples/door1.urdf
View File

@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="urdf_door">
<link name="door">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.8 0.05 0.8"/>
</geometry>
<material name="doormat0">
<color rgba="0.8 0.8 0.3 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.8 0.05 0.8"/>
</geometry>
</collision>
</link>
</robot>

69
examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
View File

@@ -1,69 +0,0 @@
<?xml version="0.0" ?>
<robot name="urdf_door">
<link name="frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.35"/>
<mass value="1.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.05 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="framemat0">
<color
rgba="0.9 0.4 0. 1" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.95 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.5 0 0.95"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.5 0 0.05"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
<material name="framemat0"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.05 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.95 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.5 0 0.95"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.5 0 0.05"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
</collision>
</link>
</robot>

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

@@ -308,6 +308,11 @@ class UrdfEditor(object):
file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n") file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\ str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision) urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
str = '\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_rpy[0],\
urdfJoint.joint_origin_rpy[1],urdfJoint.joint_origin_rpy[2],\
urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
file.write(str) file.write(str)
str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\ str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision) urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
@@ -337,6 +342,10 @@ class UrdfEditor(object):
if (len(self.urdfLinks)==0): if (len(self.urdfLinks)==0):
return -1 return -1
#for i in range (len(self.urdfLinks)):
# print("link", i, "=",self.urdfLinks[i].link_name)
base = self.urdfLinks[0] base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[] #v.tmp_collision_shape_ids=[]
@@ -364,6 +373,7 @@ class UrdfEditor(object):
baseOrientationsArray.append(orn) baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)): if (len(baseShapeTypeArray)):
#print("fileNameArray=",fileNameArray)
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray, radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray, halfExtents=baseHalfExtentsArray,
@@ -376,28 +386,34 @@ class UrdfEditor(object):
urdfVisuals = base.urdf_visual_shapes urdfVisuals = base.urdf_visual_shapes
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals], shapeTypes=[v.geom_type for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals], halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
lengths=[v.geom_length[0] for v in urdfVisuals], radii=[v.geom_radius for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals], lengths=[v.geom_length[0] for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals], fileNames=[v.geom_meshfilename for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals], meshScales=[v.geom_meshscale for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals], rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], visualFramePositions=[v.origin_xyz for v in urdfVisuals]
physicsClientId=physicsClientId) visualFrameOrientations=[v.origin_rpy for v in urdfVisuals]
# urdfVisual = base.urdf_visual_shapes[0] baseVisualShapeIndex = -1
# baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type,
# halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents],
# radius=urdfVisual.geom_radius,
# length=urdfVisual.geom_length[0],
# fileName=urdfVisual.geom_meshfilename,
# meshScale=urdfVisual.geom_meshscale,
# rgbaColor=urdfVisual.material_rgba,
# visualFramePosition=urdfVisual.origin_xyz,
# visualFrameOrientation=urdfVisual.origin_rpy,
# physicsClientId=physicsClientId)
if (len(shapeTypes)):
#print("len(shapeTypes)=",len(shapeTypes))
#print("len(halfExtents)=",len(halfExtents))
#print("len(radii)=",len(radii))
#print("len(lengths)=",len(lengths))
#print("len(fileNames)=",len(fileNames))
#print("len(meshScales)=",len(meshScales))
#print("len(rgbaColors)=",len(rgbaColors))
#print("len(visualFramePositions)=",len(visualFramePositions))
#print("len(visualFrameOrientations)=",len(visualFrameOrientations))
print("fileNames=",fileNames)
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames,
meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId)
linkMasses=[] linkMasses=[]
linkCollisionShapeIndices=[] linkCollisionShapeIndices=[]
@@ -454,15 +470,25 @@ class UrdfEditor(object):
physicsClientId=physicsClientId) physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals], linkVisualShapeIndex = -1
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals], shapeTypes=[v.geom_type for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals], halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
lengths=[v.geom_length[0] for v in urdfVisuals], radii=[v.geom_radius for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals], lengths=[v.geom_length[0] for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals], fileNames=[v.geom_meshfilename for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals], meshScales=[v.geom_meshscale for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals], rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals]
if (len(shapeTypes)):
print("fileNames=",fileNames)
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,
fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors,
visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,
physicsClientId=physicsClientId) physicsClientId=physicsClientId)
linkMasses.append(linkMass) linkMasses.append(linkMass)