Fix calculateInverseDynamics losing one joint DOF for floating bodies

[ConnorTingley]

resolves #4578

The issue here is that the last positional degree of freedom of floating bodies is ignored in calculateInverseDynamics.
This is because the indexing of q does not account for the drop in degree of freedom when converting from a quaternion angle (4 DOF) to an Euler angle (3 DOF).

bullet3/examples/SharedMemory/PhysicsServerCommandProcessor.cpp

Line 12333 in 6bb8d11

int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;

With a floating body, baseDofQ is set to be 7.

bullet3/examples/SharedMemory/PhysicsServerCommandProcessor.cpp

Lines 12344 to 12366 in 6bb8d11

	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[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;
	q[1] = pitchY;
	q[2] = yawZ;
	q[3] = pos[0];
	q[4] = pos[1];
	q[5] = pos[2];
	}
	for (int i = 0; i < num_dofs; i++)
	{
	q[i + baseDofQ] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
	}

q[0] through q[5] are set in the if statement. However the for loop starts at q[7], meaning that q[6] is never set and the last joint position is excluded from the array.

This can be fixed by subtracting 1 from the indexing of q (not clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ as this uses a quaternion) in the case of a floating body.

[erwincoumans]

Good catch and thanks for the fix. Let's merge it.