allow to use colors from MJCF file as option (default to random Google colors), use p.loadMJCF(filename, flags=p.URDF_MJCF_COLORS_FROM_FILE

fix quadruped.py example.
add PyBullet.isConnected() API, more friendly than PyBullet.getConnectionInfo()["connected"]

examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp

@@ -139,7 +139,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
 				btConvexShape* convex = (btConvexShape*)collisionShape;
 				{
 					btShapeHull* hull = new btShapeHull(convex);
-					hull->buildHull(0.0);
+					hull->buildHull(0.0, 1);
 
 					{
 						//int strideInBytes = 9*sizeof(float);

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -301,6 +301,15 @@ void OpenGLGuiHelper::changeTexture(int textureUniqueId, const unsigned char* rg
 
 int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
 {
+	if (textureId == -2)
+	{
+		if (m_data->m_checkedTextureGrey<0)
+		{
+			m_data->m_checkedTextureGrey = createCheckeredTexture(192, 192, 192);
+		}
+		textureId = m_data->m_checkedTextureGrey;
+	}
+
 	int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices,primitiveType, textureId);
 	return shapeId;
 }

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -2,7 +2,10 @@
 #include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
 #include "Bullet3Common/b3FileUtils.h"
 #include "Bullet3Common/b3HashMap.h"
-
+#include "LinearMath/btQuickprof.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"
+#include "../../CommonInterfaces/CommonRenderInterface.h"
+#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
 #include <string>
 #include "../../Utils/b3ResourcePath.h"
 #include <iostream>
@@ -13,6 +16,9 @@
 #include "../ImportURDFDemo/urdfLexicalCast.h"
 #include "../ImportObjDemo/LoadMeshFromObj.h"
 #include "../ImportSTLDemo/LoadMeshFromSTL.h"
+#include "../ImportColladaDemo/LoadMeshFromCollada.h"
+#include "../OpenGLWindow/ShapeData.h"
+
 #include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
 #include "../ImportMeshUtility/b3ImportMeshUtility.h"
 
@@ -31,6 +37,20 @@
 
 
 
+#define mjcf_sphere_indiced textured_detailed_sphere_indices
+#define mjcf_sphere_vertices textured_detailed_sphere_vertices
+
+
+
+static btVector4 sGoogleColors[4] =
+{
+	btVector4(60. / 256., 186. / 256., 84. / 256., 1),
+	btVector4(244. / 256., 194. / 256., 13. / 256., 1),
+	btVector4(219. / 256., 50. / 256., 54. / 256., 1),
+	btVector4(72. / 256., 133. / 256., 237. / 256., 1),
+};
+
+
 #include <vector>
 
 enum ePARENT_LINK_ENUMS
@@ -192,12 +212,14 @@ struct BulletMJCFImporterInternalData
 	mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
 
 	int m_flags;
+	int m_textureId;
 
 	BulletMJCFImporterInternalData()
 		:m_inertiaFromGeom(true),
 		m_activeModel(-1),
 		m_activeBodyUniqueId(-1),
-		m_flags(0)
+		m_flags(0),
+		m_textureId(-1)
 	{
 		m_pathPrefix[0] = 0;
 	}
@@ -705,6 +727,12 @@ struct BulletMJCFImporterInternalData
 			linkPtr->m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
 		}
 
+		{
+			geom.m_localMaterial.m_matColor.m_rgbaColor = sGoogleColors[linkIndex & 3];
+			geom.m_localMaterial.m_matColor.m_specularColor.setValue(1, 1, 1);
+			geom.m_hasLocalMaterial = true;
+		}
+
 		std::string rgba = defaults.m_defaultGeomRgba;
 		if (const char* rgbattr = link_xml->Attribute("rgba"))
 		{
@@ -713,11 +741,16 @@ struct BulletMJCFImporterInternalData
 		if (!rgba.empty())
 		{
 			// "0 0.7 0.7 1"
-			parseVector4(geom.m_localMaterial.m_matColor.m_rgbaColor, rgba);
-			geom.m_hasLocalMaterial = true;
-			geom.m_localMaterial.m_name = rgba;
+			if ((m_flags&CUF_MJCF_COLORS_FROM_FILE))
+			{
+				parseVector4(geom.m_localMaterial.m_matColor.m_rgbaColor, rgba);
+				geom.m_hasLocalMaterial = true;
+				geom.m_localMaterial.m_name = rgba;
+			}
 		}
 
+		
+
 		const char* posS = link_xml->Attribute("pos");
 		if (posS)
 		{
@@ -880,31 +913,43 @@ struct BulletMJCFImporterInternalData
 			if (handledGeomType)
 			{
 						
-				UrdfCollision col;
-				col.m_flags |= URDF_HAS_COLLISION_GROUP;
-				col.m_collisionGroup = defaults.m_defaultCollisionGroup;
-				
-				col.m_flags |= URDF_HAS_COLLISION_MASK;
-				col.m_collisionMask = defaults.m_defaultCollisionMask;
-				
-				//contype, conaffinity 
-				const char* conTypeStr = link_xml->Attribute("contype");
-				if (conTypeStr)
 				{
+					UrdfCollision col;
 					col.m_flags |= URDF_HAS_COLLISION_GROUP;
-					col.m_collisionGroup = urdfLexicalCast<int>(conTypeStr);
-				}
-				const char* conAffinityStr = link_xml->Attribute("conaffinity");
-				if (conAffinityStr)
-				{
+					col.m_collisionGroup = defaults.m_defaultCollisionGroup;
+
 					col.m_flags |= URDF_HAS_COLLISION_MASK;
-					col.m_collisionMask = urdfLexicalCast<int>(conAffinityStr);
+					col.m_collisionMask = defaults.m_defaultCollisionMask;
+
+					//contype, conaffinity 
+					const char* conTypeStr = link_xml->Attribute("contype");
+					if (conTypeStr)
+					{
+						col.m_flags |= URDF_HAS_COLLISION_GROUP;
+						col.m_collisionGroup = urdfLexicalCast<int>(conTypeStr);
+					}
+					const char* conAffinityStr = link_xml->Attribute("conaffinity");
+					if (conAffinityStr)
+					{
+						col.m_flags |= URDF_HAS_COLLISION_MASK;
+						col.m_collisionMask = urdfLexicalCast<int>(conAffinityStr);
+					}
+
+					col.m_geometry = geom;
+					col.m_linkLocalFrame = linkLocalFrame;
+					col.m_sourceFileLocation = sourceFileLocation(link_xml);
+					linkPtr->m_collisionArray.push_back(col);
+				}
+				{
+					UrdfVisual vis;
+					vis.m_geometry = geom;
+					vis.m_linkLocalFrame = linkLocalFrame;
+					vis.m_sourceFileLocation = sourceFileLocation(link_xml);
+					linkPtr->m_visualArray.push_back(vis);
+
 				}
 
-				col.m_geometry = geom;
-				col.m_linkLocalFrame = linkLocalFrame;
-				col.m_sourceFileLocation = sourceFileLocation(link_xml);
-				linkPtr->m_collisionArray.push_back(col);
+
 
 			} else
 			{
@@ -1380,6 +1425,7 @@ BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVi
 	m_data->m_guiHelper = helper;
 	m_data->m_customVisualShapesConverter = customConverter;
 	m_data->m_flags = flags;
+	m_data->m_textureId = -1;
 }
 
 BulletMJCFImporter::~BulletMJCFImporter()
@@ -1525,6 +1571,49 @@ std::string BulletMJCFImporter::getBodyName() const
 	return m_data->m_fileModelName;
 }
 
+
+
+bool BulletMJCFImporter::getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
+{
+	bool hasLinkColor = false;
+	{
+		const UrdfLink* link = m_data->getLink(m_data->m_activeModel, linkIndex);
+		if (link)
+		{
+			for (int i = 0; i < link->m_visualArray.size(); i++)
+			{
+				if (link->m_visualArray[i].m_geometry.m_hasLocalMaterial)
+				{
+					matCol = link->m_visualArray[i].m_geometry.m_localMaterial.m_matColor;
+					hasLinkColor = true;
+					break;
+				}
+			}
+
+			if (!hasLinkColor)
+			{
+				for (int i = 0; i < link->m_collisionArray.size(); i++)
+				{
+					if (link->m_collisionArray[i].m_geometry.m_hasLocalMaterial)
+					{
+						matCol = link->m_collisionArray[0].m_geometry.m_localMaterial.m_matColor;
+						hasLinkColor = true;
+					}
+					break;
+				}
+			}
+		}
+	}
+
+	if (!hasLinkColor)
+	{
+		matCol.m_rgbaColor = sGoogleColors[linkIndex & 3];
+		matCol.m_specularColor.setValue(1, 1, 1);
+		hasLinkColor = true;
+	}
+	return hasLinkColor;
+}
+
 bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
 {
 //	UrdfLink* link = m_data->getLink(linkIndex);
@@ -1664,9 +1753,512 @@ bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
 	return true;
 }
 
+void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const
+{
+
+
+	GLInstanceGraphicsShape* glmesh = 0;
+	int strideInBytes = 9 * sizeof(float);
+
+	btConvexShape* convexColShape = 0;
+
+	switch (visual->m_geometry.m_type)
+	{
+	case URDF_GEOM_CAPSULE:
+	{
+		
+#if 1
+
+		btScalar height = visual->m_geometry.m_capsuleHeight;
+
+		btTransform capsuleTrans;
+		capsuleTrans.setIdentity();
+		if (visual->m_geometry.m_hasFromTo)
+		{
+			btVector3 f = visual->m_geometry.m_capsuleFrom;
+			btVector3 t = visual->m_geometry.m_capsuleTo;
+
+			//compute the local 'fromto' transform
+			btVector3 localPosition = btScalar(0.5)*(t + f);
+			btQuaternion localOrn;
+			localOrn = btQuaternion::getIdentity();
+
+			btVector3 diff = t - f;
+			btScalar lenSqr = diff.length2();
+			height = 0.f;
+
+			if (lenSqr > SIMD_EPSILON)
+			{
+				height = btSqrt(lenSqr);
+				btVector3 ax = diff / height;
+
+				btVector3 zAxis(0, 0, 1);
+				localOrn = shortestArcQuat(zAxis, ax);
+			}
+			capsuleTrans.setOrigin(localPosition);
+			capsuleTrans.setRotation(localOrn);
+		}
+		
+		btScalar diam = 2.*visual->m_geometry.m_capsuleRadius;
+		b3AlignedObjectArray<GLInstanceVertex> transformedVertices;
+		int numVertices = sizeof(mjcf_sphere_vertices) / strideInBytes;
+		transformedVertices.resize(numVertices);
+		for (int i = 0; i<numVertices; i++)
+		{
+
+			btVector3 vert;
+			vert.setValue(mjcf_sphere_vertices[i * 9 + 0],
+				mjcf_sphere_vertices[i * 9 + 1],
+				mjcf_sphere_vertices[i * 9 + 2]);
+
+			btScalar halfHeight = 0.5*height;
+			btVector3 trVer = (diam*vert);
+			int up = 2; //default to z axis up for capsule
+			if (trVer[up]>0)
+				trVer[up] += halfHeight;
+			else
+				trVer[up] -= halfHeight;
+
+			trVer = capsuleTrans*trVer;
+
+			transformedVertices[i].xyzw[0] = trVer[0];
+			transformedVertices[i].xyzw[1] = trVer[1];
+			transformedVertices[i].xyzw[2] = trVer[2];
+			transformedVertices[i].xyzw[3] = 0;
+			transformedVertices[i].normal[0] = mjcf_sphere_vertices[i * 9 + 4];
+			transformedVertices[i].normal[1] = mjcf_sphere_vertices[i * 9 + 5];
+			transformedVertices[i].normal[2] = mjcf_sphere_vertices[i * 9 + 6];
+			//transformedVertices[i].uv[0] = mjcf_sphere_vertices[i * 9 + 7];
+			//transformedVertices[i].uv[1] = mjcf_sphere_vertices[i * 9 + 8];
+
+			btScalar u = btAtan2(transformedVertices[i].normal[0], transformedVertices[i].normal[2]) / (2 * SIMD_PI) + 0.5;
+			btScalar v = transformedVertices[i].normal[1] * 0.5 + 0.5;
+			transformedVertices[i].uv[0] = u;
+			transformedVertices[i].uv[1] = v;
+		}
+
+		glmesh = new GLInstanceGraphicsShape;
+		//		int index = 0;
+		glmesh->m_indices = new b3AlignedObjectArray<int>();
+		glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+		int numIndices = sizeof(mjcf_sphere_indiced) / sizeof(int);
+		for (int i = 0; i < numIndices; i++)
+		{
+			glmesh->m_indices->push_back(mjcf_sphere_indiced[i]);
+		}
+		for (int i = 0; i < transformedVertices.size(); i++)
+		{
+			glmesh->m_vertices->push_back(transformedVertices[i]);
+		}
+		glmesh->m_numIndices = numIndices;
+		glmesh->m_numvertices = transformedVertices.size();
+		glmesh->m_scaling[0] = 1;
+		glmesh->m_scaling[1] = 1;
+		glmesh->m_scaling[2] = 1;
+		glmesh->m_scaling[3] = 1;
+#else
+		if (visual->m_geometry.m_hasFromTo)
+		{
+			btVector3 f = visual->m_geometry.m_capsuleFrom;
+			btVector3 t = visual->m_geometry.m_capsuleTo;
+			btVector3 fromto[2] = { f, t };
+			btScalar radii[2] = { btScalar(visual->m_geometry.m_capsuleRadius)
+				, btScalar(visual->m_geometry.m_capsuleRadius) };
+
+			btMultiSphereShape* ms = new btMultiSphereShape(fromto, radii, 2);
+			convexColShape = ms;
+		}
+		else
+		{
+			btCapsuleShapeZ* cap = new btCapsuleShapeZ(visual->m_geometry.m_capsuleRadius,
+				visual->m_geometry.m_capsuleHeight);
+			convexColShape = cap;
+		}
+#endif
+
+		break;
+	}
+	
+	case URDF_GEOM_CYLINDER:
+	{
+		btAlignedObjectArray<btVector3> vertices;
+
+		//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+		int numSteps = 32;
+		for (int i = 0; i<numSteps; i++)
+		{
+
+			btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
+			btScalar cylLength = visual->m_geometry.m_capsuleHeight;
+
+			btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
+			vertices.push_back(vert);
+			vert[2] = -cylLength / 2.;
+			vertices.push_back(vert);
+		}
+
+		btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+		cylZShape->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin);
+		cylZShape->recalcLocalAabb();
+		convexColShape = cylZShape;
+		break;
+	}
+
+	case URDF_GEOM_BOX:
+	{
+		btVector3 extents = visual->m_geometry.m_boxSize;
+		btBoxShape* boxShape = new btBoxShape(extents*0.5f);
+		//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
+		convexColShape = boxShape;
+		convexColShape->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin);
+		break;
+	}
+
+	case URDF_GEOM_SPHERE:
+	{
+#if 1
+		btScalar sphereSize = 2.*visual->m_geometry.m_sphereRadius;
+		b3AlignedObjectArray<GLInstanceVertex> transformedVertices;
+		int numVertices = sizeof(mjcf_sphere_vertices) / strideInBytes;
+		transformedVertices.resize(numVertices);
+		
+
+		glmesh = new GLInstanceGraphicsShape;
+		//		int index = 0;
+		glmesh->m_indices = new b3AlignedObjectArray<int>();
+		glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+		printf("vertices:\n");
+		for (int i = 0; i<numVertices; i++)
+		{
+			btVector3 vert;
+			vert.setValue(mjcf_sphere_vertices[i * 9 + 0],
+				mjcf_sphere_vertices[i * 9 + 1],
+				mjcf_sphere_vertices[i * 9 + 2]);
+
+			btVector3 trVer = sphereSize*vert;
+			transformedVertices[i].xyzw[0] = trVer[0];
+			transformedVertices[i].xyzw[1] = trVer[1];
+			transformedVertices[i].xyzw[2] = trVer[2];
+			transformedVertices[i].xyzw[3] = 0;
+			transformedVertices[i].normal[0] = mjcf_sphere_vertices[i * 9 + 4];
+			transformedVertices[i].normal[1] = mjcf_sphere_vertices[i * 9 + 5];
+			transformedVertices[i].normal[2] = mjcf_sphere_vertices[i * 9 + 6];
+			//transformedVertices[i].uv[0] = mjcf_sphere_vertices[i * 9 + 7];
+			//transformedVertices[i].uv[1] = mjcf_sphere_vertices[i * 9 + 8];
+
+			btScalar u = btAtan2(transformedVertices[i].normal[0], transformedVertices[i].normal[2]) / (2 * SIMD_PI) + 0.5;
+			btScalar v = transformedVertices[i].normal[1] * 0.5 + 0.5;
+			transformedVertices[i].uv[0] = u;
+			transformedVertices[i].uv[1] = v;
+
+		}
+		int numIndices = sizeof(mjcf_sphere_indiced) / sizeof(int);
+		for (int i = 0; i < numIndices; i++)
+		{
+			glmesh->m_indices->push_back(mjcf_sphere_indiced[i]);
+		}
+		for (int i = 0; i < transformedVertices.size(); i++)
+		{
+			glmesh->m_vertices->push_back(transformedVertices[i]);
+		}
+		glmesh->m_numIndices = numIndices;
+		glmesh->m_numvertices = transformedVertices.size();
+		glmesh->m_scaling[0] = 1;
+		glmesh->m_scaling[1] = 1;
+		glmesh->m_scaling[2] = 1;
+		glmesh->m_scaling[3] = 1;
+
+#else
+		
+		btScalar radius = visual->m_geometry.m_sphereRadius;
+		btSphereShape* sphereShape = new btSphereShape(radius);
+		convexColShape = sphereShape;
+		convexColShape->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin);
+#endif
+		break;
+	}
+
+	case URDF_GEOM_MESH:
+	{
+		switch (visual->m_geometry.m_meshFileType)
+		{
+		case UrdfGeometry::FILE_OBJ:
+		{
+			b3ImportMeshData meshData;
+			if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
+			{
+
+				if (meshData.m_textureImage1)
+				{
+					MJCFURDFTexture texData;
+					texData.m_width = meshData.m_textureWidth;
+					texData.m_height = meshData.m_textureHeight;
+					texData.textureData1 = meshData.m_textureImage1;
+					texData.m_isCached = meshData.m_isCached;
+					texturesOut.push_back(texData);
+				}
+				glmesh = meshData.m_gfxShape;
+			}
+			break;
+		}
+
+		case UrdfGeometry::FILE_STL:
+		{
+			glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
+			break;
+		}
+
+		case UrdfGeometry::FILE_COLLADA:
+		{
+			btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+			btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+			btTransform upAxisTrans; upAxisTrans.setIdentity();
+			float unitMeterScaling = 1;
+			int upAxis = 2;
+
+			LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(),
+				visualShapes,
+				visualShapeInstances,
+				upAxisTrans,
+				unitMeterScaling,
+				upAxis);
+
+			glmesh = new GLInstanceGraphicsShape;
+			//		int index = 0;
+			glmesh->m_indices = new b3AlignedObjectArray<int>();
+			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+			for (int i = 0; i<visualShapeInstances.size(); i++)
+			{
+				ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+				GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+
+				b3AlignedObjectArray<GLInstanceVertex> verts;
+				verts.resize(gfxShape->m_vertices->size());
+
+				int baseIndex = glmesh->m_vertices->size();
+
+				for (int i = 0; i<gfxShape->m_vertices->size(); i++)
+				{
+					verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+					verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+					verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+					verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+					verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+					verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+					verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+					verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+					verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+
+				}
+
+				int curNumIndices = glmesh->m_indices->size();
+				int additionalIndices = gfxShape->m_indices->size();
+				glmesh->m_indices->resize(curNumIndices + additionalIndices);
+				for (int k = 0; k<additionalIndices; k++)
+				{
+					glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
+				}
+
+				//compensate upAxisTrans and unitMeterScaling here
+				btMatrix4x4 upAxisMat;
+				upAxisMat.setIdentity();
+				//								upAxisMat.setPureRotation(upAxisTrans.getRotation());
+				btMatrix4x4 unitMeterScalingMat;
+				unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
+				btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
+				//btMatrix4x4 worldMat = instance->m_worldTransform;
+				int curNumVertices = glmesh->m_vertices->size();
+				int additionalVertices = verts.size();
+				glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
+
+				for (int v = 0; v<verts.size(); v++)
+				{
+					btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
+					pos = worldMat*pos;
+					verts[v].xyzw[0] = float(pos[0]);
+					verts[v].xyzw[1] = float(pos[1]);
+					verts[v].xyzw[2] = float(pos[2]);
+					glmesh->m_vertices->push_back(verts[v]);
+				}
+			}
+			glmesh->m_numIndices = glmesh->m_indices->size();
+			glmesh->m_numvertices = glmesh->m_vertices->size();
+			//glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName);
+
+			break;
+		}
+		} // switch file type
+
+		if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices <= 0)
+		{
+			b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str());
+			break;
+		}
+
+		//apply the geometry scaling
+		for (int i = 0; i<glmesh->m_vertices->size(); i++)
+		{
+			glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
+			glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
+			glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
+		}
+		break;
+	}
+	case URDF_GEOM_PLANE:
+	{
+		b3Warning("No default visual for URDF_GEOM_PLANE");
+		break;
+	}
+	default:
+	{
+		b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type);
+	}
+	}
+
+	//if we have a convex, tesselate into localVertices/localIndices
+	if ((glmesh == 0) && convexColShape)
+	{
+		BT_PROFILE("convexColShape");
+
+		btShapeHull* hull = new btShapeHull(convexColShape);
+		hull->buildHull(0.0);
+		{
+			//	int strideInBytes = 9*sizeof(float);
+			int numVertices = hull->numVertices();
+			int numIndices = hull->numIndices();
+
+
+			glmesh = new GLInstanceGraphicsShape;
+			//	int index = 0;
+			glmesh->m_indices = new b3AlignedObjectArray<int>();
+			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+
+			
+
+			for (int i = 0; i < numVertices; i++)
+			{
+				GLInstanceVertex vtx;
+				btVector3 pos = hull->getVertexPointer()[i];
+				vtx.xyzw[0] = pos.x();
+				vtx.xyzw[1] = pos.y();
+				vtx.xyzw[2] = pos.z();
+				vtx.xyzw[3] = 1.f;
+				btVector3 normal = pos.normalized();
+				vtx.normal[0] = normal.x();
+				vtx.normal[1] = normal.y();
+				vtx.normal[2] = normal.z();
+				btScalar u = btAtan2(normal[0], normal[2]) / (2 * SIMD_PI) + 0.5;
+				btScalar v = normal[1] * 0.5 + 0.5;
+				vtx.uv[0] = u;
+				vtx.uv[1] = v;
+				glmesh->m_vertices->push_back(vtx);
+			}
+
+			btAlignedObjectArray<int> indices;
+			for (int i = 0; i < numIndices; i++)
+			{
+				glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
+			}
+
+			glmesh->m_numvertices = glmesh->m_vertices->size();
+			glmesh->m_numIndices = glmesh->m_indices->size();
+		}
+		delete hull;
+		delete convexColShape;
+		convexColShape = 0;
+
+	}
+
+	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
+	{
+		BT_PROFILE("glmesh");
+		int baseIndex = verticesOut.size();
+
+
+
+		for (int i = 0; i < glmesh->m_indices->size(); i++)
+		{
+			indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
+		}
+
+		for (int i = 0; i < glmesh->m_vertices->size(); i++)
+		{
+			GLInstanceVertex& v = glmesh->m_vertices->at(i);
+			btVector3 vert(v.xyzw[0], v.xyzw[1], v.xyzw[2]);
+			btVector3 vt = visualTransform*vert;
+			v.xyzw[0] = vt[0];
+			v.xyzw[1] = vt[1];
+			v.xyzw[2] = vt[2];
+			btVector3 triNormal(v.normal[0], v.normal[1], v.normal[2]);
+			triNormal = visualTransform.getBasis()*triNormal;
+			v.normal[0] = triNormal[0];
+			v.normal[1] = triNormal[1];
+			v.normal[2] = triNormal[2];
+			verticesOut.push_back(v);
+		}
+	}
+	delete glmesh;
+}
+
 int BulletMJCFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
 {
-	return -1;
+	int graphicsIndex = -1;	
+	if (m_data->m_flags&CUF_MJCF_COLORS_FROM_FILE)
+	{
+		btAlignedObjectArray<GLInstanceVertex> vertices;
+		btAlignedObjectArray<int> indices;
+		btTransform startTrans; startTrans.setIdentity();
+		btAlignedObjectArray<MJCFURDFTexture> textures;
+
+		const UrdfModel& model = *m_data->m_models[m_data->m_activeModel];
+		UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
+		if (linkPtr)
+		{
+
+			const UrdfLink* link = *linkPtr;
+
+			for (int v = 0; v < link->m_visualArray.size(); v++)
+			{
+				const UrdfVisual& vis = link->m_visualArray[v];
+				btTransform childTrans = vis.m_linkLocalFrame;
+				btHashString matName(vis.m_materialName.c_str());
+				UrdfMaterial *const * matPtr = model.m_materials[matName];
+
+				convertURDFToVisualShapeInternal(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices, textures);
+
+			}
+		}
+		if (vertices.size() && indices.size())
+		{
+			if (1)
+			{
+				int textureIndex = -2;
+				if (textures.size())
+				{
+					textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
+				}
+				{
+					B3_PROFILE("registerGraphicsShape");
+					graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
+				}
+
+			}
+		}
+
+		//delete textures
+		for (int i = 0; i < textures.size(); i++)
+		{
+			B3_PROFILE("free textureData");
+			if (!textures[i].m_isCached)
+			{
+				free(textures[i].textureData1);
+			}
+		}
+	}
+	return graphicsIndex;
 }
 
 bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -12,12 +12,20 @@ struct MJCFErrorLogger
 	virtual void printMessage(const char* msg)=0;
 };
 
