implement joint reaction forces for mobilizer motor/limit, by passing the constraint forces as 'external forces' and going through the Articulated Body Algorithm

minor refactor for forwardKinematics, store the cached world transform in each btMultiBody::link

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1553,6 +1553,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
     // Temporary matrices/vectors -- use scratch space from caller
     // so that we don't have to keep reallocating every frame
 
+	
 	int num_links = getNumLinks();	
     scratch_r.resize(m_dofCount);
     scratch_v.resize(4*num_links + 4);	    
@@ -2295,4 +2296,50 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 }
 
 
+void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
+{
+	
+	int num_links = getNumLinks();
+
+	// Cached 3x3 rotation matrices from parent frame to this frame.
+	btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
+
+	rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
+	
+	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);
+	}
+		
+	int nLinks = getNumLinks();
+	///base + num m_links
+	world_to_local.resize(nLinks+1);
+	local_origin.resize(nLinks+1);
+
+	world_to_local[0] = getWorldToBaseRot();
+	local_origin[0] = getBasePos();
+	
+	for (int k=0;k<getNumLinks();k++)
+	{
+		const int parent = getParent(k);
+		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
+		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
+	}
+
+	for (int link=0;link<getNumLinks();link++)
+	{
+		int index = link+1;
+
+		btVector3 posr = local_origin[index];
+		btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(posr);
+		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+		getLink(link).m_cachedWorldTransform = tr;
+			
+	}
+
+}
 

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -190,6 +190,7 @@ public:
 		btTransform tr;
 		tr.setOrigin(getBasePos());
 		tr.setRotation(getWorldToBaseRot().inverse());
+		return tr;
 	}
 
     void setBaseVel(const btVector3 &vel) 
@@ -552,6 +553,8 @@ public:
 	{
 		return m_internalNeedsJointFeedback;
 	}
+	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
+	
 
 private:
     btMultiBody(const btMultiBody &);  // not implemented

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -98,7 +98,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			rel_pos1 = posAworld - multiBodyA->getBasePos();
 		} else
 		{
-			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_worldPosition;
+			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
 
 		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
@@ -164,7 +164,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			rel_pos2 = posBworld - multiBodyB->getBasePos();
 		} else
 		{
-			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_worldPosition;
+			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
 		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -294,7 +294,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			rel_pos1 = pos1 - multiBodyA->getBasePos();
 		} else
 		{
-			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_worldPosition;
+			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
 		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 
@@ -346,7 +346,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			rel_pos2 = pos2 - multiBodyB->getBasePos();
 		} else
 		{
-			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_worldPosition;
+			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
 		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
@@ -957,7 +957,7 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 					if (!c.m_useJointForce)
 					{
 						c.m_multiBodyB->addLinkForce(c.m_linkB,force);
-						b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
+						//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
 						c.m_multiBodyB->addLinkTorque(c.m_linkB,torque);
 					}
 					

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -399,40 +399,10 @@ void	btMultiBodyDynamicsWorld::forwardKinematics()
 	btAlignedObjectArray<btQuaternion> world_to_local;
 	btAlignedObjectArray<btVector3> local_origin;
 
-	
-
 	for (int b=0;b<m_multiBodies.size();b++)
 	{
 		btMultiBody* bod = m_multiBodies[b];
-		int nLinks = bod->getNumLinks();
+		bod->forwardKinematics(world_to_local,local_origin);
-		///base + num m_links
-		world_to_local.resize(nLinks+1);
-		local_origin.resize(nLinks+1);
-
-		world_to_local[0] = bod->getWorldToBaseRot();
-		local_origin[0] = bod->getBasePos();
-	
-		for (int k=0;k<bod->getNumLinks();k++)
-		{
-			const int parent = bod->getParent(k);
-			world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
-			local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
-		}
-
-		for (int link=0;link<bod->getNumLinks();link++)
-		{
-			int index = link+1;
-
-			btVector3 posr = local_origin[index];
-			btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
-			btTransform tr;
-			tr.setIdentity();
-			tr.setOrigin(posr);
-			tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-			bod->getLink(link).m_worldPosition = posr;
-			
-		}
-
 	}
 }
 void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
@@ -507,10 +477,7 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 	}
 
-	m_solverMultiBodyIslandCallback->processConstraints();
 	
-	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
-
 
 	{
 		BT_PROFILE("btMultiBody stepVelocities");
@@ -709,6 +676,11 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 	}
 
+	m_solverMultiBodyIslandCallback->processConstraints();
+	
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+
+
 
 }
 
