more work-in-progress on joint-torque sensor sample and server/client shared memory API

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -6,7 +6,7 @@
 							
 #include "../CommonInterfaces/CommonMultiBodyBase.h"
 
-
+btScalar radius(0.2);
 
 struct TestJointTorqueSetup : public CommonMultiBodyBase
 {
@@ -78,17 +78,21 @@ void TestJointTorqueSetup::initPhysics()
     //create a static ground object
     if (1)
         {
-            btVector3 groundHalfExtents(20,20,20);
+            btVector3 groundHalfExtents(1,1,0.2);
             groundHalfExtents[upAxis]=1.f;
             btBoxShape* box = new btBoxShape(groundHalfExtents);
             box->initializePolyhedralFeatures();
 
             m_guiHelper->createCollisionShapeGraphicsObject(box);
             btTransform start; start.setIdentity();
-            btVector3 groundOrigin(0,0,0);
+            btVector3 groundOrigin(-0.4f, 3.f, 0.f);
-            groundOrigin[upAxis]=-1.5;
+			btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
+            groundOrigin[upAxis] -=.5;
+			groundOrigin[2]-=0.6;
             start.setOrigin(groundOrigin);
+			
             btRigidBody* body =  createRigidBody(0,start,box);
+			body->setFriction(0);
             btVector4 color = colors[curColor];
 			curColor++;
 			curColor&=3;
@@ -99,7 +103,7 @@ void TestJointTorqueSetup::initPhysics()
         bool floating = false;
         bool damping = true;
         bool gyro = false;
-        int numLinks = 1;
+        int numLinks = 2;
         bool spherical = false;					//set it ot false -to use 1DoF hinges instead of 3DoF sphericals
         bool canSleep = false;
         bool selfCollide = false;
@@ -161,11 +165,25 @@ void TestJointTorqueSetup::initPhysics()
 			{
                 //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, -radius * 2.f, 0);						//par body's COM to cur body's COM offset
+					btVector3 currentPivotToCurrentCom(0, -radius, 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);
 		
@@ -255,7 +273,9 @@ void TestJointTorqueSetup::initPhysics()
 				//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
                
                 tr.setOrigin(local_origin[0]);
-                tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+				btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
+				
+                tr.setRotation(orn);
                 col->setWorldTransform(tr);
 
 				bool isDynamic = (baseMass > 0 && floating);
@@ -290,8 +310,17 @@ void TestJointTorqueSetup::initPhysics()
         //	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);
+			}
 
-            btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
             m_guiHelper->createCollisionShapeGraphicsObject(shape);
             btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
 
@@ -326,7 +355,7 @@ void TestJointTorqueSetup::initPhysics()
 void TestJointTorqueSetup::stepSimulation(float deltaTime)
 {
 	//m_multiBody->addLinkForce(0,btVector3(100,100,100));
-    if (0)//m_once)
+    if (1)//m_once)
     {
        m_once=false;
         m_multiBody->addJointTorque(0, 10.0);

examples/SharedMemory/PhysicsClient.cpp

@@ -10,6 +10,7 @@
 
 class PhysicsClient : public SharedMemoryCommon
 {
+protected:
 	SharedMemoryInterface* m_sharedMemory;
 	SharedMemoryExampleData*   m_testBlock1;
     int m_counter;
@@ -19,7 +20,8 @@ class PhysicsClient : public SharedMemoryCommon
 	bool m_serverLoadUrdfOK;
 	bool m_waitingForServer;
 	
-
+	void	processServerCommands();
+	void	createClientCommand();
 
     
 public:
@@ -99,7 +101,7 @@ m_wantsTermination(false),
 m_serverLoadUrdfOK(false),
 m_waitingForServer(false)
 {
-	b3Printf("Started PhysicsClient");
+	b3Printf("Started PhysicsClient\n");
 #ifdef _WIN32
 	m_sharedMemory = new Win32SharedMemoryClient();
 #else
@@ -109,7 +111,7 @@ m_waitingForServer(false)
 
 PhysicsClient::~PhysicsClient()
 {
-    b3Printf("~PhysicsClient");
+    b3Printf("~PhysicsClient\n");
     m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
 	delete m_sharedMemory;
 }
@@ -117,45 +119,61 @@ PhysicsClient::~PhysicsClient()
 
 void	PhysicsClient::initPhysics()
 {
+	if (m_guiHelper && m_guiHelper->getParameterInterface())
 	{
-		bool isTrigger = false;
+		{
-		ButtonParams button("Load URDF",CMD_LOAD_URDF,  isTrigger);
+			bool isTrigger = false;
-		button.m_callback = MyCallback;
+			ButtonParams button("Load URDF",CMD_LOAD_URDF,  isTrigger);
-		button.m_userPointer = this;
+			button.m_callback = MyCallback;
-		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+			button.m_userPointer = this;
-    }
+			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+		}
 
-	{
+		{
-		bool isTrigger = false;
+			bool isTrigger = false;
-		ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
+			ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
-		button.m_callback = MyCallback;
+			button.m_callback = MyCallback;
-		button.m_userPointer = this;
+			button.m_userPointer = this;
-		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
-    }
+		}
 
+		{
+			bool isTrigger = false;
+			ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
+			button.m_callback = MyCallback;
+			button.m_userPointer = this;
+			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+		}
+		
+		{
+			bool isTrigger = false;
+			ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
+			button.m_callback = MyCallback;
+			button.m_userPointer = this;
+			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+		}
+		
+		{
+			bool isTrigger = false;
+			ButtonParams button("Shut Down",CMD_SHUTDOWN,  isTrigger);
+			button.m_callback = MyCallback;
+			button.m_userPointer = this;
+			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+		}
+	} else
 	{
-		bool isTrigger = false;
+		m_userCommandRequests.push_back(CMD_LOAD_URDF);
-		ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
-		button.m_callback = MyCallback;
+		//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
-		button.m_userPointer = this;
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
-		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+		//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
-    }
+		//m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
-	/*
+		//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
-	{
+		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
-		bool isTrigger = false;
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
-		ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
+		m_userCommandRequests.push_back(CMD_SHUTDOWN);
-		button.m_callback = MyCallback;
+		
-		button.m_userPointer = this;
+	}
-		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
-    }
-	*/
-	{
-		bool isTrigger = false;
-		ButtonParams button("Shut Down",CMD_SHUTDOWN,  isTrigger);
-		button.m_callback = MyCallback;
-		button.m_userPointer = this;
-		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
-    }
 
 
     m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
@@ -164,106 +182,119 @@ void	PhysicsClient::initPhysics()
      //   btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
         if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
         {
-            b3Error("Error: please start server before client");
+            b3Error("Error: please start server before client\n");
             m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
             m_testBlock1 = 0;
         } else
 		{
-			b3Printf("Shared Memory status is OK");
+			b3Printf("Shared Memory status is OK\n");
 		}
-    }
+    } else
-    
+	{
+		m_wantsTermination = true;
+	}
 }
 
-void	PhysicsClient::stepSimulation(float deltaTime)
+void	PhysicsClient::processServerCommands()
 {
-    
+	btAssert(m_testBlock1);
-    
-    if (m_testBlock1)
-    {
-        //check progress and submit further commands
-        //we ignore overflow right now
-        
-        if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
-        {
-            btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
-        
-            const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
-            
-            //consume the command
-            switch (serverCmd.m_type)
-            {
 
-				case CMD_URDF_LOADING_COMPLETED:
+	if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
+	{
+		btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
+        
+		const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
+            
+		//consume the command
+		switch (serverCmd.m_type)
+		{
+
+			case CMD_URDF_LOADING_COMPLETED:
+			{
+				m_serverLoadUrdfOK = true;
+				b3Printf("Server loading the URDF OK\n");
+				break;
+			}
+			case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
+			{
+				break;
+			}
+			case CMD_URDF_LOADING_FAILED:
+			{
+				b3Printf("Server failed loading the URDF...\n");
+				m_serverLoadUrdfOK = false;
+				break;
+			}
+			case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
 				{
-					m_serverLoadUrdfOK = true;
+					b3Printf("Received actual state\n");
-					b3Printf("Server loading the URDF OK");
+						
+					int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
+					int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
+					b3Printf("size Q = %d, size U = %d\n", numQ,numU);
+					char msg[1024];
+
+					sprintf(msg,"Q=[");
+						
+					for (int i=0;i<numQ;i++)
+					{
+						if (i<numQ-1)
+						{
+							sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]);
+						} else
+						{
+							sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]);
+						}
+					}
+					sprintf(msg,"%s]",msg);
+						
+					b3Printf(msg);
+					b3Printf("\n");
 					break;
 				}
-                case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
+			default:
-                {
-                    break;
-                }
-                case CMD_URDF_LOADING_FAILED:
-                {
-                    b3Printf("Server failed loading the URDF...");
-					m_serverLoadUrdfOK = false;
-                    break;
-                }
-				case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
-					{
-						b3Printf("Received actual state");
-						
-						int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
-						int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
-						b3Printf("size Q = %d, size U = %d\n", numQ,numU);
-						char msg[1024];
-
-						sprintf(msg,"Q=[");
-						
-						for (int i=0;i<numQ;i++)
-						{
-							if (i<numQ-1)
-							{
-								sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]);
-							} else
-							{
-								sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]);
-							}
-						}
-						sprintf(msg,"%s]",msg);
-						
-						b3Printf(msg);
-						
-						break;
-					}
-                default:
-                {
-                    b3Error("Unknown server command");
-                    btAssert(0);
-                }
-            };
-			
-			
-            m_testBlock1->m_numProcessedServerCommands++;
-
-			if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
 			{
-				m_waitingForServer = false;
+				b3Error("Unknown server command\n");
-			} else
+				btAssert(0);
-			{
-				m_waitingForServer = true;
 			}