-
+struct MJCFURDFTexture
+{
+	int m_width;
+	int m_height;
+	unsigned char* textureData1;
+	bool m_isCached;
+};
 
 class BulletMJCFImporter : public URDFImporterInterface
 {
 	struct BulletMJCFImporterInternalData* m_data;
 
+	void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const;
+
 public:
 	BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, int flags);
 	virtual ~BulletMJCFImporter();
@@ -46,6 +54,7 @@ public:
 
 	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+	bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
 
 	//optional method to get collision group (type) and mask (affinity)
 	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const ;

examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -25,6 +25,7 @@ enum ConvertURDFFlags {
   CUF_RESERVED=64,
   CUF_USE_IMPLICIT_CYLINDER=128,
   CUF_GLOBAL_VELOCITIES_MB=256,
+  CUF_MJCF_COLORS_FROM_FILE=512,
 };
 
 void ConvertURDF2Bullet(const URDFImporterInterface& u2b,

examples/SharedMemory/SharedMemoryPublic.h

@@ -674,6 +674,7 @@ enum eURDF_Flags
 	URDF_RESERVED=64,
 	URDF_USE_IMPLICIT_CYLINDER =128,
 	URDF_GLOBAL_VELOCITIES_MB =256,
+	MJCF_COLORS_FROM_FILE=512,
 };
 
 enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -470,12 +470,14 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 				vtx.xyzw[1] = pos.y();
 				vtx.xyzw[2] = pos.z();
 				vtx.xyzw[3] = 1.f;
