From ad43de285caf2a9e0d8d9ed542a347e422da6161 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 8 May 2018 10:12:19 -0700 Subject: [PATCH 1/6] add bullet_client.py to pybullet_utils, with simple example --- .../gym/pybullet_utils/bullet_client.py | 50 +++++++++++++++++++ .../pybullet_utils/examples/multipleScenes.py | 20 ++++++++ 2 files changed, 70 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_utils/bullet_client.py create mode 100644 examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py 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/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()) + From d436be1d79057f1c5b149611b31268058a4aefe2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 8 May 2018 13:31:30 -0700 Subject: [PATCH 2/6] add __init__.py for pybullet_utils examples --- examples/pybullet/gym/pybullet_utils/examples/__init__.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_utils/examples/__init__.py 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 From 2d1689cba5394e67c49e0484470e15731b1d9ce3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 8 May 2018 17:18:58 -0700 Subject: [PATCH 3/6] add example to joint two URDF files, using the urdfEditor (combineUrdf.py) --- .../pybullet_utils/examples/combineUrdf.py | 87 +++++++++++++++++++ .../gym/pybullet_utils/examples/combined.py | 9 ++ .../gym/pybullet_utils/examples/door1.urdf | 29 +++++++ .../gym/pybullet_utils/examples/frame1.urdf | 69 +++++++++++++++ 4 files changed, 194 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py create mode 100644 examples/pybullet/gym/pybullet_utils/examples/combined.py create mode 100644 examples/pybullet/gym/pybullet_utils/examples/door1.urdf create mode 100644 examples/pybullet/gym/pybullet_utils/examples/frame1.urdf 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..e80c58073 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py @@ -0,0 +1,87 @@ +import pybullet_utils.bullet_client as bc +import pybullet_utils.urdfEditor as ed +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) + +frame = p1.loadURDF("frame1.urdf") +door = p0.loadURDF("door1.urdf") + +ed0 = ed.UrdfEditor() +ed0.initializeFromBulletBody(frame, p1._client) +ed1 = ed.UrdfEditor() +ed1.initializeFromBulletBody(door, p0._client) + +parentLinkIndex = 0 +childLinkIndex = len(ed0.urdfLinks) + +#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.4,0,-0.4] +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] + + +ed0.urdfJoints.append(joint) + +ed0.saveUrdf("combined.urdf") + +print(p0._client) +print(p1._client) +print("p0.getNumBodies()=",p0.getNumBodies()) +print("p1.getNumBodies()=",p1.getNumBodies()) + 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..349cc887b --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/combined.py @@ -0,0 +1,9 @@ +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 (1): + 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 new file mode 100644 index 000000000..4323d70ce --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/door1.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf b/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf new file mode 100644 index 000000000..1f8d2f3f2 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 1e51e2b85944865f9fd35cff168c818a098dbe92 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 8 May 2018 23:14:39 -0700 Subject: [PATCH 4/6] tweaks in urdfEditor.py and combineUrdf.py, put KUKA on top of Husky as test. --- .../pybullet_utils/examples/combineUrdf.py | 28 ++++-- .../gym/pybullet_utils/examples/combined.py | 9 +- .../gym/pybullet_utils/examples/door1.urdf | 29 ------- .../gym/pybullet_utils/examples/frame1.urdf | 69 --------------- .../pybullet/gym/pybullet_utils/urdfEditor.py | 86 ++++++++++++------- 5 files changed, 83 insertions(+), 138 deletions(-) delete mode 100644 examples/pybullet/gym/pybullet_utils/examples/door1.urdf delete mode 100644 examples/pybullet/gym/pybullet_utils/examples/frame1.urdf 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) From be7c6ac967a106c43fd3e253371b2119453f788b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 8 May 2018 23:23:53 -0700 Subject: [PATCH 5/6] PyBullet add getCameraImage to debug urdfEditor/createMultiBody --- examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py | 1 + examples/pybullet/gym/pybullet_utils/examples/combined.py | 1 + 2 files changed, 2 insertions(+) diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py index a764341ce..a7192ce70 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py @@ -98,4 +98,5 @@ 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 index f8117376b..3a7538442 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combined.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combined.py @@ -8,5 +8,6 @@ p.loadURDF("combined.urdf", useFixedBase=True) # 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.) From 414b3ef9bb296df814b524a77969868f3fecff88 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 9 May 2018 10:28:12 -0700 Subject: [PATCH 6/6] PyBullet urdfEditor improvements: saveUrdf mesh scale, fix incorrect index, add getCameraImage in examples/combineUrdf.py --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 4 ++-- .../pybullet/gym/pybullet_utils/examples/combineUrdf.py | 8 ++++++-- examples/pybullet/gym/pybullet_utils/examples/combined.py | 1 + examples/pybullet/gym/pybullet_utils/urdfEditor.py | 8 +++++--- 4 files changed, 14 insertions(+), 7 deletions(-) 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/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py index a764341ce..5ce0a3d14 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py @@ -85,17 +85,21 @@ joint.joint_axis_xyz = [0,0,1] #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) +ed0.saveUrdf("combined.urdf") + + while (pgui.isConnected()): - time.sleep(1./240.) + pgui.getCameraImage(320,200) + time.sleep(1./240.) \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/examples/combined.py b/examples/pybullet/gym/pybullet_utils/examples/combined.py index f8117376b..3a7538442 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combined.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combined.py @@ -8,5 +8,6 @@ p.loadURDF("combined.urdf", useFixedBase=True) # 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/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 8c2325fde..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(\