prepare robotics learning examples, see examples/RoboticsLearning/b3RobotSimAPI.h

prepare compliant contact work, urdf loading of parameters (see data/cube.urdf)

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -1005,6 +1005,17 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
 	return false;
 }
 
+bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
+{
+	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+	if (linkPtr)
+	{
+		const UrdfLink* link = *linkPtr;
+		contactInfo = link->m_contactInfo;
+		return true;
+	}
+	return false;
+}
 
 void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const
 {

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -37,6 +37,8 @@ public:
     virtual std::string getLinkName(int linkIndex) const;
 
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
 	
     virtual std::string getJointName(int linkIndex) const;
     

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -132,7 +132,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gFileNameArray.size()==0)
 		{
-			gFileNameArray.push_back("sphere2.urdf");
+			gFileNameArray.push_back("r2d2.urdf");
 
 		}
 
@@ -200,7 +200,7 @@ void ImportUrdfSetup::initPhysics()
 
 
 	btVector3 gravity(0,0,0);
-	gravity[upAxis]=-9.8;
+	//gravity[upAxis]=-9.8;
 
 	m_dynamicsWorld->setGravity(gravity);
 

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -392,9 +392,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                 
                 u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
 
-                btScalar friction = 0.5f;
+				URDFLinkContactInfo contactInfo;
+				u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
 
-                col->setFriction(friction);
+				if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
+				{
+					col->setFriction(contactInfo.m_lateralFriction);
+				}
+				if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
+				{
+					col->setRollingFriction(contactInfo.m_rollingFriction);
+				}
+				
 
                 if (mbLinkIndex>=0) //???? double-check +/- 1
                 {

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -6,6 +6,7 @@
 #include "LinearMath/btTransform.h"
 #include "URDFJointTypes.h"
 
+
 class URDFImporterInterface
 {
 
@@ -28,6 +29,9 @@ public:
     virtual std::string getLinkName(int linkIndex) const =0;
 	/// 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 { return false;}
+
+	///this API will likely change, don't override it!
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const  { return false;}
     
     virtual std::string getJointName(int linkIndex) const = 0;
 

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -11,6 +11,33 @@ enum UrdfJointTypes
 		URDFPlanarJoint,
 		URDFFixedJoint,
 };
+#include "LinearMath/btScalar.h"
+
+enum URDF_LinkContactFlags
+{
+	URDF_CONTACT_HAS_LATERAL_FRICTION=1,
+	URDF_CONTACT_HAS_ROLLING_FRICTION=2,
+	URDF_CONTACT_HAS_CONTACT_CFM=4,
+	URDF_CONTACT_HAS_CONTACT_ERP=8
+};
+
+struct URDFLinkContactInfo
+{
+	btScalar m_lateralFriction;
+	btScalar m_rollingFriction;
+	btScalar m_contactCfm;
+	btScalar m_contactErp;
+	int m_flags;
+
+	URDFLinkContactInfo()
+		:m_lateralFriction(0.5),
+		m_rollingFriction(0),
+		m_contactCfm(0),
+		m_contactErp(0)
+	{
+		m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
+	}
+};
 
 
 #endif //URDF_JOINT_TYPES_H

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -569,6 +569,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
         }
     }
 
+	{
+		//optional 'contact' parameters
+	 TiXmlElement* ci = config->FirstChildElement("contact");
+	  if (ci)
+	  {
+		TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
+		if (friction_xml)
+		{
+			if (m_parseSDF)
+			{
+				link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
+			} else
+			{
+				if (!friction_xml->Attribute("value"))
+				{
+				  logger->reportError("Link/contact: lateral_friction element must have value attribute");
+				  return false;
+				}
+
+				link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
+			}
+		}
+	  }
+	}
+
   // Inertial (optional)
   TiXmlElement *i = config->FirstChildElement("inertial");
   if (i)

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -99,6 +99,8 @@ struct UrdfLink
 	
 	int m_linkIndex;
 	
+	URDFLinkContactInfo m_contactInfo;
+
 	UrdfLink()
 		:m_parentLink(0),
 		m_parentJoint(0)