-				pos.normalize();
-				vtx.normal[0] = pos.x();
-				vtx.normal[1] = pos.y();
-				vtx.normal[2] = pos.z();
-				vtx.uv[0] = 0.5f;
-				vtx.uv[1] = 0.5f;
+				btVector3 normal = pos.safeNormalize();
+				vtx.normal[0] = normal.x();
+				vtx.normal[1] = normal.y();
+				vtx.normal[2] = normal.z();
+				btScalar u = btAtan2(normal[0], normal[2]) / (2 * SIMD_PI) + 0.5;
+				btScalar v = normal[1] * 0.5 + 0.5;
+				vtx.uv[0] = u;
+				vtx.uv[1] = v;
 				glmesh->m_vertices->push_back(vtx);
 			}
 
@@ -586,18 +588,33 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 			{
 				color.setValue(1,1,1,1);
 			}
-			if (model && useVisual)
+			if (model)
 			{
-				btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
-				UrdfMaterial*const* matPtr = model->m_materials[matName];
-				if (matPtr)
+				if (useVisual)
 				{
-					for (int i=0; i<4; i++)
+					btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
+					UrdfMaterial*const* matPtr = model->m_materials[matName];
+					if (matPtr)
 					{
-						rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i];
+						for (int i = 0; i < 4; i++)
+						{
+							rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i];
+						}
+						//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
+			{
+
+				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];
 					}
