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

deal with 1-DOF and 3-DOF joints separately in resetJointState, fixes Issue 2076

Browse Source
This commit is contained in:
erwincoumans
2019-01-26 17:53:10 -08:00
parent a6244d714e
commit 52c8e14646
1 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -68,6 +68,7 @@
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN #endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifdef STATIC_LINK_SPD_PLUGIN #ifdef STATIC_LINK_SPD_PLUGIN
#include "plugins/stablePDPlugin/BulletConversion.h" #include "plugins/stablePDPlugin/BulletConversion.h"
#include "plugins/stablePDPlugin/RBDModel.h" #include "plugins/stablePDPlugin/RBDModel.h"
@@ -8298,17 +8299,25 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
} }
} }
if (hasPosVar) if (hasPosVar)
{
if (mb->getLink(i).m_dofCount == 1)
{
mb->setJointPos(i, clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
mb->setJointVel(i, 0);//backwards compatibility
}
if (mb->getLink(i).m_dofCount == 3)
{ {
btQuaternion q( btQuaternion q(
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex], clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+1], clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 1],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+2], clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 2],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+3]); clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 3]);
q.normalize(); q.normalize();
mb->setJointPosMultiDof(i, &q[0]); mb->setJointPosMultiDof(i, &q[0]);
double vel[6] = { 0, 0, 0, 0, 0, 0 }; double vel[6] = { 0, 0, 0, 0, 0, 0 };
mb->setJointVelMultiDof(i, vel); mb->setJointVelMultiDof(i, vel);
} }
}
bool hasVel = true; bool hasVel = true;
for (int j = 0; j < mb->getLink(i).m_dofCount; j++) for (int j = 0; j < mb->getLink(i).m_dofCount; j++)
@@ -8321,9 +8330,17 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
} }
if (hasVel) if (hasVel)
{
if (mb->getLink(i).m_dofCount == 1)
{
btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
mb->setJointVel(i, vel);
}
if (mb->getLink(i).m_dofCount == 3)
{ {
mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]); mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
} }
}
posVarCountIndex += mb->getLink(i).m_posVarCount; posVarCountIndex += mb->getLink(i).m_posVarCount;
uDofIndex += mb->getLink(i).m_dofCount; uDofIndex += mb->getLink(i).m_dofCount;