@@ -850,8 +822,8 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 		{
 			BT_PROFILE("btMultiBody debugDrawWorld");
 			
-			btAlignedObjectArray<btQuaternion> world_to_local;
+			btAlignedObjectArray<btQuaternion> world_to_local1;
-			btAlignedObjectArray<btVector3> local_origin;
+			btAlignedObjectArray<btVector3> local_origin1;
 
 			for (int c=0;c<m_multiBodyConstraints.size();c++)
 			{
@@ -862,52 +834,40 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 			for (int b = 0; b<m_multiBodies.size(); b++)
 			{
 				btMultiBody* bod = m_multiBodies[b];
-				int nLinks = bod->getNumLinks();
+				bod->forwardKinematics(world_to_local1,local_origin1);
-
-				///base + num m_links
-				world_to_local.resize(nLinks + 1);
-				local_origin.resize(nLinks + 1);
-
-					
-				world_to_local[0] = bod->getWorldToBaseRot();
-				local_origin[0] = bod->getBasePos();
-
 				
-				{
+				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
-					btVector3 posr = local_origin[0];
-					//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
-					btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
-					btTransform tr;
-					tr.setIdentity();
-					tr.setOrigin(posr);
-					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
-
-					getDebugDrawer()->drawTransform(tr, 0.1);
-
-				}
-
-				for (int k = 0; k<bod->getNumLinks(); k++)
-				{
-					const int parent = bod->getParent(k);
-					world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
-					local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
-				}
 
+				int nLinks = bod->getNumLinks();
 
 				for (int m = 0; m<bod->getNumLinks(); m++)
 				{
 					int link = m;
-					int index = link + 1;
+					
-
+					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
-					btVector3 posr = local_origin[index];
-					//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
-					btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
-					btTransform tr;
-					tr.setIdentity();
-					tr.setOrigin(posr);
-					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
 
 					getDebugDrawer()->drawTransform(tr, 0.1);
+
+						//draw the joint axis
+					if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
+					
+						btVector4 color(0,0,0,1);//1,1,1);
+						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from,to,color);
+					}
+					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+					
+						btVector4 color(0,0,0,1);//1,1,1);
+						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from,to,color);
+					}
+					
 				}
 			}
 		}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -85,5 +85,6 @@ public:
 	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 	
 	void	forwardKinematics();
+	
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -21,9 +21,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 
+
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
 	//:btMultiBodyConstraint(body,0,link,-1,2,true),
-	:btMultiBodyConstraint(body,body,link,link,2,true),
+	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
 	m_lowerBound(lower),
 	m_upperBound(upper)
 {
@@ -43,7 +44,6 @@ void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 	jacobianA(0)[offset] = 1;
 	// row 1: the upper bound
 	//jacobianA(1)[offset] = -1;
-
 	jacobianB(1)[offset] = -1;
 
 	m_numDofsFinalized = m_jacSizeBoth;
@@ -92,6 +92,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal)
 {
+	
     // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
 
@@ -106,9 +107,12 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 
     // row 1: the upper bound
     setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
-
+	
 	for (int row=0;row<getNumRows();row++)
 	{
+		
+		btScalar direction = row? -1 : 1;
+
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
@@ -116,7 +120,42 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		const btVector3 dummy(0, 0, 0);
 
 		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
-		constraintRow.m_useJointForce = true;
+
+		if (m_bodyA->isMultiDof())
+		{
+			constraintRow.m_useJointForce = false;
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
+					
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1=prismaticAxisInWorld;
+					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+			
+		}
+
 		{
 			btScalar penetration = getPosition(row);
 			btScalar positionalError = 0.f;

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -22,11 +22,11 @@ subject to the following restrictions:
 
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
-	:btMultiBodyConstraint(body,body,link,link,1,true),
+	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
 	m_desiredVelocity(desiredVelocity)
 {
 
-
+	int parent = body->getLink(link).m_parent;
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
     // initialize them here
@@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 
 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),
+	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
 	m_desiredVelocity(desiredVelocity)
 {
 	btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -98,7 +98,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 {
     // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
-
+	
 	if (m_numDofsFinalized != m_jacSizeBoth)
 	{
         finalizeMultiDof();
@@ -117,7 +117,42 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 
 
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
-		constraintRow.m_useJointForce = true;
+
+		if (m_bodyA->isMultiDof())
+		{
+			constraintRow.m_useJointForce = false;
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
+					
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1=prismaticAxisInWorld;
+					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+			
+		}
+
 	}
 
 }

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -122,7 +122,7 @@ struct btMultibodyLink
 	
 	struct btMultiBodyJointFeedback*	m_jointFeedback;
 
-	btVector3	m_worldPosition;
+	btTransform	m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
 
     // ctor: set some sensible defaults
 	btMultibodyLink()
@@ -135,9 +135,10 @@ struct btMultibodyLink
 			m_dofCount(0),
 			m_posVarCount(0),
 			m_jointType(btMultibodyLink::eInvalid),
-			m_jointFeedback(0),
+			m_jointFeedback(0)
-			m_worldPosition(0,0,0)
+			
 	{
+		
 		m_inertiaLocal.setValue(1, 1, 1);
 		setAxisTop(0, 0., 0., 0.);
 		setAxisBottom(0, 1., 0., 0.);
@@ -150,6 +151,7 @@ struct btMultibodyLink
 		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
 		m_jointPos[3] = 1.f;			//"quat.w"
 		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
+		m_cachedWorldTransform.setIdentity();
 	}
 
     // routine to update m_cachedRotParentToThis and m_cachedRVector