From 2c7918423dace1e7691d78cd93be52cde7649a31 Mon Sep 17 00:00:00 2001 From: ctingley Date: Tue, 27 Feb 2024 07:17:48 -0800 Subject: [PATCH] Quaternion to Euler correction --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1d691c550c..b8fc344337 100644 --- 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++) {