-        }
+		};
-    
+			
-		if (!m_waitingForServer)
+			
+		m_testBlock1->m_numProcessedServerCommands++;
+		//we don't have more than 1 command outstanding (in total, either server or client)
+		btAssert(m_testBlock1->m_numProcessedServerCommands == m_testBlock1->m_numServerCommands);
+
+		if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
+		{
+			m_waitingForServer = false;
+		} else
+		{
+			m_waitingForServer = true;
+		}
+	}
+}
+
+void	PhysicsClient::createClientCommand()
+{
+	if (!m_waitingForServer)
 		{
 			//process outstanding requests
 			if (m_userCommandRequests.size())
 			{
 				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
 				int command = m_userCommandRequests[0];
-				m_userCommandRequests.remove(command);
+				
+				//don't use 'remove' because it will re-order the commands
+				//m_userCommandRequests.remove(command);
+				//pop_front
+				for (int i=1;i<m_userCommandRequests.size();i++)
+				{
+					m_userCommandRequests[i-1] = m_userCommandRequests[i];
+				}
+
+				m_userCommandRequests.pop_back();
+				m_waitingForServer = true;
+
 				switch (command)
 				{
 				case CMD_LOAD_URDF:
@@ -276,11 +307,10 @@ void	PhysicsClient::stepSimulation(float deltaTime)
 							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
 
 							m_testBlock1->m_numClientCommands++;
-							b3Printf("Client created CMD_LOAD_URDF");
+							b3Printf("Client created CMD_LOAD_URDF\n");
-							m_waitingForServer = true;
 						} else
 						{
-							b3Warning("Server already loaded URDF, no client command submitted");
+							b3Warning("Server already loaded URDF, no client command submitted\n");
 						}
 						break;
 					}
@@ -288,13 +318,13 @@ void	PhysicsClient::stepSimulation(float deltaTime)
 					{
 						if (m_serverLoadUrdfOK)
 						{
-							b3Printf("Requesting actual state");
+							b3Printf("Requesting actual state\n");
 							m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
 							m_testBlock1->m_numClientCommands++;
 
 						} else
 						{
-							b3Warning("No URDF loaded");
+							b3Warning("No URDF loaded\n");
 						}
 						break;
 					}
@@ -307,10 +337,9 @@ void	PhysicsClient::stepSimulation(float deltaTime)
 							m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
 							m_testBlock1->m_numClientCommands++;
 							b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
-							m_waitingForServer = true;
 						} else
 						{
-							b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted");
+							b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
 						}
 						break;
 					}
