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

37
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"
@@ -8299,15 +8300,23 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
} }
if (hasPosVar) if (hasPosVar)
{ {
btQuaternion q( if (mb->getLink(i).m_dofCount == 1)
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex], {
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+1], mb->setJointPos(i, clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+2], mb->setJointVel(i, 0);//backwards compatibility
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+3]); }
q.normalize(); if (mb->getLink(i).m_dofCount == 3)
mb->setJointPosMultiDof(i, &q[0]); {
double vel[6] = { 0, 0, 0, 0, 0, 0 }; btQuaternion q(
mb->setJointVelMultiDof(i, vel); clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 1],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 2],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 3]);
q.normalize();
mb->setJointPosMultiDof(i, &q[0]);
double vel[6] = { 0, 0, 0, 0, 0, 0 };
mb->setJointVelMultiDof(i, vel);
}
} }
bool hasVel = true; bool hasVel = true;
@@ -8322,7 +8331,15 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
if (hasVel) if (hasVel)
{ {
mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]); 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]);
}
} }
posVarCountIndex += mb->getLink(i).m_posVarCount; posVarCountIndex += mb->getLink(i).m_posVarCount;