diff options
| author | erwincoumans <erwincoumans@google.com> | 2021-11-12 05:28:32 +0000 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2021-11-12 05:28:32 +0000 |
| commit | 88c04a8eaec4d8846a9e21e329a378801aef7753 (patch) | |
| tree | 67804f6e727dfa5149bc99c947803b74129c4d11 | |
| parent | 98254b51a934371e6c6bab6e42d98784c79b3598 (diff) | |
| parent | 63446558cd10cf22a0c40fb876ca9ad8c61fa98b (diff) | |
| download | bullet3-88c04a8eaec4d8846a9e21e329a378801aef7753.tar.gz | |
Merge pull request #4016 from stephentu/patch-1
Fix bug in server handling of inverse dynamics
| -rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 17 |
1 files changed, 7 insertions, 10 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ff6ad05ee..0d826af41 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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++) { |