From 3116cd82da1ccf722ddb0dfe4b6ec7a655f5abe7 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 10 May 2018 12:04:13 -0700 Subject: [PATCH] fix MSVC 2010 compile issue urdfEditor: fix scale incorrectly saved in urdfEditor.saveUrdf urdfEditor: fix linkMeshScaleArray in wrong location --- examples/pybullet/gym/pybullet_utils/urdfEditor.py | 11 ++++++----- examples/pybullet/pybullet.c | 7 ++++--- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 9ec226cfb..48d8cb1fb 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -117,8 +117,8 @@ class UrdfEditor(object): if (v[2]==p.GEOM_SPHERE): urdfCollision.geom_radius = v[3][0] if (v[2]==p.GEOM_MESH): - urdfCollision.geom_meshfilename = v[4].decode("utf-8") - urdfCollision.geom_meshscale = v[3] + urdfCollision.geom_meshfilename = v[4].decode("utf-8") + urdfCollision.geom_meshscale = v[3] if (v[2]==p.GEOM_CYLINDER): urdfCollision.geom_length=v[3][0] urdfCollision.geom_radius=v[3][1] @@ -255,7 +255,7 @@ class UrdfEditor(object): prec=precision) file.write(str) if urdfCollision.geom_type == p.GEOM_MESH: - str = '\t\t\t\t scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"\n'.format(urdfCollision.geom_meshfilename,\ + str = '\t\t\t\t\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: @@ -422,7 +422,7 @@ class UrdfEditor(object): linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] - linkMeshScaleArray=[] + linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] @@ -446,6 +446,7 @@ class UrdfEditor(object): linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] + linkMeshScaleArray=[] linkPositionsArray=[] linkOrientationsArray=[] @@ -470,7 +471,7 @@ class UrdfEditor(object): collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) - + urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = -1 shapeTypes=[v.geom_type for v in urdfVisuals] diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 55c79c0f4..405d849ce 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -605,6 +605,10 @@ static PyObject* pybullet_syncBodyInfo(PyObject* self, PyObject* args, PyObject* b3PhysicsClientHandle sm = 0; int physicsClientId = 0; static char* kwlist[] = {"physicsClientId", NULL}; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; @@ -616,9 +620,6 @@ static PyObject* pybullet_syncBodyInfo(PyObject* self, PyObject* args, PyObject* return NULL; } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; command = b3InitSyncBodyInfoCommand(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);