/// <summary>
/// Calculate Inverse Dynamics. The returned values are only for the joints. Be careful when using this for floating base models.
/// MultiBodyTree includes the 6 DOF for the base (Unlike for a Multbody), so the input and output arrays need to be prepadded with 6 extra elements.
/// </summary>
/// <param name="fixedBase"> Does the multibody have a floating base. Pad with six extra elements if floating base.</param>
/// <param name="q"> Positions of joints. Value of DOFs should be length of dofs excluding fixed base if is fixed base. Pad with six extra elements if floating base.</param>
/// <param name="u"> Velocities of joints. Value of DOFs velocity. Pad with six extra elements if floating base.</param>
/// <param name="dot_u"> Desired accelerations of joints. Pad with six extra elements if floating base. </param>
/// <param name="joint_forces"> Output jorces necessary to achieve desired accelerations. Pad with six extra elements if floating base.</param>
/// <returns></returns>
public int CalculateInverseDynamics(bool fixedBase, float[] q, float[] u, float[] dot_u, float[] joint_forces)
{
int baseDofs;
if (fixedBase)
{
baseDofs = 0;
}
else
{
baseDofs = 6;
}
int numDof = q.Length - baseDofs;
return(UnsafeNativeMethodsInverseDynamics.MultiBodyTree_calculateInverseDynamics(Native, numDof, baseDofs, q, u, dot_u, joint_forces));
}