From e488603603b755436c3e6bf2af8f310f7cbc6d71 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 10 May 2018 18:05:50 -0700 Subject: [PATCH 1/2] UrdfEditor: fix indentation, causing an issue when a link had no collision shape but visual shapes --- .../pybullet/gym/pybullet_utils/urdfEditor.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 61416e591..b29ce6c60 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -474,7 +474,7 @@ class UrdfEditor(object): #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, @@ -513,7 +513,7 @@ class UrdfEditor(object): linkMeshScaleArray=[] linkPositionsArray=[] linkOrientationsArray=[] - + for v in link.urdf_collision_shapes: shapeType = v.geom_type linkShapeTypeArray.append(shapeType) @@ -536,22 +536,22 @@ class UrdfEditor(object): collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) - urdfVisuals = link.urdf_visual_shapes - 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 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=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] + urdfVisuals = link.urdf_visual_shapes + 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 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=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] - if (len(shapeTypes)): - print("fileNames=",fileNames) + if (len(shapeTypes)): + print("fileNames=",fileNames) - linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes, + linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes, halfExtents=halfExtents,radii=radii,lengths=lengths, fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors, visualFramePositions=visualFramePositions, From 06a23ecd6df76147c9a2d03906c2bbce375f7083 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 10 May 2018 18:37:48 -0700 Subject: [PATCH 2/2] PyBullet TinyRenderer, fix cylinder (don't add caps like a capsule), also convert colors stored in visual shapes, if needed. --- .../TinyRendererVisualShapeConverter.cpp | 33 ++++++++++++++----- .../pybullet_utils/examples/combineUrdf.py | 3 +- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 90ddbb4d8..11f207132 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -243,10 +243,13 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi vertices.push_back(vert); } } - btVector3 pole1 = p1 - dir * rad; - btVector3 pole2 = p2 + dir * rad; - vertices.push_back(pole1); - vertices.push_back(pole2); + if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) + { + btVector3 pole1 = p1 - dir * rad; + btVector3 pole2 = p2 + dir * rad; + vertices.push_back(pole1); + vertices.push_back(pole2); + } } else { //assume a capsule along the Z-axis, centered at the origin @@ -260,10 +263,13 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi vert[2] = -len / 2.; vertices.push_back(vert); } - btVector3 pole1(0, 0, + len / 2. + rad); - btVector3 pole2(0, 0, - len / 2. - rad); - vertices.push_back(pole1); - vertices.push_back(pole2); + if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) + { + btVector3 pole1(0, 0, + len / 2. + rad); + btVector3 pole2(0, 0, - len / 2. - rad); + vertices.push_back(pole1); + vertices.push_back(pole2); + } } visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1]; @@ -612,6 +618,17 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); //m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); + } else + { + ///programmatic created models may have the color in the visual + if (vis && vis->m_geometry.m_hasLocalMaterial) + { + for (int i = 0; i < 4; i++) + { + rgbaColor[i] = vis->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[i]; + } + } + } } diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py index 09066451a..77d71ad0a 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py @@ -32,6 +32,7 @@ jointPivotXYZInChild = [0,0,0] jointPivotRPYInChild = [0,0,0] newjoint = ed0.joinUrdf(ed1, parentLinkIndex , jointPivotXYZInParent, jointPivotRPYInParent, jointPivotXYZInChild, jointPivotRPYInChild, p0._client, p1._client) +newjoint.joint_type = p0.JOINT_FIXED ed0.saveUrdf("combined.urdf") @@ -47,5 +48,5 @@ ed0.createMultiBody([0,0,0],orn, pgui._client) pgui.setRealTimeSimulation(1) while (pgui.isConnected()): - pgui.getCameraImage(320,200) + pgui.getCameraImage(320,200, renderer=pgui.ER_BULLET_HARDWARE_OPENGL) time.sleep(1./240.)