From 63446558cd10cf22a0c40fb876ca9ad8c61fa98b Mon Sep 17 00:00:00 2001 From: Stephen Tu Date: Thu, 28 Oct 2021 12:52:15 -0700 Subject: Fix bug in server handling of inverse dynamics The current implementation of processInverseDynamicsCommand has two issues: 1. For floating bases, the handler is not properly copying over the position and orientation from the client inputs. 2. For floating bases, the handler is ignoring the position of the joint angles from the client inputs. This CL fixes both issues. While the fix for (2) is straightforward, I am not sure whether or not the right pybullet API convention is to have (x, y, z, orn) passed in, or instead (orn, x, y, z). The former makes more sense to me and is what I implemented. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 708ea7463..701b7ef1b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -11542,17 +11542,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; @@ -11562,12 +11562,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++) { -- cgit v1.2.1