Merge pull request #418 from erwincoumans/master

expose btMultiBodyConstraint applied impulse (force) on its degree of…

examples/ExampleBrowser/CMakeLists.txt

@@ -98,6 +98,7 @@ SET(App_ExampleBrowser_SRCS
   ../Vehicles/Hinge2Vehicle.h
   ../MultiBody/TestJointTorqueSetup.cpp
   ../MultiBody/TestJointTorqueSetup.h
+ ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../Constraints/TestHingeTorque.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -20,6 +20,7 @@
 #include "../Constraints/Dof6Spring2Setup.h"
 #include "../Constraints/ConstraintPhysicsSetup.h"
 #include "../MultiBody/TestJointTorqueSetup.h"
+#include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../VoronoiFracture/VoronoiFractureDemo.h"
 #include "../SoftDemo/SoftDemo.h"
@@ -83,7 +84,6 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc),
     ExampleEntry(1,"TestHingeTorque", "Apply a torque in the hinge axis. This example uses a btHingeConstraint and btRigidBody. The setup is similar to the multi body example TestJointTorque.",
                  TestHingeTorqueCreateFunc),
-    
 //	ExampleEntry(0,"What's new in 2.83"),
 	
 	ExampleEntry(1,"6DofSpring2","Show the use of the btGeneric6DofSpring2Constraint. This is a replacement of the btGeneric6DofSpringConstraint, it has various improvements. This includes improved spring implementation and better control over the restitution (bounce) when the constraint hits its limits.", 
@@ -99,6 +99,7 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(0,"MultiBody"),
 	ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
 	ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
+ ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 	
 	
 #ifdef INCLUDE_CLOTH_DEMOS

examples/ExampleBrowser/premake4.lua

@@ -73,6 +73,7 @@
 		"../Raycast/*",
 		"../MultiBody/MultiDofDemo.cpp",
 		"../MultiBody/TestJointTorqueSetup.cpp",
+		"../MultiBody/MultiBodyConstraintFeedback.cpp",
 		"../ThirdPartyLibs/stb_image/*",
 		"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
 		"../ThirdPartyLibs/tinyxml/*",

examples/MultiBody/MultiBodyConstraintFeedback.cpp

@@ -0,0 +1,432 @@
+#include "MultiBodyConstraintFeedback.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+							
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+static btScalar radius(0.2);
+
+struct MultiBodyConstraintFeedbackSetup : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+	btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+	btMultiBodyJointMotor* m_motor;
+    bool m_once;
+public:
+
+    MultiBodyConstraintFeedbackSetup(struct GUIHelperInterface* helper);
+    virtual ~MultiBodyConstraintFeedbackSetup();
+
+    virtual void initPhysics();
+
+    virtual void stepSimulation(float deltaTime);
+
+	virtual void resetCamera()
+	{
+		float dist = 5;
+		float pitch = 270;
+		float yaw = 21;
+		float targetPos[3]={-1.34,3.4,-0.44};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+
+
+};
+
+MultiBodyConstraintFeedbackSetup::MultiBodyConstraintFeedbackSetup(struct GUIHelperInterface* helper)
+:CommonMultiBodyBase(helper),
+m_motor(0),
+m_once(true)
+{
+}
+
+MultiBodyConstraintFeedbackSetup::~MultiBodyConstraintFeedbackSetup()
+{
+
+}
+
+///this is a temporary global, until we determine if we need the option or not
+extern  bool gJointFeedbackInWorldSpace;
+extern bool gJointFeedbackInJointFrame;
+void MultiBodyConstraintFeedbackSetup::initPhysics()
+{
+    int upAxis = 2;
+	gJointFeedbackInWorldSpace = true;
+	gJointFeedbackInJointFrame = true;
+	m_guiHelper->setUpAxis(upAxis);
+
+    btVector4 colors[4] =
+    {
+        btVector4(1,0,0,1),
+        btVector4(0,1,0,1),
+        btVector4(0,1,1,1),
+        btVector4(1,1,0,1),
+    };
+    int curColor = 0;
+
+
+
+    
+
+	this->createEmptyDynamicsWorld();
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+        //btIDebugDraw::DBG_DrawConstraints
+        +btIDebugDraw::DBG_DrawWireframe
+        +btIDebugDraw::DBG_DrawContactPoints
+        +btIDebugDraw::DBG_DrawAabb
+        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+	
+
+    //create a static ground object
+    if (1)
+        {
+            btVector3 groundHalfExtents(10,10,0.2);
+            btBoxShape* box = new btBoxShape(groundHalfExtents);
+            box->initializePolyhedralFeatures();
+
+            m_guiHelper->createCollisionShapeGraphicsObject(box);
+            btTransform start; start.setIdentity();
+            btVector3 groundOrigin(-0.4f, 3.f, 0.f);
+			btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
+            groundOrigin[upAxis] -=.5;
+			groundOrigin[2]-=0.6;
+            start.setOrigin(groundOrigin);
+			btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
+		
+		//	start.setRotation(groundOrn);
+            btRigidBody* body =  createRigidBody(0,start,box);
+			body->setFriction(0);
+            btVector4 color = colors[curColor];
+			curColor++;
+			curColor&=3;
+            m_guiHelper->createRigidBodyGraphicsObject(body,color);
+        }
+
+    {
+        bool floating = false;
+        bool damping = false;
+        bool gyro = false;
+        int numLinks = 2;
+        bool spherical = false;					//set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = false;
+          btVector3 linkHalfExtents(0.05, 0.5, 0.1);
+        btVector3 baseHalfExtents(0.05, 0.5, 0.1);
+
+        btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
+        //mbC->forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
+        //init the base
+        btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+        float baseMass = 0.01f;
+
+        if(baseMass)
+        {
+            //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+			btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+            shape->calculateLocalInertia(baseMass, baseInertiaDiag);
+            delete shape;
+        }
+
+        bool isMultiDof = true;
+        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
+		
+        m_multiBody = pMultiBody;
+        btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+	//	baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
+        pMultiBody->setBasePos(basePosition);
+        pMultiBody->setWorldToBaseRot(baseOriQuat);
+        btVector3 vel(0, 0, 0);
+    //	pMultiBody->setBaseVel(vel);
+
+        //init the links
+        btVector3 hingeJointAxis(1, 0, 0);
+        
+        //y-axis assumed up
+        btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);						//par body's COM to cur body's COM offset
+        btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);							//cur body's COM to cur body's PIV offset
+        btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;	//par body's COM to cur body's PIV offset
+
+        //////
+        btScalar q0 = 0.f * SIMD_PI/ 180.f;
+        btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+        quat0.normalize();
+        /////
+
+        for(int i = 0; i < numLinks; ++i)
+        {
+			float linkMass = i==0? 0.0001 : 1.f;
+			//if (i==3 || i==2)
+			//	linkMass= 1000;
+			btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+
+			btCollisionShape* shape = 0;
+			if (i==0)
+			{
+				shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
+			} else
+			{
+				shape = new btSphereShape(radius);
+			}
+			shape->calculateLocalInertia(linkMass, linkInertiaDiag);
+			delete shape;
+
+
+            if(!spherical)
+			{
+                //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
+		
+				if (i==0)
+				{
+				pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, 
+					btQuaternion(0.f, 0.f, 0.f, 1.f), 
+					hingeJointAxis, 
+					parentComToCurrentPivot, 
+					currentPivotToCurrentCom, false);
+				} else
+				{
+					btVector3 parentComToCurrentCom(0, -linkHalfExtents[1], 0);						//par body's COM to cur body's COM offset
+					btVector3 currentPivotToCurrentCom(0, 0, 0);							//cur body's COM to cur body's PIV offset
+					//btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;	//par body's COM to cur body's PIV offset
+
+
+					pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, 
+					btQuaternion(0.f, 0.f, 0.f, 1.f), 
+					parentComToCurrentPivot, 
+					currentPivotToCurrentCom, false);
+				}
+					
+				//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
+		
+			}
+            else
+			{
+                //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
+                pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
+			}
+        }
+
+        pMultiBody->finalizeMultiDof();
+
+		//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
+			for (int i=0;i<pMultiBody->getNumLinks();i++)
+		{
+			btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
+			pMultiBody->getLink(i).m_jointFeedback = fb;
+			m_jointFeedbacks.push_back(fb);
+			//break;
+		}
+        btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
+
+        ///
+        world->addMultiBody(pMultiBody);
+        btMultiBody* mbC = pMultiBody;
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+        //
+        if(!damping)
+        {
+            mbC->setLinearDamping(0.f);
+            mbC->setAngularDamping(0.f);
+        }else
+        {	mbC->setLinearDamping(0.1f);
+            mbC->setAngularDamping(0.9f);
+        }
+        //
+    	m_dynamicsWorld->setGravity(btVector3(0,0,-10));
+
+        //////////////////////////////////////////////
+        if(0)//numLinks > 0)
+        {
+            btScalar q0 = 45.f * SIMD_PI/ 180.f;
+            if(!spherical)
+                if(mbC->isMultiDof())
+                    mbC->setJointPosMultiDof(0, &q0);
+                else
+                    mbC->setJointPos(0, q0);
+            else
+            {
+                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+                quat0.normalize();
+                mbC->setJointPosMultiDof(0, quat0);
+            }
+        }
+        ///
+
+        btAlignedObjectArray<btQuaternion> world_to_local;
+        world_to_local.resize(pMultiBody->getNumLinks() + 1);
+
+        btAlignedObjectArray<btVector3> local_origin;
+        local_origin.resize(pMultiBody->getNumLinks() + 1);
+        world_to_local[0] = pMultiBody->getWorldToBaseRot();
+        local_origin[0] = pMultiBody->getBasePos();
+        double friction = 1;
+        {
+
+        //	float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+            float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
+
+
+            if (1)
+            {
+                btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
+                m_guiHelper->createCollisionShapeGraphicsObject(shape);
+
+                btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
+                col->setCollisionShape(shape);
+
+                btTransform tr;
+                tr.setIdentity();
+//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
+				//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
+               
+                tr.setOrigin(local_origin[0]);
+				btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
+				
+                tr.setRotation(orn);
+                col->setWorldTransform(tr);
+
+				bool isDynamic = (baseMass > 0 && floating);
+				short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+				short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+
+                world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
+
+                btVector3 color(0.0,0.0,0.5);
+                m_guiHelper->createCollisionObjectGraphicsObject(col,color);
+
+//                col->setFriction(friction);
+                pMultiBody->setBaseCollider(col);
+
+            }
+        }
+
+
+        for (int i=0; i < pMultiBody->getNumLinks(); ++i)
+        {
+            const int parent = pMultiBody->getParent(i);
+            world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
+            local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
+        }
+
+
+        for (int i=0; i < pMultiBody->getNumLinks(); ++i)
+        {
+
+            btVector3 posr = local_origin[i+1];
+        //	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+
+            float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
+			btCollisionShape* shape =0;
+
+			if (i==0)
+			{
+				shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
+			} else
+			{
+				
+				shape = new btSphereShape(radius);
+			}
+
+            m_guiHelper->createCollisionShapeGraphicsObject(shape);
+            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+
+            col->setCollisionShape(shape);
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+            col->setWorldTransform(tr);
+     //       col->setFriction(friction);
+			bool isDynamic = 1;//(linkMass > 0);
+			short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+			short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+			//if (i==0||i>numLinks-2)
+			{
+				world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
+				   btVector4 color = colors[curColor];
+			curColor++;
+			curColor&=3;
+            m_guiHelper->createCollisionObjectGraphicsObject(col,color);
+
+
+            pMultiBody->getLink(i).m_collider=col;
+			}
+         
+        }
+	int link=0;
+	int targetVelocity=0.f;
+	btScalar maxForce = 100000;
+	 m_motor = new btMultiBodyJointMotor(pMultiBody,link,targetVelocity,maxForce);
+	m_dynamicsWorld->addMultiBodyConstraint(m_motor);
+    }
+}
+
+void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
+{
+	//m_multiBody->addLinkForce(0,btVector3(100,100,100));
+    if (0)//m_once)
+    {
+       m_once=false;
+        m_multiBody->addJointTorque(0, 10.0);
+    
+        btScalar torque = m_multiBody->getJointTorque(0);
+        b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
+    }
+   btScalar timeStep = 1./240.f;
+ 
+    m_dynamicsWorld->stepSimulation(timeStep,0);
+
+	static int count = 0;
+	if ((count& 0x0f)==0)
+	{
+	if (m_motor)
+	{
+	float force = m_motor->getAppliedImpulse(0)/timeStep;
+	b3Printf("motor applied force = %f\n", force);
+	}	
+
+	for (int i=0;i<m_jointFeedbacks.size();i++)
+	{
+			b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
+			i,
+			m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
+			m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
+			m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
+
+		m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
+			m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
+			m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
+
+		);
+
+	}
+	}
+	count++;
+
+
+	/*
+    b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
+             m_multiBody->getBaseOmega()[1],
+             m_multiBody->getBaseOmega()[2]
+             );
+    */
+    btScalar jointVel =m_multiBody->getJointVel(0);
+    
+//    b3Printf("child angvel = %f",jointVel);
+    
+    
+    
+}
+
+
+class CommonExampleInterface*    MultiBodyConstraintFeedbackCreateFunc(struct CommonExampleOptions& options)
+{
+	return new MultiBodyConstraintFeedbackSetup(options.m_guiHelper);
+}

