MJCF: fix capsule length when given in size="", fix slider joint limits

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -357,6 +357,7 @@ struct BulletMJCFImporterInternalData
 
 	bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
 	{
+		bool jointHandled = false;
 		const char* jType = link_xml->Attribute("type");
 		const char* limitedStr = link_xml->Attribute("limited");
 		const char* axisStr = link_xml->Attribute("axis");
@@ -386,8 +387,8 @@ struct BulletMJCFImporterInternalData
 				jointTrans.setRotation(orn);
 			}
 		}
-		btVector3 jointAxis(1,0,0);
 
+		btVector3 jointAxis(1,0,0);
 		if (axisStr)
 		{
 			std::string ax = axisStr;
@@ -396,80 +397,15 @@ struct BulletMJCFImporterInternalData
 		{
 			logger->reportWarning( (sourceFileLocation(link_xml) + ": joint without axis attribute").c_str() );
 		}
-		bool isLimited = false;
-		double range[2] = {1,0};
 
+		double range[2] = {1,0};
 		std::string lim = m_defaultJointLimited;
 		if (limitedStr)
 		{
 			lim = limitedStr;
 		}
-		if (lim=="true")
+		bool isLimited = lim=="true";
-		{
-			isLimited = true;
-			//parse the 'range' field
-			btArray<std::string> pieces;
-			btArray<float> limits;
-			btAlignedObjectArray<std::string> strArray;
-			urdfIsAnyOf(" ", strArray);
-			urdfStringSplit(pieces, rangeStr, strArray);
-			for (int i = 0; i < pieces.size(); ++i)
-			{
-				if (!pieces[i].empty())
-				{
-					limits.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
-				}
-			}
-			bool success = false;
-			if (limits.size()==2)
-			{
-				if (m_angleUnits=="degree")
-				{
-					range[0] = limits[0] * B3_PI / 180;
-					range[1] = limits[1] * B3_PI / 180;
-					success = true;
-				}
-				else if (m_angleUnits=="radian")
-				{
-					range[0] = limits[0];
-					range[1] = limits[1];
-					success = true;
-				}
-			}
-			if (!success)
-			{
-				logger->reportWarning( (sourceFileLocation(link_xml) + ": cannot parse 'range' attribute (units='" + m_angleUnits + "'')").c_str() );
-			}
-		}
 
-		// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
-		// degrees of freedom created by this joint. These are constants added to the
-		// diagonal of the inertia matrix in generalized coordinates. They make the
-		// simulation more stable, and often increase physical realism. This is because
-		// when a motor is attached to the system with a transmission that amplifies
-		// the motor force by c, the inertia of the rotor (i.e. the moving part of the
-		// motor) is amplified by c*c. The same holds for gears in the early stages of
-		// planetary gear boxes. These extra inertias often dominate the inertias of
-		// the robot parts that are represented explicitly in the model, and the
-		// armature attribute is the way to model them.
-
-		// TODO damping : real, "0" Damping applied to all degrees of
-		// freedom created by this joint. Unlike friction loss
-		// which is computed by the constraint solver, damping is
-		// simply a force linear in velocity. It is included in
-		// the passive forces. Despite this simplicity, larger
-		// damping values can make numerical integrators unstable,
-		// which is why our Euler integrator handles damping
-		// implicitly. See Integration in the Computation chapter.
-
-		bool jointHandled = false;
-		const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
-		
-		btTransform parentLinkToJointTransform;
-		parentLinkToJointTransform.setIdentity();
-		parentLinkToJointTransform = parentToLinkTrans*jointTrans;
-
-		jointTransOut = jointTrans;
 		UrdfJointTypes ejtype;
 		if (jType)
 		{
@@ -500,6 +436,64 @@ struct BulletMJCFImporterInternalData
 			logger->reportWarning( (sourceFileLocation(link_xml) + ": expected 'type' attribute for joint").c_str() );
 		}
 
+		if (isLimited)
+		{
+			//parse the 'range' field
+			btArray<std::string> pieces;
+			btArray<float> limits;
+			btAlignedObjectArray<std::string> strArray;
+			urdfIsAnyOf(" ", strArray);
+			urdfStringSplit(pieces, rangeStr, strArray);
+			for (int i = 0; i < pieces.size(); ++i)
+			{
+				if (!pieces[i].empty())
+				{
+					limits.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
+				}
+			}
+			if (limits.size()==2)
+			{
+				range[0] = limits[0];
+				range[1] = limits[1];
+				if (m_angleUnits=="degree" && ejtype==URDFRevoluteJoint)
+				{
+					range[0] = limits[0] * B3_PI / 180;
+					range[1] = limits[1] * B3_PI / 180;
+				}
+			}
+			else
+			{
+				logger->reportWarning( (sourceFileLocation(link_xml) + ": cannot parse 'range' attribute (units='" + m_angleUnits + "'')").c_str() );
+			}
+		}
+
+		// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
+		// degrees of freedom created by this joint. These are constants added to the
+		// diagonal of the inertia matrix in generalized coordinates. They make the
+		// simulation more stable, and often increase physical realism. This is because
+		// when a motor is attached to the system with a transmission that amplifies
+		// the motor force by c, the inertia of the rotor (i.e. the moving part of the
+		// motor) is amplified by c*c. The same holds for gears in the early stages of
+		// planetary gear boxes. These extra inertias often dominate the inertias of
+		// the robot parts that are represented explicitly in the model, and the
+		// armature attribute is the way to model them.
+
+		// TODO damping : real, "0" Damping applied to all degrees of
+		// freedom created by this joint. Unlike friction loss
+		// which is computed by the constraint solver, damping is
+		// simply a force linear in velocity. It is included in
+		// the passive forces. Despite this simplicity, larger
+		// damping values can make numerical integrators unstable,
+		// which is why our Euler integrator handles damping
+		// implicitly. See Integration in the Computation chapter.
+
+		const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
+		
+		btTransform parentLinkToJointTransform;
+		parentLinkToJointTransform.setIdentity();
+		parentLinkToJointTransform = parentToLinkTrans*jointTrans;
+		jointTransOut = jointTrans;
+
 		if (jointHandled)
 		{
 			UrdfJoint* jointPtr = new UrdfJoint();
@@ -660,14 +654,14 @@ struct BulletMJCFImporterInternalData
 				}
 
 				geom.m_capsuleRadius = 0;
-				geom.m_capsuleHalfHeight = 0.f;
+				geom.m_capsuleHeight = 0.f;
 
 				if (sizes.size()>0)
 				{
 					geom.m_capsuleRadius = sizes[0];
 					if (sizes.size()>1)
 					{
-						geom.m_capsuleHalfHeight = sizes[1];
+						geom.m_capsuleHeight = 2*sizes[1];
 					}
 				} else
 				{
@@ -1687,7 +1681,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 				} else
 				{
 					btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,
-						2.*col->m_geometry.m_capsuleHalfHeight);
+						col->m_geometry.m_capsuleHeight);
 					childShape = cap;
 				}
 				break;

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -564,9 +564,9 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 		case URDF_GEOM_CAPSULE:
         {
 			btScalar radius = collision->m_geometry.m_capsuleRadius;
-			btScalar height = btScalar(2.f)*collision->m_geometry.m_capsuleHalfHeight;
+			btScalar height = collision->m_geometry.m_capsuleHeight;
 			btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height);
