dirty changes - stabilization hacks

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -396,7 +396,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
 				{	
 					if (1)
 					{
-						btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); 
+						btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,0,500000); 
 						world->addMultiBodyConstraint(con);
 					}
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1111,6 +1111,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
 	static btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
 	static btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
+	static btSpatialTransformationMatrix fromWorld;
+	fromWorld.m_trnVec.setZero();
 	/////////////////
 
     // ptr to the joint accel part of the output
@@ -1136,7 +1138,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));	
 
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 10.;
+		btScalar linDampMult = 1., angDampMult = 1.;
 		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
 								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
 
@@ -1163,33 +1165,76 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 
     rot_from_world[0] = rot_from_parent[0];
 
-    for (int i = 0; i < num_links; ++i) {
+	//
+	bool useGlobalVelocities = false;
+    for (int i = 0; i < num_links; ++i) {		
         const int parent = m_links[i].m_parent;
         rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
-		
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;		
-
         rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
-        
+
-        // vhat_i = i_xhat_p(i) * vhat_p(i)		
+		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-		fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+		fromWorld.m_rotMat = rot_from_world[i+1];
-		//nice alternative below (using operator *) but it generates temps
+
+		////clamp parent's omega
+		//btScalar parOmegaMod = spatVel[parent+1].getAngular().length();
+		//btScalar parOmegaModMax = 0.1;
+		//if(parOmegaMod > parOmegaModMax)
+		//{
+		//	//btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear());
+		//	btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod);
+		//	fromParent.transform(clampedParVel, spatVel[i+1]);
+		//	spatVel[parent+1] *= (parOmegaModMax / parOmegaMod);
+		//}
+		//else
+		{
+			// vhat_i = i_xhat_p(i) * vhat_p(i)		
+			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+			//nice alternative below (using operator *) but it generates temps
+		}
 		//////////////////////////////////////////////////////////////		
         
-        // now set vhat_i to its true value by doing
+		//if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical)
+		//{
+		//	btScalar mod2 = 0;
+		//	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+		//		mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof];
+
+		//	btScalar angvel = sqrt(mod2);
+		//	btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075;
+		//	btScalar step = 1; //dt
+		//	if (angvel*step > maxAngVel)
+		//	{
+		//		btScalar * qd =	getJointVelMultiDof(i);
+		//		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+		//			qd[dof] *= (maxAngVel/step) /angvel;			
+		//	}
+		//}
+
+		// now set vhat_i to its true value by doing
         // vhat_i += qidot * shat_i			
-		spatJointVel.setZero();		
+		if(!useGlobalVelocities)
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)			
+		{
-			spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
+			spatJointVel.setZero();
-		
+
-		// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-		spatVel[i+1] += spatJointVel;
+				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
-		//
+
-		// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
-		//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
+			spatVel[i+1] += spatJointVel;
+
+			//
+			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
+			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
+
+		}
+		else
+		{
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
+		}
 
 		// we can now calculate chat_i 		
-		spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);
+		spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);		
 
         // calculate zhat_i^A
 		//
@@ -1197,7 +1242,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
 		//
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 10.;
+		btScalar linDampMult = 1., angDampMult = 1.;
 		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
 									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
 		
@@ -1219,6 +1264,15 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 			zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular()));			
 		//		
 		zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
+		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
+		////clamp parent's omega
+		//btScalar parOmegaMod = temp.length();
+		//btScalar parOmegaModMax = 1000;
+		//if(parOmegaMod > parOmegaModMax)
+		//	temp *= parOmegaModMax / parOmegaMod;
+		//zeroAccSpatFrc[i+1].addLinear(temp);
+		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
+		
 		
 
 		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
