diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
index e80c58073..a764341ce 100644
--- a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
+++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
@@ -2,6 +2,7 @@ import pybullet_utils.bullet_client as bc
import pybullet_utils.urdfEditor as ed
import pybullet
import pybullet_data
+import time
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
@@ -10,18 +11,20 @@ p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#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")
-door = p0.loadURDF("door1.urdf")
+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)
ed0 = ed.UrdfEditor()
-ed0.initializeFromBulletBody(frame, p1._client)
+ed0.initializeFromBulletBody(husky, p1._client)
ed1 = ed.UrdfEditor()
-ed1.initializeFromBulletBody(door, p0._client)
+ed1.initializeFromBulletBody(kuka, p0._client)
+#ed1.saveUrdf("combined.urdf")
+
parentLinkIndex = 0
childLinkIndex = len(ed0.urdfLinks)
+insertJointIndex = len(ed0.urdfJoints)
#combine all links, and add a joint
for link in ed1.urdfLinks:
@@ -35,7 +38,7 @@ for joint in ed1.urdfJoints:
jointPivotXYZInParent = [0.1,0,0.1]
jointPivotRPYInParent = [0,0,0]
-jointPivotXYZInChild = [-0.4,0,-0.4]
+jointPivotXYZInChild = [0,0,0]
jointPivotRPYInChild = [0,0,0]
jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
@@ -76,7 +79,11 @@ joint.joint_origin_rpy = jointPivotRPYInParent
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")
@@ -85,3 +92,10 @@ print(p1._client)
print("p0.getNumBodies()=",p0.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.)
diff --git a/examples/pybullet/gym/pybullet_utils/examples/combined.py b/examples/pybullet/gym/pybullet_utils/examples/combined.py
index 349cc887b..f8117376b 100644
--- a/examples/pybullet/gym/pybullet_utils/examples/combined.py
+++ b/examples/pybullet/gym/pybullet_utils/examples/combined.py
@@ -1,9 +1,12 @@
import pybullet as p
p.connect(p.GUI)
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)
-while (1):
+while (p.isConnected()):
import time
time.sleep(1./240.)
diff --git a/examples/pybullet/gym/pybullet_utils/examples/door1.urdf b/examples/pybullet/gym/pybullet_utils/examples/door1.urdf
deleted file mode 100644
index 4323d70ce..000000000
--- a/examples/pybullet/gym/pybullet_utils/examples/door1.urdf
+++ /dev/null
@@ -1,29 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf b/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
deleted file mode 100644
index 1f8d2f3f2..000000000
--- a/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
+++ /dev/null
@@ -1,69 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py
index ee1a2278a..8c2325fde 100644
--- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py
+++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py
@@ -308,6 +308,11 @@ class UrdfEditor(object):
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)
+ str = '\t\t\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)
str = '\t\t\n'.format(urdfJoint.joint_axis_xyz[0],\
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
@@ -337,6 +342,10 @@ class UrdfEditor(object):
if (len(self.urdfLinks)==0):
return -1
+ #for i in range (len(self.urdfLinks)):
+ # print("link", i, "=",self.urdfLinks[i].link_name)
+
+
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
@@ -364,6 +373,7 @@ class UrdfEditor(object):
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
+ #print("fileNameArray=",fileNameArray)
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
@@ -376,28 +386,34 @@ class UrdfEditor(object):
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],
- radii=[v.geom_radius for v in urdfVisuals],
- lengths=[v.geom_length[0] 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],
- physicsClientId=physicsClientId)
-# urdfVisual = base.urdf_visual_shapes[0]
-# 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)
+
+ 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]
+ 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]
+ baseVisualShapeIndex = -1
+ 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=[]
linkCollisionShapeIndices=[]
@@ -454,15 +470,25 @@ class UrdfEditor(object):
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
- linkVisualShapeIndex = p.createVisualShapeArray(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],
- 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],
+ linkVisualShapeIndex = -1
+ 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]
+ 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]
+
+ 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)
linkMasses.append(linkMass)