Merge pull request #433 from erwincoumans/master

allow to control of powered joints after loading a URDF file, through…

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -457,7 +457,11 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
 		mb->finalizeMultiDof();
 
 		mb->setBaseWorldTransform(rootTransformInWorldSpace);
-
+		btAlignedObjectArray<btQuaternion> scratch_q;
+		btAlignedObjectArray<btVector3> scratch_m;
+		mb->forwardKinematics(scratch_q,scratch_m);
+		mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
+		
 		world1->addMultiBody(mb);
 	}
 }

examples/SharedMemory/PhysicsClient.cpp

@@ -8,15 +8,26 @@
 #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
 
 
+//copied from btMultiBodyLink.h
+enum JointType
+{
+	eRevoluteType = 0,
+	ePrismaticType = 1,
+};
+
 struct PhysicsClientSharedMemoryInternalData
 {
 	SharedMemoryInterface* m_sharedMemory;
 	SharedMemoryExampleData*   m_testBlock1;
 
+	btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
+	btAlignedObjectArray<PoweredJointInfo> m_poweredJointInfo;
+	
 	int m_counter;
 	bool m_serverLoadUrdfOK;
 	bool m_isConnected;
 	bool m_waitingForServer;
+	bool m_hasLastServerStatus;
 	
 	PhysicsClientSharedMemoryInternalData()
 		:m_sharedMemory(0),
@@ -24,7 +35,8 @@ struct PhysicsClientSharedMemoryInternalData
 		m_counter(0),
 		m_serverLoadUrdfOK(false),
 		m_isConnected(false),
-		m_waitingForServer(false)
+		m_waitingForServer(false),
+		m_hasLastServerStatus(false)
 	{
 	}
 
@@ -37,6 +49,18 @@ struct PhysicsClientSharedMemoryInternalData
 
 };
 
+
+int		PhysicsClientSharedMemory::getNumPoweredJoints() const
+{
+	return m_data->m_poweredJointInfo.size();
+}
+
+void PhysicsClientSharedMemory::getPoweredJointInfo(int index, PoweredJointInfo& info) const
+{
+	info = m_data->m_poweredJointInfo[index];
+}
+
+
 PhysicsClientSharedMemory::PhysicsClientSharedMemory()
 
 {
@@ -99,16 +123,19 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
 
 
 
-void	PhysicsClientSharedMemory::processServerStatus()
+bool	PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus)
 {
 	btAssert(m_data->m_testBlock1);
-
+	bool hasStatus = false;
+	
 	if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
 	{
 		btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
         
 		const SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
-            
+		hasStatus = true;
+		serverStatus = serverCmd;
+		
 		//consume the command
 		switch (serverCmd.m_type)
 		{
@@ -123,10 +150,14 @@ void	PhysicsClientSharedMemory::processServerStatus()
 						bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
 						bf->setFileDNAisMemoryDNA();
 						bf->parse(false);
+						m_data->m_robotMultiBodyData.push_back(bf);
+						
 						for (int i=0;i<bf->m_multiBodies.size();i++)
 						{
 							int flag = bf->getFlags();
-
+							int qOffset = 7;
+							int uOffset=6;
+							
 							if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
 							{
 								Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
@@ -136,15 +167,31 @@ void	PhysicsClientSharedMemory::processServerStatus()
 								}
 								for (int link=0;link<mb->m_numLinks;link++)
 								{
-									if (mb->m_links[link].m_linkName)
+									if ((mb->m_links[link].m_jointType == eRevoluteType)||
+										(mb->m_links[link].m_jointType == ePrismaticType))
 									{
-										b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
+										PoweredJointInfo info;
-									}
+										info.m_qIndex = qOffset;
-									if (mb->m_links[link].m_jointName)
+										info.m_uIndex = uOffset;
-									{
+										
-										b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
+										if (mb->m_links[link].m_linkName)
+										{
+											b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
+											info.m_linkName = mb->m_links[link].m_linkName;
+										}
+										if (mb->m_links[link].m_jointName)
+										{
+											b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
+											info.m_jointName = mb->m_links[link].m_jointName;
+											info.m_jointType = mb->m_links[link].m_jointType;
+										}
+										m_data->m_poweredJointInfo.push_back(info);
 									}
+									qOffset+= mb->m_links[link].m_posVarCount;
+									uOffset+= mb->m_links[link].m_dofCount;
+									
 								}
+								
 							} else
 							{
 								Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
@@ -154,19 +201,39 @@ void	PhysicsClientSharedMemory::processServerStatus()
 								}
 								for (int link=0;link<mb->m_numLinks;link++)
 								{
-									if (mb->m_links[link].m_linkName)
+									if ((mb->m_links[link].m_jointType == eRevoluteType)||
+										(mb->m_links[link].m_jointType == ePrismaticType))
 									{
-										b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
+										PoweredJointInfo info;
-									}
+										info.m_qIndex = qOffset;
-									b3Printf("link [%d] type = %d",link, mb->m_links[link].m_jointType);
+										info.m_uIndex = uOffset;
-									if (mb->m_links[link].m_jointName)
+										
-									{
+										if (mb->m_links[link].m_linkName)
-										b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
+										{
+											b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
+											info.m_linkName = mb->m_links[link].m_linkName;
+										}
+										if (mb->m_links[link].m_jointName)
+										{
+											b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
+											info.m_jointName = mb->m_links[link].m_jointName;
+											info.m_jointType = mb->m_links[link].m_jointType;
+										}
+										m_data->m_poweredJointInfo.push_back(info);
 									}
+									qOffset+= mb->m_links[link].m_posVarCount;
+									uOffset+= mb->m_links[link].m_dofCount;
 								}
+
 							}
 						}
-						printf("ok!\n");
+						if (bf->ok())
+						{
+							b3Printf("Received robot description ok!\n");
+						} else
+						{
+							b3Warning("Robot description not received");
+						}
 					}
 				break;
 			}