examples/MultiBody/MultiBodyConstraintFeedback.h

@@ -0,0 +1,7 @@
+#ifndef MULTIBODY_CONSTRAINT_FEEDBACK_H
+#define MULTIBODY_CONSTRAINT_FEEDBACK_H
+
+class CommonExampleInterface*    MultiBodyConstraintFeedbackCreateFunc(struct CommonExampleOptions& options);
+
+#endif //MULTIBODY_CONSTRAINT_FEEDBACK_H
+

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -6,7 +6,7 @@
 							
 #include "../CommonInterfaces/CommonMultiBodyBase.h"
 
-btScalar radius(0.2);
+static btScalar radius(0.2);
 
 struct TestJointTorqueSetup : public CommonMultiBodyBase
 {
@@ -48,10 +48,16 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
 
 ///this is a temporary global, until we determine if we need the option or not
 extern  bool gJointFeedbackInWorldSpace;
+extern bool gJointFeedbackInJointFrame;
+
+
+
 void TestJointTorqueSetup::initPhysics()
 {
     int upAxis = 1;
 	gJointFeedbackInWorldSpace = true;
+	gJointFeedbackInJointFrame = true;
+
 	m_guiHelper->setUpAxis(upAxis);
 
     btVector4 colors[4] =
@@ -417,4 +423,4 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
 class CommonExampleInterface*    TestJointTorqueCreateFunc(struct CommonExampleOptions& options)
 {
 	return new TestJointTorqueSetup(options.m_guiHelper);
-}
+}

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -58,6 +58,7 @@ protected:
 	btScalar		m_maxAppliedImpulse;
 
 
+    // warning: the data block lay out is not consistent for all constraints
     // data block laid out as follows:
     // cached impulses. (one per row.)
     // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
@@ -108,6 +109,19 @@ public:
 		return m_bodyB;
 	}
 
