diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6418648e6..fcf863956 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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;im_visualShapes.size();i++) diff --git a/examples/pybullet/gym/pybullet_utils/bullet_client.py b/examples/pybullet/gym/pybullet_utils/bullet_client.py new file mode 100644 index 000000000..8fc1fcd5c --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_utils/examples/__init__.py b/examples/pybullet/gym/pybullet_utils/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py new file mode 100644 index 000000000..a7192ce70 --- /dev/null +++ b/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.) diff --git a/examples/pybullet/gym/pybullet_utils/examples/combined.py b/examples/pybullet/gym/pybullet_utils/examples/combined.py new file mode 100644 index 000000000..3a7538442 --- /dev/null +++ b/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.) diff --git a/examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py b/examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py new file mode 100644 index 000000000..7018cfead --- /dev/null +++ b/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()) + diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index ee1a2278a..9ec226cfb 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/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\n'.format(urdfVisual.geom_meshfilename,\ + + str = '\t\t\t\t\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\n'.format(urdfCollision.geom_meshfilename,\ - prec=precision) + str = '\t\t\t\t scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"\n'.format(urdfCollision.geom_meshfilename,\ + 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\n'.format(\ @@ -308,6 +310,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 +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], - 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 +472,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)