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"]
2018-01-11 21:04:08 -08:00
parent 22b4809891
commit 79051b7611
13 changed files with 986 additions and 55 deletions
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/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