-					//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);
 				}
 			}
 			

examples/pybullet/examples/quadruped.py

@@ -4,7 +4,10 @@ import math
 
 
 def drawInertiaBox(parentUid, parentLinkIndex, color):
-	mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
+	dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
+	mass=dyn[0]
+	frictionCoeff=dyn[1]
+	inertia = dyn[2]
 	if (mass>0):
 		Ixx = inertia[0]
 		Iyy = inertia[1]
@@ -79,7 +82,7 @@ p.setTimeStep(fixedTimeStep)
 
 orn = p.getQuaternionFromEuler([0,0,0.4])
 p.setRealTimeSimulation(0)
-quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates)
+quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_USE_IMPLICIT_CYLINDER)
 nJoints = p.getNumJoints(quadruped)
 
 jointNameToId = {}
@@ -123,7 +126,11 @@ halfpi = 1.57079632679
 twopi = 4*halfpi
 kneeangle = -2.1834
 
-mass, friction, localInertiaDiagonal = p.getDynamicsInfo(quadruped,-1, flags=p.DYNAMICS_INFO_REPORT_INERTIA )
+dyn = p.getDynamicsInfo(quadruped,-1)
+mass=dyn[0]
+friction=dyn[1]
+localInertiaDiagonal = dyn[2]
+
 print("localInertiaDiagonal",localInertiaDiagonal)
 
 #this is a no-op, just to show the API

