fix some out-of-date comments in btMultiBody, thanks to Thomas Buschmann

disable disactivation for btRigidBody in import urdf demo, increase strength of joint motor for btMultiBody

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -130,7 +130,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gFileNameArray.size()==0)
 		{
-			gFileNameArray.push_back("r2d2.urdf");//husky/husky.urdf");
+			gFileNameArray.push_back("prismatic.urdf");
 
 		}
 
@@ -224,8 +224,10 @@ void ImportUrdfSetup::initPhysics()
 	URDFImporterInterface& u2b = *bla;
 	bool loadOk =  u2b.loadURDF(m_fileName);
 
+#ifdef TEST_MULTIBODY_SERIALIZATION	
 	//test to serialize a multibody to disk or shared memory, with base, link and joint names
 	btSerializer* s = new btDefaultSerializer;
+#endif //TEST_MULTIBODY_SERIALIZATION
 
 	if (loadOk)
 	{
@@ -255,8 +257,9 @@ void ImportUrdfSetup::initPhysics()
 			{
 				std::string*   name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
 				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
 				s->registerNameForPointer(name->c_str(),name->c_str());
-
+#endif//TEST_MULTIBODY_SERIALIZATION
 				mb->setBaseName(name->c_str());
 				//create motors for each btMultiBody joint
 				int numLinks = mb->getNumLinks();
@@ -267,9 +270,10 @@ void ImportUrdfSetup::initPhysics()
 
 					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
 					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION					
 					s->registerNameForPointer(jointName->c_str(),jointName->c_str());
 					s->registerNameForPointer(linkName->c_str(),linkName->c_str());
-
+#endif//TEST_MULTIBODY_SERIALIZATION
 					m_nameMemory.push_back(jointName);
 					m_nameMemory.push_back(linkName);
 
@@ -291,7 +295,7 @@ void ImportUrdfSetup::initPhysics()
 							slider.m_minVal=-4;
 							slider.m_maxVal=4;
 							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
-							float maxMotorImpulse = 0.1f;
+							float maxMotorImpulse = 10.1f;
 							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
 							//motor->setMaxAppliedImpulse(0);
 							m_data->m_jointMotors[m_data->m_numMotors]=motor;
@@ -376,7 +380,7 @@ void ImportUrdfSetup::initPhysics()
 		m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
 	}
 
-	
+#ifdef TEST_MULTIBODY_SERIALIZATION
 	m_dynamicsWorld->serialize(s);
 	b3ResourcePath p;
 	char resourcePath[1024];
@@ -386,6 +390,8 @@ void ImportUrdfSetup::initPhysics()
 		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
 		fclose(f);
 	}
+#endif//TEST_MULTIBODY_SERIALIZATION
+
 }
 
 void ImportUrdfSetup::stepSimulation(float deltaTime)

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -32,6 +32,8 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
     btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
     rbci.m_startWorldTransform = initialWorldTrans;
     btRigidBody* body = new btRigidBody(rbci);
+	body->forceActivationState(DISABLE_DEACTIVATION);
+	
     return body;
 }
     

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -182,7 +182,7 @@ void btMultiBody::setupPrismatic(int i,
                                int parent,
                                const btQuaternion &rotParentToThis,
                                const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisComOffset,
+                               const btVector3 &parentComToThisPivotOffset,
 							   const btVector3 &thisPivotToThisComOffset,
 							   bool disableParentCollision)
 {
@@ -198,7 +198,7 @@ void btMultiBody::setupPrismatic(int i,
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
     m_links[i].setAxisTop(0, 0., 0., 0.);
     m_links[i].setAxisBottom(0, jointAxis);
-    m_links[i].m_eVector = parentComToThisComOffset;
+    m_links[i].m_eVector = parentComToThisPivotOffset;
 	m_links[i].m_dVector = thisPivotToThisComOffset;
     m_links[i].m_cachedRotParentToThis = rotParentToThis;
 

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -83,7 +83,7 @@ public:
                                int parent,
                                const btQuaternion &rotParentToThis,
                                const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisComOffset,
+                               const btVector3 &parentComToThisPivotOffset,
 							   const btVector3 &thisPivotToThisComOffset,
 							   bool disableParentCollision);
 

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -56,11 +56,14 @@ struct btMultibodyLink
 
     btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
 
-    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
+    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant.
+	//this is set to zero for planar joint (see also m_eVector comment)
+	
+    // m_eVector is constant, but depends on the joint type:
+    // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
+	// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
+	// todo: fix the planar so it is consistent with the other joints
 	
-    // m_eVector is constant, but depends on the joint type
-    // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
-    // revolute: vector from parent's COM to the pivot point, in PARENT's frame.
     btVector3 m_eVector;
 
 	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;