Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #433 from erwincoumans/master

Browse Source 
allow to control of powered joints after loading a URDF file, through…
This commit is contained in:
erwincoumans
2015-07-20 23:44:20 -07:00
parent 9bbc88cbe1 8e163c984d
commit eb8acf3689
10 changed files with 279 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -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);
}
}

151
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -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);
}
if (mb->m_links[link].m_jointName)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
PoweredJointInfo info;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
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);
}
b3Printf("link [%d] type = %d",link, mb->m_links[link].m_jointType);
if (mb->m_links[link].m_jointName)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
PoweredJointInfo info;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
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;
}
}
}
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]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
if (i<numQ-1)
{
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

25
examples/SharedMemory/PhysicsClient.h
View File

@@ -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();
virtual bool getLastServerStatus(ServerStatus& status)
{
return false;
}
// return true if there is a status, and fill in 'serverStatus'
virtual bool processServerStatus(ServerStatus& serverStatus);
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

13
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -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())
{

10
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -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]*physicsDeltaTime;
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_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(timeStep);
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime);
SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];

64
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -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_desiredStateForceTorque[i] = 100;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
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())

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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;
};

54
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -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;

2
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -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;

54
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -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);
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);
}
}
bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
} else
{
bod->clearVelocities();