@@ -212,21 +279,42 @@ void	PhysicsClientSharedMemory::processServerStatus()
 					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,"Q=[");
+							
+						for (int i=0;i<numQ;i++)
 						{
-							sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
+							if (i<numQ-1)
-						} else
+							{
-						{
+								sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
-							sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
+							} else
+							{
+								sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
+							}
 						}
-					}
+						sprintf(msg,"%s]",msg);
-					sprintf(msg,"%s]",msg);
+					}	
-						
 					b3Printf(msg);
+					
+					{
+						sprintf(msg,"U=[");
+						
+						for (int i=0;i<numU;i++)
+						{
+							if (i<numU-1)
+							{
+								sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
+							} else
+							{
+								sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
+							}
+						}
+						sprintf(msg,"%s]",msg);
+						
+					}
+					b3Printf(msg);
+					
+					
 					b3Printf("\n");
 					break;
 				}
@@ -250,6 +338,7 @@ void	PhysicsClientSharedMemory::processServerStatus()
 			m_data->m_waitingForServer = true;
 		}
 	}
+	return hasStatus;
 }
 
 bool PhysicsClientSharedMemory::canSubmitCommand() const
@@ -378,7 +467,7 @@ bool	PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
 						{
 						
 							m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
-							m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
+						//	m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
 							m_data->m_testBlock1->m_numClientCommands++;
 							b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++);
 						} else

examples/SharedMemory/PhysicsClient.h

@@ -3,6 +3,17 @@
 
 #include "SharedMemoryCommands.h"
 
+#include <string>
+
+struct PoweredJointInfo
+{
+	std::string m_linkName;
+	std::string m_jointName;
+	int m_jointType;
+	int m_qIndex;
+	int m_uIndex;
+};
+
 
 class PhysicsClientSharedMemory  //: public CommonPhysicsClientInterface
 {
@@ -14,22 +25,22 @@ public:
 	PhysicsClientSharedMemory();
 	virtual ~PhysicsClientSharedMemory();
 
-	//todo: implement 'allocateSharedMemory' from client side in 'connect' call
+	//return true if connection succesfull, can also check 'isConnected'
 	virtual bool	connect(bool allowSharedMemoryInitialization = true);
 
 	virtual bool	isConnected() const;
 
-	virtual void	processServerStatus();
+	// return true if there is a status, and fill in 'serverStatus'
-
+	virtual bool	processServerStatus(ServerStatus& serverStatus);
-	virtual bool getLastServerStatus(ServerStatus& status)
-	{
-		return false;
-	}
 	
 	virtual bool	canSubmitCommand() const;
 	
 	virtual bool	submitClientCommand(const SharedMemoryCommand& command);
 
+	virtual int		getNumPoweredJoints() const;
+	
+	virtual void	getPoweredJointInfo(int index, PoweredJointInfo& info) const;
+	
 };
 
 #endif //BT_PHYSICS_CLIENT_API_H

examples/SharedMemory/PhysicsClientExample.cpp

