Merge pull request #1678 from erwincoumans/master

PyBullet urdfEditor improvements

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2156,9 +2156,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 		UrdfModel model;// = m_data->m_urdfParser.getModel();
 		UrdfLink link;
 
-		if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
+		if (m_createBodyArgs.m_linkVisualShapeUniqueIds[urdfIndex]>=0)
 		{
-			const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
+			const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[urdfIndex]);
 			if (visHandle)
 			{
 				for (int i=0;i<visHandle->m_visualShapes.size();i++)

examples/pybullet/gym/pybullet_utils/bullet_client.py

@@ -0,0 +1,50 @@
+"""A wrapper for pybullet to manage different clients."""
+from __future__ import absolute_import
+from __future__ import division
+import functools
+import inspect
+import pybullet
+
+
+class BulletClient(object):
+  """A wrapper for pybullet to manage different clients."""
+
+  def __init__(self, connection_mode=None):
+    """Creates a Bullet client and connects to a simulation.
+
+    Args:
+      connection_mode:
+        `None` connects to an existing simulation or, if fails, creates a
+          new headless simulation,
+        `pybullet.GUI` creates a new simulation with a GUI,
+        `pybullet.DIRECT` creates a headless simulation,
+        `pybullet.SHARED_MEMORY` connects to an existing simulation.
+    """
+    self._shapes = {}
+
+    if connection_mode is None:
+      self._client = pybullet.connect(pybullet.SHARED_MEMORY)
+      if self._client >= 0:
+        return
+      else:
+        connection_mode = pybullet.DIRECT
+    self._client = pybullet.connect(connection_mode)
+
+  def __del__(self):
+    """Clean up connection if not already done."""
+    try:
+      pybullet.disconnect(physicsClientId=self._client)
+    except pybullet.error:
+      pass
+
+  def __getattr__(self, name):
+    """Inject the client id into Bullet functions."""
+    attribute = getattr(pybullet, name)
+    if inspect.isbuiltin(attribute):
+      if name not in [
+          "invertTransform", "multiplyTransforms", "getMatrixFromQuaternion",
+          "getEulerFromQuaternion", "computeViewMatrixFromYawPitchRoll",
+          "computeProjectionMatrixFOV", "getQuaternionFromEuler",
+      ]:  # A temporary hack for now.
+        attribute = functools.partial(attribute, physicsClientId=self._client)
+    return attribute

examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py

@@ -0,0 +1,102 @@
+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())
+
+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
+
+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(husky, p1._client)
+ed1 = ed.UrdfEditor()
+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:
+	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]
+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)
+
+ed0.saveUrdf("combined.urdf")
+
+print(p0._client)
+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()):
+	pgui.getCameraImage(320,200)
+	time.sleep(1./240.)

examples/pybullet/gym/pybullet_utils/examples/combined.py

@@ -0,0 +1,13 @@
+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)
+p.setRealTimeSimulation(1)
+while (p.isConnected()):
+	p.getCameraImage(320,200)
+	import time
+	time.sleep(1./240.)

examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py

@@ -0,0 +1,20 @@
+import pybullet_utils.bullet_client as bc
+import pybullet
+import pybullet_data
+
+p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
+p0.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+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)
+
+p0.loadURDF("r2d2.urdf")
+p1.loadSDF("stadium.sdf")
+print(p0._client)
+print(p1._client)
+print("p0.getNumBodies()=",p0.getNumBodies())
+print("p1.getNumBodies()=",p1.getNumBodies())
+

examples/pybullet/gym/pybullet_utils/urdfEditor.py

@@ -216,7 +216,9 @@ class UrdfEditor(object):
 				prec=precision)
 			file.write(str)
 		if urdfVisual.geom_type == p.GEOM_MESH:
-			str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfilename,\
+			
+			str = '\t\t\t\t<mesh filename=\"{}\" scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(
+			urdfVisual.geom_meshfilename,urdfVisual.geom_meshscale[0],urdfVisual.geom_meshscale[1],urdfVisual.geom_meshscale[2],\
 				prec=precision)
 			file.write(str)
 		if urdfVisual.geom_type == p.GEOM_CYLINDER:
@@ -253,8 +255,8 @@ class UrdfEditor(object):
 				prec=precision)
 			file.write(str)
 		if urdfCollision.geom_type == p.GEOM_MESH:
-			str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfilename,\
+			str = '\t\t\t\t<mesh filename=\"{}\"/> scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"\n'.format(urdfCollision.geom_meshfilename,\
-				prec=precision)
+				urdfCollision.geom_meshscale[0],urdfCollision.geom_meshscale[1],urdfCollision.geom_meshscale[2],prec=precision)
 			file.write(str)
 		if urdfCollision.geom_type == p.GEOM_CYLINDER:
 			str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
@@ -308,6 +310,11 @@ class UrdfEditor(object):
 		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],\
 			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)
 		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)
@@ -337,6 +344,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 +375,7 @@ class UrdfEditor(object):
 			baseOrientationsArray.append(orn)
 
 		if (len(baseShapeTypeArray)):
+			#print("fileNameArray=",fileNameArray)
 			baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
 					radii=baseRadiusArray,
 					halfExtents=baseHalfExtentsArray,
@@ -376,28 +388,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],
+		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=[]
 		linkCollisionShapeIndices=[]
@@ -454,15 +472,25 @@ class UrdfEditor(object):
 					physicsClientId=physicsClientId)
 
 				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)
 
 			linkMasses.append(linkMass)