-            shape = capsuleShape;
+			shape = capsuleShape;
 			shape ->setMargin(gUrdfDefaultCollisionMargin);
 			break;
 		}
@@ -574,7 +574,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
         case URDF_GEOM_CYLINDER:
         {
 			btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
-			btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
+			btScalar cylLength = collision->m_geometry.m_capsuleHeight;
 			
             btAlignedObjectArray<btVector3> vertices;
             //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
@@ -797,7 +797,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 			{
 
 				btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
-				btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
+				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);

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -403,7 +403,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
 	  }
 		geom.m_hasFromTo = false;
 		geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
-		geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
+		geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
 		
 	}
 	else if (type_name == "capsule")
@@ -417,7 +417,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
 		}
 		geom.m_hasFromTo = false;
 		geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
-		geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
+		geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
 	}
 	else if (type_name == "mesh")
 	{

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -65,7 +65,7 @@ struct UrdfGeometry
 	btVector3 m_boxSize;
 	
 	double m_capsuleRadius;
-	double m_capsuleHalfHeight;
+	double m_capsuleHeight;
 	int m_hasFromTo;
 	btVector3 m_capsuleFrom;
 	btVector3 m_capsuleTo;

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -220,7 +220,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 				rad = visual->m_geometry.m_capsuleRadius;
 			} else {
 				tr = visual->m_linkLocalFrame;
-				len = visual->m_geometry.m_capsuleHalfHeight;
+				len = visual->m_geometry.m_capsuleHeight;
 				rad = visual->m_geometry.m_capsuleRadius;
 			}
 			visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];