@@ -319,20 +348,33 @@ void	PhysicsClient::stepSimulation(float deltaTime)
 						m_wantsTermination = true;
 						m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
 						m_testBlock1->m_numClientCommands++;
-						m_waitingForServer = true;
 						m_serverLoadUrdfOK = false;
 						b3Printf("client created CMD_SHUTDOWN\n");
 						break;
 					}
 				default:
 					{
-						b3Error("unknown command requested");
+						b3Error("unknown command requested\n");
 					}
 				}
 			}
 		}
 
-	  }
+}
+void	PhysicsClient::stepSimulation(float deltaTime)
+{
+    
+    btAssert(m_testBlock1);
+
+    if (m_testBlock1)
+    {
+		processServerCommands();
+    
+		if (!m_waitingForServer)
+		{
+			createClientCommand();
+		}
+	}
 	
 
 }

examples/SharedMemory/PhysicsServer.cpp

@@ -62,8 +62,10 @@ m_wantsShutdown(false)
 
 void PhysicsServer::releaseSharedMemory()
 {
+	b3Printf("releaseSharedMemory1\n");
     if (m_testBlock1)
     {
+		b3Printf("m_testBlock1\n");
         m_testBlock1->m_magicId = 0;
         b3Printf("magic id = %d\n",m_testBlock1->m_magicId);
         btAssert(m_sharedMemory);
@@ -71,7 +73,7 @@ void PhysicsServer::releaseSharedMemory()
     }
     if (m_sharedMemory)
     {
-
+		b3Printf("m_sharedMemory\n");
         delete m_sharedMemory;
         m_sharedMemory = 0;
         m_testBlock1 = 0;
@@ -343,6 +345,8 @@ void	PhysicsServer::stepSimulation(float deltaTime)
     }
     if (wantsShutdown)
     {
+		b3Printf("releaseSharedMemory!\n");
+		
         m_wantsShutdown = true;
         releaseSharedMemory();
     }

examples/SharedMemory/SharedMemoryCommands.h

@@ -4,19 +4,29 @@
 
 //this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
 
-enum SharedMemoryServerCommand{
+enum SharedMemoryServerCommand
+{
     CMD_URDF_LOADING_COMPLETED,
     CMD_URDF_LOADING_FAILED,
+	CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
+	CMD_RIGID_BODY_CREATION_COMPLETED,
+	CMD_SET_JOINT_FEEDBACK_COMPLETED,
 	CMD_ACTUAL_STATE_UPDATE_COMPLETED,
-//	CMD_DESIRED_STATE_RECEIVED_COMPLETED,
+	CMD_DESIRED_STATE_RECEIVED_COMPLETED,
     CMD_STEP_FORWARD_SIMULATION_COMPLETED,
     CMD_MAX_SERVER_COMMANDS
 };
 
-enum SharedMemoryClientCommand{
+enum SharedMemoryClientCommand
+{
     CMD_LOAD_URDF,
+	CMD_CREATE_BOX_COLLISION_SHAPE,
+	CMD_DELETE_BOX_COLLISION_SHAPE,
+	CMD_CREATE_RIGID_BODY,
+	CMD_DELETE_RIGID_BODY,
+	CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback
+	CMD_SEND_DESIRED_STATE,
 	CMD_REQUEST_ACTUAL_STATE,
-//	CMD_SEND_DESIRED_STATE,
     CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE
     CMD_SHUTDOWN,
     CMD_MAX_CLIENT_COMMANDS
@@ -35,6 +45,13 @@ struct UrdfArgs
 	bool m_useFixedBase;
 };
 
+struct SetJointFeedbackArgs
+{
+	int m_bodyUniqueId;
+	int m_linkId;
+	bool m_isEnabled;
+};
+
 struct SendDesiredStateArgs
 {
 	int m_bodyUniqueId;
@@ -45,6 +62,19 @@ struct RequestActualStateArgs
 	int m_bodyUniqueId;
 };
 
+struct CreateBoxShapeArgs
+{
+	double	m_halfExtents[3];
+};
+
+struct CreateRigidBodyArgs
+{
+	int		m_shapeId;
+	double	m_initialPosition[3];
+	double	m_initialOrientation[3];	
+};
+
+
 struct SendActualStateArgs
 {
 	int m_bodyUniqueId;
@@ -53,6 +83,7 @@ struct SendActualStateArgs
 };
 
 
+
 struct StepSimulationArgs
 {
     double m_deltaTimeInSeconds;

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1027,8 +1027,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 		
 			m_internalNeedsJointFeedback = true;
 			m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
-			m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
+			m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
-			m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			//m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
+			//m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
 		}
 
     }

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -858,6 +858,15 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
 						getDebugDrawer()->drawLine(from,to,color);
 					}
+					if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
+					{
+						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);
+					}
 					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
 					{
 						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);