Merge remote-tracking branch 'bp/master'

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -195,6 +195,14 @@ struct BulletMJCFImporterInternalData
 		m_pathPrefix[0] = 0;
 	}
 
+	~BulletMJCFImporterInternalData()
+	{
+		for (int i=0;i<m_models.size();i++)
+		{
+			delete m_models[i];
+		}
+	}
+
 	std::string sourceFileLocation(TiXmlElement* e)
 	{
 #if 0
@@ -630,11 +638,7 @@ struct BulletMJCFImporterInternalData
 		bool handledGeomType = false;
 		UrdfGeometry geom;
 
-		
-
-		const char* gType = link_xml->Attribute("type");
 		const char* sz = link_xml->Attribute("size");
-		const char* posS = link_xml->Attribute("pos");
 		int conDim = defaults.m_defaultConDim;
 
 		const char* conDimS = link_xml->Attribute("condim");
@@ -656,7 +660,7 @@ struct BulletMJCFImporterInternalData
 			btArray<float> frictions;
 			btAlignedObjectArray<std::string> strArray;
 			urdfIsAnyOf(" ", strArray);
-			urdfStringSplit(pieces, sz, strArray);
+			urdfStringSplit(pieces, frictionS, strArray);
 			for (int i = 0; i < pieces.size(); ++i)
 			{
 				if (!pieces[i].empty())
@@ -707,6 +711,7 @@ struct BulletMJCFImporterInternalData
 			geom.m_localMaterial.m_name = rgba;
 		}
 
+		const char* posS = link_xml->Attribute("pos");
 		if (posS)
 		{
 			btVector3 pos(0,0,0);
@@ -716,18 +721,32 @@ struct BulletMJCFImporterInternalData
 				linkLocalFrame.setOrigin(pos);
 			}
 		}
+
 		const char* ornS = link_xml->Attribute("quat");
 		if (ornS)
 		{
-			std::string ornStr = ornS;
 			btQuaternion orn(0,0,0,1);
 			btVector4 o4;
-			if (parseVector4(o4,ornStr))
+			if (parseVector4(o4, ornS))
 			{
 				orn.setValue(o4[1],o4[2],o4[3],o4[0]);
 				linkLocalFrame.setRotation(orn);
 			}
 		}
+
+		const char* axis_and_angle = link_xml->Attribute("axisangle");
+		if (axis_and_angle)
+		{
+			btQuaternion orn(0,0,0,1);
+			btVector4 o4;
+			if (parseVector4(o4, axis_and_angle))
+			{
+				orn.setRotation(btVector3(o4[0],o4[1],o4[2]), o4[3]);
+				linkLocalFrame.setRotation(orn);
+			}
+		}
+
+		const char* gType = link_xml->Attribute("type");
 		if (gType)
 		{
 			std::string geomType = gType;
@@ -792,8 +811,8 @@ struct BulletMJCFImporterInternalData
 					}
 				}
 
-				geom.m_capsuleRadius = 0;
+				geom.m_capsuleRadius = 2.00f; // 2 to make it visible if something is wrong
-				geom.m_capsuleHeight = 0.f;
+				geom.m_capsuleHeight = 2.00f;
 
 				if (sizes.size()>0)
 				{
@@ -877,6 +896,7 @@ struct BulletMJCFImporterInternalData
 
 				col.m_geometry = geom;
 				col.m_linkLocalFrame = linkLocalFrame;
+				col.m_sourceFileLocation = sourceFileLocation(link_xml);
 				linkPtr->m_collisionArray.push_back(col);
 
 			} else
@@ -950,22 +970,6 @@ struct BulletMJCFImporterInternalData
 					col->m_geometry.m_boxSize[2];
 				break;
 			}
-
-			case URDF_GEOM_CYLINDER:
-			{
-				double r = col->m_geometry.m_capsuleRadius;
-				btScalar h(0);
-				//and one cylinder of 'height'
-				if (col->m_geometry.m_hasFromTo)
-				{
-					h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length();
-				} else
-				{
-					h = col->m_geometry.m_capsuleHeight;
-				}
-				totalVolume += SIMD_PI*r*r*h;
-				break;
-			}
 			case URDF_GEOM_MESH:
 			{
 				//todo (based on mesh bounding box?)
@@ -976,11 +980,15 @@ struct BulletMJCFImporterInternalData
 				//todo
 				break;
 			}
