Align Featherstone solver wrench handling with COM convention

docs/concepts/conventions.rst

@@ -89,9 +89,9 @@ aligning with Isaac Lab's approach, but with one important exception:
   convert from this convention to MuJoCo's mixed-frame format when using the SolverMuJoCo.
 
 * **Featherstone solver**  
-  Newton's Featherstone implementation uses the **spatial twist** convention 
-  from *Modern Robotics*. Here :attr:`State.body_qd` represents a spatial 
-  twist :math:`V_s = (v_s, \omega_s)` where :math:`v_s` is the linear 
+  Newton's :attr:`State.body_qd` stores **both** linear and angular velocities 
+  in the world frame, with linear velocity representing the COM velocity (same as other solvers).
+  Internally, Featherstone uses spatial twist convention :math:`V_s = (v_s, \omega_s)` where :math:`v_s` is the linear 
   velocity of a hypothetical point on the moving body that is instantaneously 
   at the world origin, **not** the COM velocity.
 
@@ -126,14 +126,10 @@ Summary of Conventions
      - COM, **world frame**
      - **World frame**
      - "Root" linear / angular velocity
-   * - **Newton** (standard solvers)
+   * - **Newton** (all solvers)
      - COM, **world frame**
      - **World frame**
      - Physics engine convention
-   * - **Newton** (Featherstone)
-     - World origin, **world frame**
-     - **World frame**
-     - Spatial twist :math:`V_s`
 
 Mapping Between Representations
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

newton/_src/sim/articulation.py

