AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-19 05:20:06 +00:00
Code Issues Releases Wiki Activity

Merge pull request #4016 from stephentu/patch-1

Browse Source 
Fix bug in server handling of inverse dynamics
This commit is contained in:
erwincoumans 2021-11-12 05:28:32 +00:00 committed by GitHub
commit 88c04a8eae
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -11555,17 +11555,17 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
//PyBullet expects xyz and quaternion in that order, so convert and swap to have a more consistent PyBullet API
if (baseDofQ)
{
btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[4],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[5],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[6]);
btScalar yawZ, pitchY, rollX;
orn.getEulerZYX(yawZ, pitchY, rollX);
q[0] = rollX;
@ -11575,12 +11575,9 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
q[4] = pos[1];
q[5] = pos[2];
}
else
for (int i = 0; i < num_dofs; i++)
{
for (int i = 0; i < num_dofs; i++)
{
q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
}
q[i + baseDofQ] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
}
for (int i = 0; i < num_dofs + baseDofQdot; i++)
{