examples/pybullet/pybullet.c

@@ -526,6 +526,33 @@ void b3pybulletExitFunc(void)
 	}
 }
 
+static PyObject* pybullet_isConnected(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	int isConnected = 0;
+	int method = 0;
+	PyObject* pylist = 0;
+	PyObject* val = 0;
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = { "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm != 0)
+	{
+		if (b3CanSubmitCommand(sm))
+		{
+			isConnected = 1;
+			method = sPhysicsClientsGUI[physicsClientId];
+		}
+	}
+
+	return PyLong_FromLong(isConnected);
+}
+
+
 
 static PyObject* pybullet_getConnectionInfo(PyObject* self, PyObject* args, PyObject* keywds)
 {
@@ -8249,6 +8276,10 @@ static PyMethodDef SpamMethods[] = {
 	  "getConnectionInfo(physicsClientId=0)\n"
 	  "Return if a given client id is connected, and using what method."},
 
+	 { "isConnected", (PyCFunction)pybullet_isConnected, METH_VARARGS | METH_KEYWORDS,
+	"isConnected(physicsClientId=0)\n"
+	"Return if a given client id is connected." },
+
 	{"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS,
 	"resetSimulation(physicsClientId=0)\n"
 	 "Reset the simulation: remove all objects and start from an empty world."},
@@ -8818,7 +8849,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
 	PyModule_AddIntConstant(m, "URDF_USE_IMPLICIT_CYLINDER", URDF_USE_IMPLICIT_CYLINDER);
 	PyModule_AddIntConstant(m, "URDF_GLOBAL_VELOCITIES_MB", URDF_GLOBAL_VELOCITIES_MB);
-		
+	PyModule_AddIntConstant(m, "MJCF_COLORS_FROM_FILE", MJCF_COLORS_FROM_FILE);
+
 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);
 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS);