Test cases for G, and g : Scapula-on-Thorax Ellipsoidal Constraint

Following our conversation, I'm creating this issue to formalize a test case where we have multiple dof constrained together, here three translations as function of two rotations.

The goal is to implement a scapula-on-thorax model, where the scapula's motion is constrained to the surface of an ellipsoid representing the thorax. This would serve as a non-trivial test case for constrained forward dynamics, particularly for validating the computation of G and g from Featherstone's book.

Here a code to pilot a scapula or any solid on an ellipsoid:

def get_pose_on_ellipsoid(
    q1: float,
    q2: float,
    ellipsoid_radii: np.ndarray,
    ellipsoid_center: np.ndarray
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Calculates the 3D pose of a body constrained to an ellipsoid surface.

    This function maps two angular parameters (q1, q2) to a translation
    and rotation, defining the pose of the body (e.g., scapula).

    Args:
        q1: The first angular parameter (related to protraction/retraction).
        q2: The second angular parameter (related to elevation/depression).
        ellipsoid_radii: Radii of the ellipsoid [a, b, c].
        ellipsoid_center: 3D center of the ellipsoid [x, y, z].

    Returns:
        A tuple containing:
        - translation (np.ndarray): The 3D position vector.
        - rotation (np.ndarray): The 3x3 rotation matrix.
    """
    a, b, c = ellipsoid_radii
    
    # 1. Calculate position on the ellipsoid surface
    position_on_surface = np.array([
        a * np.sin(q2),
        -b * np.sin(q1) * np.cos(q2),
        c * np.cos(q1) * np.cos(q2),
    ])
    translation = ellipsoid_center + position_on_surface

    # 2. Calculate orientation based on the parameters
    cos_q1, sin_q1 = np.cos(q1), np.sin(q1)
    cos_q2, sin_q2 = np.cos(q2), np.sin(q2)

    rot_x = np.array([
        [1.0, 0.0, 0.0],
        [0.0, cos_q1, -sin_q1],
        [0.0, sin_q1, cos_q1],
    ])
    
    rot_y = np.array([
        [cos_q2, 0.0, sin_q2],
        [0.0, 1.0, 0.0],
        [-sin_q2, 0.0, cos_q2],
    ])
    
    rotation = rot_x @ rot_y
    
    return translation, rotation

Based on that we need a custom way to compute G, Gdot. We can add dummy inertial properties to the floating solid, representing the scapula.

Inspiration from : https://journals.plos.org/plosone/article?id=10.1371/journal.pone.0141028