+	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+	{
+		btAssert(dof>=0);
+		btAssert(dof < getNumRows());
+		m_data[dof] = appliedImpulse;
+	}
+	
+	btScalar	getAppliedImpulse(int dof)
+	{
+		btAssert(dof>=0);
+		btAssert(dof < getNumRows());
+		return m_data[dof];
+	}
 	// current constraint position
     // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
     // NOTE: ignored position for friction rows.

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -612,7 +612,9 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
-	solverConstraint.m_useJointForce = false;
+    solverConstraint.m_orgConstraint = 0;
+    solverConstraint.m_orgDofIndex = -1;
+    
 	solverConstraint.m_frictionIndex = frictionIndex;
 	bool isFriction = true;
 
@@ -680,10 +682,11 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			int frictionIndex = m_multiBodyNormalContactConstraints.size();
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
-			solverConstraint.m_useJointForce = false;
 
 	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+            solverConstraint.m_orgConstraint = 0;
+            solverConstraint.m_orgDofIndex = -1;
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 			solverConstraint.m_multiBodyA = mbA;
@@ -891,6 +894,11 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 	
 	//bod->addBaseForce(m_gravity * bod->getBaseMass());
 	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+
+	if (c.m_orgConstraint)
+	{
+		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
+	}
 	
 
 	if (c.m_multiBodyA)

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -114,6 +114,9 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		btScalar direction = row? -1 : 1;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+        constraintRow.m_orgConstraint = this;
+        constraintRow.m_orgDofIndex = row;
+        
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
 		const btScalar posError = 0;						//why assume it's zero?