@@ -193,6 +193,7 @@ def eval_single_articulation_fk(
     joint_axis: wp.array(dtype=wp.vec3),
     joint_dof_dim: wp.array(dtype=int, ndim=2),
     body_com: wp.array(dtype=wp.vec3),
+    convert_velocity: bool,
     # outputs
     body_q: wp.array(dtype=wp.transform),
     body_qd: wp.array(dtype=wp.spatial_vector),
@@ -334,7 +335,18 @@ def eval_single_articulation_fk(
         v_wc = v_wpj + wp.spatial_vector(linear_vel, angular_vel)
 
         body_q[child] = X_wc
-        body_qd[child] = v_wc
+
+        # For FREE and DISTANCE joints, v_wc is a spatial twist in Featherstone
+        # but body_qd should store COM velocity. Transform it:
+        # v_com = v_origin + ω x r_com
+        if convert_velocity and (type == JointType.FREE or type == JointType.DISTANCE):
+            v_origin = wp.spatial_top(v_wc)
+            omega = wp.spatial_bottom(v_wc)
+            r_com = wp.transform_point(X_wc, body_com[child])
+            v_com = v_origin + wp.cross(omega, r_com)
+            body_qd[child] = wp.spatial_vector(v_com, omega)
+        else:
+            body_qd[child] = v_wc
 
 
 @wp.kernel
@@ -357,6 +369,7 @@ def eval_articulation_fk(
     joint_axis: wp.array(dtype=wp.vec3),
     joint_dof_dim: wp.array(dtype=int, ndim=2),
     body_com: wp.array(dtype=wp.vec3),
+    convert_velocity: bool,
     # outputs
     body_q: wp.array(dtype=wp.transform),
     body_qd: wp.array(dtype=wp.spatial_vector),
@@ -398,6 +411,7 @@ def eval_articulation_fk(
         joint_axis,
         joint_dof_dim,
         body_com,
+        convert_velocity,
         # outputs
         body_q,
         body_qd,
@@ -411,6 +425,7 @@ def eval_fk(
     state: State | object,
     mask: wp.array(dtype=bool) | None = None,
     indices: wp.array(dtype=int) | None = None,
+    convert_velocity: bool = False,
 ):
     """
     Evaluates the model's forward kinematics given the joint coordinates and updates the state's body information (:attr:`State.body_q` and :attr:`State.body_qd`).
@@ -423,6 +438,7 @@ def eval_fk(
         mask (array): The mask to use to enable / disable FK for an articulation. If None then treat all as enabled, shape [articulation_count], bool
         indices (array): Integer indices of articulations to update. If None, updates all articulations.
                         Cannot be used together with mask parameter.
+        convert_velocity (bool): If True, converts FREE/DISTANCE joint velocities to COM frame (needed by Featherstone). Default: False.
     """
     # Validate inputs
     if mask is not None and indices is not None:
@@ -454,6 +470,7 @@ def eval_fk(
             model.joint_axis,
             model.joint_dof_dim,
             model.body_com,
+            convert_velocity,
         ],
         outputs=[
             state.body_q,

newton/_src/sim/ik/ik_common.py

@@ -75,6 +75,7 @@ def _eval_fk_articulation_batched(
         joint_axis,
         joint_dof_dim,
         body_com,
+        False,
         body_q[problem_idx],
         body_qd[problem_idx],
     )

newton/_src/sim/state.py

@@ -51,8 +51,9 @@ def __init__(self) -> None:
         First three entries: linear force; last three: torque.
 
         Note:
-            :attr:`body_f` represents external wrenches in world frame, measured at the body's center of mass
-            for all solvers except :class:`~newton.solvers.SolverFeatherstone`, which expects wrenches at the world origin.
+            :attr:`body_f` represents external wrenches in world frame, measured at the body's center of mass (COM).
+            The linear force component is applied at the COM, and the torque is about the COM.
+            This convention is consistent across all solvers (XPBD, SemiImplicit, Featherstone, MuJoCo, VBD).
         """
 
         self.joint_q: wp.array | None = None

newton/_src/solvers/featherstone/kernels.py

@@ -748,6 +748,31 @@ def compute_link_velocity(
     body_I_s[child] = I_s
 
 
+# Convert body forces from COM-frame to world-origin-frame and negate for use in Featherstone dynamics.
+@wp.kernel
+def convert_body_force_com_to_origin(
+    body_q: wp.array(dtype=wp.transform),
+    body_X_com: wp.array(dtype=wp.transform),
+    # outputs
+    body_f_ext: wp.array(dtype=wp.spatial_vector),
+):
+    tid = wp.tid()
+
+    f_ext_com = body_f_ext[tid]
+
+    # skip if force is zero
+    if wp.length(f_ext_com) == 0.0:
+        return
+
+    body_q_com_val = body_q[tid] * body_X_com[tid]
+    r_com = wp.transform_get_translation(body_q_com_val)
+
+    force = wp.spatial_top(f_ext_com)
+    torque_com = wp.spatial_bottom(f_ext_com)
+
+    body_f_ext[tid] = -wp.spatial_vector(force, torque_com + wp.cross(r_com, force))
+
+
 # Inverse dynamics via Recursive Newton-Euler algorithm (Featherstone Table 5.1)
 @wp.kernel
 def eval_rigid_id(

newton/_src/solvers/featherstone/solver_featherstone.py

@@ -35,6 +35,7 @@
 from .kernels import (
     compute_com_transforms,
     compute_spatial_inertia,
+    convert_body_force_com_to_origin,
     create_inertia_matrix_cholesky_kernel,
     create_inertia_matrix_kernel,
     eval_dense_cholesky_batched,
@@ -313,6 +314,13 @@ def step(
 
             if state_in.body_count:
                 body_f = state_in.body_f
+                wp.launch(
+                    convert_body_force_com_to_origin,
+                    dim=model.body_count,
+                    inputs=[state_in.body_q, self.body_X_com],
+                    outputs=[body_f],
+                    device=model.device,
+                )
 
             # damped springs
             eval_spring_forces(model, state_in, particle_f)
@@ -664,7 +672,7 @@ def step(
                 )
 
                 # update maximal coordinates
-                eval_fk(model, state_out.joint_q, state_out.joint_qd, state_out)
+                eval_fk(model, state_out.joint_q, state_out.joint_qd, state_out, convert_velocity=True)
 
             self.integrate_particles(model, state_in, state_out, dt)
 

newton/tests/test_body_force.py

@@ -136,7 +136,7 @@ def test_3d_articulation(test: TestBodyForce, device, solver_fn, test_angular, u
 
 devices = get_test_devices()
 solvers = {
-    # "featherstone": lambda model: newton.solvers.SolverFeatherstone(model, angular_damping=0.0),
+    "featherstone": lambda model: newton.solvers.SolverFeatherstone(model, angular_damping=0.0),
     "mujoco_cpu": lambda model: newton.solvers.SolverMuJoCo(model, use_mujoco_cpu=True, disable_contacts=True),
     "mujoco_warp": lambda model: newton.solvers.SolverMuJoCo(model, use_mujoco_cpu=False, disable_contacts=True),
     "xpbd": lambda model: newton.solvers.SolverXPBD(model, angular_damping=0.0),

newton/tests/test_control_force.py

@@ -124,7 +124,7 @@ def test_3d_articulation(test: TestControlForce, device, solver_fn):
 
 devices = get_test_devices()
 solvers = {
-    # "featherstone": lambda model: newton.solvers.SolverFeatherstone(model, angular_damping=0.0),
+    "featherstone": lambda model: newton.solvers.SolverFeatherstone(model, angular_damping=0.0),
     "mujoco_cpu": lambda model: newton.solvers.SolverMuJoCo(
         model, use_mujoco_cpu=True, update_data_interval=0, disable_contacts=True
     ),