@@ -211,7 +211,18 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
     
 	if (m_physicsClient.isConnected())
     {
-		m_physicsClient.processServerStatus();
+		ServerStatus status;
+		bool hasStatus = m_physicsClient.processServerStatus(status);
+		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
+		{
+			for (int i=0;i<m_physicsClient.getNumPoweredJoints();i++)
+			{
+				PoweredJointInfo info;
+				m_physicsClient.getPoweredJointInfo(i,info);
+				b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName.c_str(),info.m_qIndex,info.m_uIndex);
+				
+			}
+		}
     
 		if (m_physicsClient.canSubmitCommand())
 		{

examples/SharedMemory/PhysicsServer.cpp

@@ -33,7 +33,7 @@ struct PhysicsServerInternalData
 	SharedMemoryInterface* m_sharedMemory;
     SharedMemoryExampleData* m_testBlock1;
 	bool m_isConnected;
-
+	btScalar m_physicsDeltaTime;
 	//btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
 
 	btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
@@ -49,6 +49,7 @@ struct PhysicsServerInternalData
 		:m_sharedMemory(0),
 		m_testBlock1(0),
 		m_isConnected(false),
+		m_physicsDeltaTime(1./60.),
 		m_dynamicsWorld(0),
 		m_guiHelper(0)
 	{
@@ -426,9 +427,7 @@ void PhysicsServerSharedMemory::processClientCommands()
 														btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
 														motor->setVelocityTarget(desiredVelocity);
 
-														btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation'
+														btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
-
-														btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
 														motor->setMaxAppliedImpulse(maxImp);
 														numMotors++;
 
@@ -552,8 +551,7 @@ void PhysicsServerSharedMemory::processClientCommands()
                 {
                    
                     b3Printf("Step simulation request");
-                    double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
+                    m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime);
-                    m_data->m_dynamicsWorld->stepSimulation(timeStep);
                     
                     SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
                     

examples/SharedMemory/RobotControlExample.cpp

@@ -14,7 +14,13 @@
 //const char* blaatnaam = "basename";
 
 
-
+struct MyMotorInfo
+{
+	btScalar m_velTarget;
+	btScalar m_maxForce;
+	int		m_uIndex;
+};
+#define MAX_NUM_MOTORS 128
 
 class RobotControlExample : public SharedMemoryCommon
 {
@@ -27,10 +33,15 @@ class RobotControlExample : public SharedMemoryCommon
 	   
 	btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
 
+		
 	void	createButton(const char* name, int id, bool isTrigger );
 	
 public:
-    
+    //@todo, add accessor methods
+	MyMotorInfo m_motorTargetVelocities[MAX_NUM_MOTORS];
+	int m_numMotors;
+
+	
 	RobotControlExample(GUIHelperInterface* helper);
     
 	virtual ~RobotControlExample();
@@ -73,7 +84,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
 	case  CMD_LOAD_URDF:
 		{
 			command.m_type =CMD_LOAD_URDF;
-			sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf");
+			sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");
 			command.m_urdfArguments.m_initialPosition[0] = 0.0;
 			command.m_urdfArguments.m_initialPosition[1] = 0.0;
 			command.m_urdfArguments.m_initialPosition[2] = 0.0;
@@ -107,6 +118,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
 	
     case CMD_SEND_DESIRED_STATE:
 		{
+			
 				command.m_type =CMD_SEND_DESIRED_STATE;
 				int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
 
@@ -118,8 +130,15 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
 					{
 						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
 						{
-							command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 1;
+							command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
-							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
+							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
+						}
+						for (int i=0;i<cl->m_numMotors;i++)
+						{
+							btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
+							int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
+							command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
+							
 						}
 						break;
 					}
@@ -165,7 +184,8 @@ void	RobotControlExample::createButton(const char* name, int buttonId, bool isTr
 RobotControlExample::RobotControlExample(GUIHelperInterface* helper)
 :SharedMemoryCommon(helper),
 m_wantsShutdown(false),
-m_sequenceNumberGenerator(0)
+m_sequenceNumberGenerator(0),
+m_numMotors(0)
 {
 	
 	bool useServer = true;
@@ -243,8 +263,36 @@ void	RobotControlExample::stepSimulation(float deltaTime)
 
 	if (m_physicsClient.isConnected())
     {
-		m_physicsClient.processServerStatus();
+		
-    
+		ServerStatus status;
+		bool hasStatus = m_physicsClient.processServerStatus(status);
+		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
+		{
+			for (int i=0;i<m_physicsClient.getNumPoweredJoints();i++)
+			{
+				PoweredJointInfo info;
+				m_physicsClient.getPoweredJointInfo(i,info);
+				b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName.c_str(),info.m_qIndex,info.m_uIndex);
+				
+				if (m_numMotors<MAX_NUM_MOTORS)
+				{
+					char motorName[1024];
+					sprintf(motorName,"%s q'", info.m_jointName.c_str());
+					MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
+					motorInfo->m_velTarget = 0.f;
+					motorInfo->m_uIndex = info.m_uIndex;
+				
+					SliderParams slider(motorName,&motorInfo->m_velTarget);
+					slider.m_minVal=-4;
+					slider.m_maxVal=4;
+					m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+					m_numMotors++;
+				}
+				
+			}
+		}
+
+		
 		if (m_physicsClient.canSubmitCommand())
 		{
 			if (m_userCommandRequests.size())

examples/SharedMemory/SharedMemoryCommands.h

@@ -72,6 +72,7 @@ struct UrdfArgs
 struct BulletDataStreamArgs
 {
 	int m_streamChunkLength;
+	int m_bodyUniqueId;
 };
 
 struct SetJointFeedbackArgs
@@ -108,17 +109,7 @@ 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
@@ -136,10 +127,6 @@ struct SendActualStateArgs
 
 
 
-struct StepSimulationArgs
-{
-    double m_deltaTimeInSeconds;
-};
 
 struct SharedMemoryCommand
 {
@@ -152,8 +139,7 @@ struct SharedMemoryCommand
     {
         UrdfArgs m_urdfArguments;
 		BulletDataStreamArgs	m_dataStreamArguments;
-        StepSimulationArgs m_stepSimulationArguments;
+  		SendDesiredStateArgs m_sendDesiredStateCommandArgument;
-		SendDesiredStateArgs m_sendDesiredStateCommandArgument;
 		RequestActualStateArgs m_requestActualStateInformationCommandArgument;
 		SendActualStateArgs m_sendActualStateArgs;
     };

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -2424,6 +2424,58 @@ void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to
 
 }
 
+void	btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
+{
+	world_to_local.resize(getNumLinks()+1);
+	local_origin.resize(getNumLinks()+1);
+	
+	world_to_local[0] = getWorldToBaseRot();
+	local_origin[0] = getBasePos();
+	
+	if (getBaseCollider())
+	{
+		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]));
+		
+		getBaseCollider()->setWorldTransform(tr);
+		
+	}
+	
+	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 m=0;m<getNumLinks();m++)
+	{
+		btMultiBodyLinkCollider* col = getLink(m).m_collider;
+		if (col)
+		{
+			int link = col->m_link;
+			btAssert(link == m);
+			
+			int index = link+1;
+			
+			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]));
+			
+			col->setWorldTransform(tr);
+		}
+	}
+}
 
 int	btMultiBody::calculateSerializeBufferSize()	const
 {
@@ -2458,6 +2510,8 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 
 				memPtr->m_jointType = getLink(i).m_jointType;
 				memPtr->m_dofCount = getLink(i).m_dofCount;
+				memPtr->m_posVarCount = getLink(i).m_posVarCount;
+				
 				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
 				memPtr->m_linkMass = getLink(i).m_mass;
 				memPtr->m_parentIndex = getLink(i).m_parent;

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -599,6 +599,8 @@ void addJointTorque(int i, btScalar Q);
 		return m_internalNeedsJointFeedback;
 	}
 	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
+
+	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
 	
 	virtual	int	calculateSerializeBufferSize()	const;
 

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -759,9 +759,7 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
-				world_to_local.resize(nLinks+1);
+			
-				local_origin.resize(nLinks+1);
-
 				if(bod->isMultiDof())
 				{
 					if(!bod->isPosUpdated())
@@ -776,54 +774,14 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 					}
 				}
 				else
+				{
 					bod->stepPositions(timeStep);			
-
-				world_to_local[0] = bod->getWorldToBaseRot();
-				local_origin[0] = bod->getBasePos();
-
-				if (bod->getBaseCollider())
-				{
-					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]));
-
-					bod->getBaseCollider()->setWorldTransform(tr);
-
-				}
-      
-				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)));
 				}
+				world_to_local.resize(nLinks+1);
+				local_origin.resize(nLinks+1);
 
-
+				bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
-				for (int m=0;m<bod->getNumLinks();m++)
+				
-				{
-					btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
-					if (col)
-					{
-						int link = col->m_link;
-						btAssert(link == m);
-
-						int index = link+1;
-
-						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]));
-
-						col->setWorldTransform(tr);
-					}
-				}
 			} else
 			{
 				bod->clearVelocities();