@@ -123,7 +126,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 
 		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)

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -116,10 +116,10 @@ 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_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
 		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)

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -33,6 +33,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
 	m_pivotInA(pivotInA),
 	m_pivotInB(pivotInB)
 {
+    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
@@ -42,6 +43,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
 	m_pivotInA(pivotInA),
 	m_pivotInB(pivotInB)
 {
+    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
 }
 
 void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -108,7 +110,8 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
         //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
-		constraintRow.m_useJointForce = false;
+	constraintRow.m_orgConstraint = this;
+	constraintRow.m_orgDofIndex = i;
         constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
         constraintRow.m_contactNormal1.setValue(0,0,0);
         constraintRow.m_relpos2CrossNormal.setValue(0,0,0);

src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 
 class btMultiBody;
+class btMultiBodyConstraint;
 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
@@ -28,7 +29,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_useJointForce(false)
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
 	{}
 
 	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
@@ -73,7 +74,9 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 	btMultiBody* m_multiBodyB;
 	int			m_linkB;
 
-	bool		m_useJointForce;//needed for write-back of joint versus link/base force/torque
+	//for writing back applied impulses
+	btMultiBodyConstraint*	m_orgConstraint;
+	int m_orgDofIndex;
 
 	enum		btSolverConstraintType
 	{