From 414b3ef9bb296df814b524a77969868f3fecff88 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 9 May 2018 10:28:12 -0700 Subject: [PATCH] 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(\