Merge pull request bulletphysics#4585 from ConnorTingley/Floating-Bod…

…y-Inverse-Dynamics Fix calculateInverseDynamics losing one joint DOF for floating bodies
jnhyatt · Apr 9, 2024 · 43d336f · 43d336f
2 parents 5e89410 + 2c79184
commit 43d336f
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -12360,9 +12360,10 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
 					q[4] = pos[1];
 					q[5] = pos[2];
 				}
+				int quatToEulerCorrection =  baseDofQ ? -1 : 0; // The conversion to Euler angles drops the DOF by one
 				for (int i = 0; i < num_dofs; i++)
 				{
-					q[i + baseDofQ] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
+					q[i + baseDofQ + quatToEulerCorrection] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
 				}
 				for (int i = 0; i < num_dofs + baseDofQdot; i++)
 				{