@@ -1360,6 +1414,12 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
     // now do the loop over the m_links
     for (int i = 0; i < num_links; ++i) 
 	{
+		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
+		//	a = apar + cor + Sqdd
+		//or
+		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
+		//	a = apar + Sqdd
+
         const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
 
@@ -1369,13 +1429,14 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 		{
 			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//			
-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
 		}
 
 		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		//D^{-1} * (Y - h^{T}*apar)
 		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
 
-		spatAcc[i+1] += spatCoriolisAcc[i];
+		spatAcc[i+1] += spatCoriolisAcc[i];		
 
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
 			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
@@ -1411,7 +1472,68 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 	/////////////////
 
     // Final step: add the accelerations (times dt) to the velocities.
-    applyDeltaVeeMultiDof(output, dt);	
+    applyDeltaVeeMultiDof(output, dt);
+
+	/////
+	//btScalar angularThres = 1;
+	//btScalar maxAngVel = 0.;		
+	//bool scaleDown = 1.;
+	//for(int link = 0; link < m_links.size(); ++link)
+	//{		
+	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
+	//	{
+	//		maxAngVel = spatVel[link+1].getAngular().length();
+	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
+	//		break;
+	//	}		
+	//}
+
+	//if(scaleDown != 1.)
+	//{
+	//	for(int link = 0; link < m_links.size(); ++link)
+	//	{
+	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
+	//		{
+	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+	//				getJointVelMultiDof(link)[dof] *= scaleDown;
+	//		}
+	//	}
+	//}
+	/////
+
+	///////////////////
+	if(useGlobalVelocities)
+	{
+		for (int i = 0; i < num_links; ++i) 
+		{
+			const int parent = m_links[i].m_parent;
+			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
+			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done
+		
+			fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+			fromWorld.m_rotMat = rot_from_world[i+1];			
+        
+			// vhat_i = i_xhat_p(i) * vhat_p(i)		
+			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+			//nice alternative below (using operator *) but it generates temps
+			/////////////////////////////////////////////////////////////
+
+			// now set vhat_i to its true value by doing
+			// vhat_i += qidot * shat_i			
+			spatJointVel.setZero();
+
+			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
+		
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			spatVel[i+1] += spatJointVel;
+
+
+			fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
+			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
+		}
+	}
+	
 }
 #endif
 

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -197,7 +197,9 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
 
 		}
-
+		for (int i = 6; i < ndofA ; ++i) 
+			printf("%.4f ", multiBodyA->getVelocityVector()[i]);
+		printf("\nrel_vel = %.4f\n------------\n", rel_vel);
 		constraintRow.m_friction = 0.f;
 
 		constraintRow.m_appliedImpulse = 0.f;

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -21,11 +21,13 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 
-btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
 	:btMultiBodyConstraint(body,body,link,link,1,true),
 	m_desiredVelocity(desiredVelocity)	
 {
+	btAssert(linkDoF < body->getLink(link).m_dofCount);
+
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
     // initialize them here
@@ -33,7 +35,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
     // note: we rely on the fact that data.m_jacobians are
     // always initialized to zero by the Constraint ctor
 
-    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
+    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
 
 	// row 0: the lower bound
 	// row 0: the lower bound

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -30,7 +30,7 @@ protected:
 
 public:
 
-	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	virtual ~btMultiBodyJointMotor();
 
 	virtual int getIslandIdA() const;

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -133,6 +133,7 @@ namespace {
 		//
 		btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
 		btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
+		btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
 		btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
 		btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
 		btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
@@ -256,6 +257,29 @@ namespace {
 			}			
 		}
 
+		template<typename SpatialVectorType>
+		void transformInverseRotationOnly(	const SpatialVectorType &inVec,
+											SpatialVectorType &outVec,
+											eOutputOperation outOp = None)
+		{
+			if(outOp == None)
+			{
+				outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
+			}
+			else if(outOp == Add)
+			{
+				outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
+			}
+			else if(outOp == Subtract)
+			{
+				outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
+			}
+			
+		}
+
 		void transformInverse(	const btSymmetricSpatialDyad &inMat,
 								btSymmetricSpatialDyad &outMat,
 								eOutputOperation outOp = None)
@@ -342,7 +366,9 @@ struct btMultibodyLink
     // 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;    
+    btVector3 m_eVector;
+
+	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
 
 	enum eFeatherstoneJointType
 	{