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

Implement CMD_INIT_POSE to set base position, base orientation and joint position (angle).

Browse Source 
See PhysicsClientExample for example use of CMD_INIT_POSE.
Change: CMD_INIT_POSE uses m_initPoseArgs and not m_sendDesiredStateCommandArgument
This commit is contained in:
erwin coumans
2015-10-13 22:23:28 -07:00
parent 4a29986662
commit a0e507280c
6 changed files with 207 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1252,15 +1252,38 @@ void PhysicsServerSharedMemory::processClientCommands()
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
mb->setBasePos(btVector3(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[2]));
mb->setWorldToBaseRot(btQuaternion(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[5],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[6]));
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
btVector3 zero(0,0,0);
mb->setBaseVel(zero);
mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0],
clientCmd.m_initPoseArgs.m_initialStateQ[1],
clientCmd.m_initPoseArgs.m_initialStateQ[2]));
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
{
mb->setBaseOmega(btVector3(0,0,0));
mb->setWorldToBaseRot(btQuaternion(
clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]));
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
{
int dofIndex = 7;
for (int i=0;i<mb->getNumLinks();i++)
{
if (mb->getLink(i).m_dofCount==1)
{
mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]);
mb->setJointVel(i,0);
}
dofIndex += mb->getLink(i).m_dofCount;
}
}
}
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);