+			case URDF_GEOM_CYLINDER:
 			case URDF_GEOM_CAPSULE:
 			{
 				//one sphere 
 				double r = col->m_geometry.m_capsuleRadius;
-				totalVolume += 4./3.*SIMD_PI*r*r*r;
+				if (col->m_geometry.m_type==URDF_GEOM_CAPSULE)
+				{
+					totalVolume += 4./3.*SIMD_PI*r*r*r;
+				}
 				btScalar h(0);
 				if (col->m_geometry.m_hasFromTo)
 				{
@@ -1017,9 +1025,9 @@ struct BulletMJCFImporterInternalData
 		UrdfModel* modelPtr = m_models[modelIndex];
 		int orgChildLinkIndex = modelPtr->m_links.size();
 		UrdfLink* linkPtr = new UrdfLink();
-		char uniqueLinkName[1024];
+		char linkn[1024];
-		sprintf(uniqueLinkName,"link%d",orgChildLinkIndex );
+		sprintf(linkn, "link%d_%d", modelIndex, orgChildLinkIndex);
-		linkPtr->m_name = uniqueLinkName;
+		linkPtr->m_name = linkn;
 		if (namePtr)
 		{
 			linkPtr->m_name = namePtr;
@@ -1205,6 +1213,7 @@ struct BulletMJCFImporterInternalData
 		}
 
 		linkPtr->m_linkTransformInWorld = linkTransform;
+
 		if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
 		{
 			//linkPtr->m_linkTransformInWorld.setIdentity();

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -285,6 +285,9 @@ void ConvertURDF2BulletInternal(
             if (!(flags & CUF_USE_URDF_INERTIA))
             {
                 compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
+                btAssert(localInertiaDiagonal[0] < 1e10);
+                btAssert(localInertiaDiagonal[1] < 1e10);
+                btAssert(localInertiaDiagonal[2] < 1e10);
             }
             URDFLinkContactInfo contactInfo;
             u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -13,48 +13,10 @@ m_activeSdfModel(-1)
 
 UrdfParser::~UrdfParser()
 {
-    cleanModel(&m_urdf2Model);
+	for (int i=0;i<m_tmpModels.size();i++)
-    
+	{
-    for (int i=0;i<m_tmpModels.size();i++)
-    {
-        cleanModel(m_tmpModels[i]);
 		delete m_tmpModels[i];
-    }
+	}
-    m_sdfModels.clear();
-    m_tmpModels.clear();
-}
-
-void UrdfParser::cleanModel(UrdfModel* model)
-{
-    for (int i=0;i<model->m_materials.size();i++)
-    {
-        UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
-        if (matPtr)
-        {
-            UrdfMaterial* mat = *matPtr;
-            delete mat;
-        }
-    }
-    
-    for (int i=0;i<model->m_links.size();i++)
-    {
-        UrdfLink** linkPtr = model->m_links.getAtIndex(i);
-        if (linkPtr)
-        {
-            UrdfLink* link = *linkPtr;
-            delete link;
-        }
-    }
-    
-    for (int i=0;i<model->m_joints.size();i++)
-    {
-        UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
-        if (jointPtr)
-        {
-            UrdfJoint* joint = *jointPtr;
-            delete joint;
-        }
-    }
 }
 
 static bool parseVector4(btVector4& vec4, const std::string& vector_str)
@@ -1471,6 +1433,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 		UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
 		if (mat)
 		{
+			delete material;
 			logger->reportWarning("Duplicate material");
 		} else
 		{
@@ -1494,6 +1457,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 			{
 				logger->reportError("Link name is not unique, link names in the same model have to be unique");
 				logger->reportError(link->m_name.c_str());
+				delete link;
 				return false;
 			} else
 			{
@@ -1542,6 +1506,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 			{
 				logger->reportError("joint '%s' is not unique.");
 				logger->reportError(joint->m_name.c_str());
+				delete joint;
 				return false;
 			}
 			else
@@ -1552,6 +1517,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 		else
 		{
 			logger->reportError("joint xml is not initialized correctly");
+			delete joint;
 			return false;
 		}
 	}
@@ -1666,7 +1632,8 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
             UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
             if (mat)
             {
-                logger->reportWarning("Duplicate material");
+		logger->reportWarning("Duplicate material");
+		delete material;
             } else
             {
                 localModel->m_materials.insert(material->m_name.c_str(),material);
@@ -1689,6 +1656,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
                 {
                     logger->reportError("Link name is not unique, link names in the same model have to be unique");
                     logger->reportError(link->m_name.c_str());
+                    delete link;
                     return false;
                 } else
                 {
@@ -1737,6 +1705,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
                 {
                     logger->reportError("joint '%s' is not unique.");
                     logger->reportError(joint->m_name.c_str());
+                    delete joint;
                     return false;
                 }
                 else
@@ -1747,6 +1716,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
             else
             {
                 logger->reportError("joint xml is not initialized correctly");
+                delete joint;
                 return false;
             }
         }

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -195,7 +195,37 @@ struct UrdfModel
 	{
 		m_rootTransformInWorld.setIdentity();
 	}
-	
+
+	~UrdfModel()
+	{
+		for (int i = 0; i < m_materials.size(); i++)
+		{
+			UrdfMaterial** ptr = m_materials.getAtIndex(i);
+			if (ptr)
+			{
+				UrdfMaterial* t = *ptr;
+				delete t;
+			}
+		}
+		for (int i = 0; i < m_links.size(); i++)
+		{
+			UrdfLink** ptr = m_links.getAtIndex(i);
+			if (ptr)
+			{
+				UrdfLink* t = *ptr;
+				delete t;
+			}
+		}
+		for (int i = 0; i < m_joints.size(); i++)
+		{
+			UrdfJoint** ptr = m_joints.getAtIndex(i);
+			if (ptr)
+			{
+				UrdfJoint* t = *ptr;
+				delete t;
+			}
+		}
+	}
 };
 
 class UrdfParser
@@ -210,7 +240,6 @@ protected:
     int m_activeSdfModel;
 
     
-    void cleanModel(UrdfModel* model);
 	bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
 	bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
 	bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -526,7 +526,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 		}
 		else
 		{
-			// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
+			// We have to see something, take collision shape. Useful for MuJoCo xml, where there are no explicit visual shapes.
 			useVisual = false;
 			cnt = linkPtr->m_collisionArray.size();
 		}

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -260,7 +260,7 @@ public:
 
 	inline int getSolveSwingLimit()
 	{
-		return m_solveTwistLimit;
+		return m_solveSwingLimit;
 	}
 
 	inline btScalar getTwistLimitSign()