diff --git a/CHANGELOG.md b/CHANGELOG.md index e12a675bbc..6171ecfaa6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ - Fix MPR convergence failure on large and extreme-aspect-ratio mesh triangles by projecting the starting point onto the triangle nearest the convex center - Fix O(W²·S²) memory explosion in `CollisionPipeline` shape-pair buffer allocation for NXN and SAP broad phase modes by computing per-world pair counts instead of a global N² - Fix `SensorRaycast` ignoring `PLANE` geometry +- Fix multi-world `qfrc_actuator` conversion using the wrong body center of mass for worlds with `worldid > 0` - Fix SDF hydroelastic broadphase scatter kernel using a grid-stride loop with binary search instead of per-pair thread launch - Fix box support-map sign flips from quaternion rotation noise (~1e-14) producing invalid GJK/MPR contacts for face-touching boxes with non-trivial base rotations - Fix USD import of multi-DOF joints from MuJoCo-converted assets where multiple revolute joints between the same two bodies caused false cycle detection; merge them into D6 joints with correct DOF label mapping for MjcActuator target resolution diff --git a/newton/_src/solvers/mujoco/kernels.py b/newton/_src/solvers/mujoco/kernels.py index 98af3a8188..1104e4b15a 100644 --- a/newton/_src/solvers/mujoco/kernels.py +++ b/newton/_src/solvers/mujoco/kernels.py @@ -2458,17 +2458,19 @@ def convert_qfrc_actuator_from_mj_kernel( """ worldid, jntid = wp.tid() + joint_id = joints_per_world * worldid + jntid + # Skip loop joints — they have no MuJoCo DOF entries q_i = mj_q_start[jntid] if q_i < 0: return qd_i = mj_qd_start[jntid] - wqd_i = joint_qd_start[joints_per_world * worldid + jntid] + wqd_i = joint_qd_start[joint_id] - type = joint_type[jntid] + jtype = joint_type[joint_id] - if type == JointType.FREE: + if jtype == JointType.FREE: # MuJoCo qfrc_actuator for free joint: # [f_x, f_y, f_z] = linear force at body origin (world frame) # [τ_x, τ_y, τ_z] = torque in body frame @@ -2496,7 +2498,7 @@ def convert_qfrc_actuator_from_mj_kernel( ) # CoM offset in world frame - child = joint_child[jntid] + child = joint_child[joint_id] com_world = wp.quat_rotate(rot, body_com[child]) # Rotate torque body -> world and shift reference origin -> CoM @@ -2508,11 +2510,11 @@ def convert_qfrc_actuator_from_mj_kernel( qfrc_actuator[wqd_i + 3] = tau_world[0] qfrc_actuator[wqd_i + 4] = tau_world[1] qfrc_actuator[wqd_i + 5] = tau_world[2] - elif type == JointType.BALL: + elif jtype == JointType.BALL: for i in range(3): qfrc_actuator[wqd_i + i] = mjw_qfrc_actuator[worldid, qd_i + i] else: - axis_count = joint_dof_dim[jntid, 0] + joint_dof_dim[jntid, 1] + axis_count = joint_dof_dim[joint_id, 0] + joint_dof_dim[joint_id, 1] for i in range(axis_count): qfrc_actuator[wqd_i + i] = mjw_qfrc_actuator[worldid, qd_i + i] diff --git a/newton/tests/test_mujoco_solver.py b/newton/tests/test_mujoco_solver.py index 4a2c3433e6..4902364fd5 100644 --- a/newton/tests/test_mujoco_solver.py +++ b/newton/tests/test_mujoco_solver.py @@ -8275,6 +8275,66 @@ def test_biasprm2_consistent_across_worlds(self): ) +class TestMultiWorldQfrcActuatorCom(unittest.TestCase): + """Multi-world qfrc_actuator must use each world's own body CoM. + + Regression test: convert_qfrc_actuator_from_mj_kernel indexed + joint_type, joint_child, and joint_dof_dim with per-world jntid + instead of the global index, so world 1 read world 0's CoM for + the free-joint force transform. + """ + + MJCF = """ + + + """ + + @classmethod + def setUpClass(cls): + def make_template(com): + t = newton.ModelBuilder() + t.add_mjcf(cls.MJCF, ctrl_direct=True) + t.request_state_attributes("mujoco:qfrc_actuator") + t.body_com[0] = com + return t + + builder = newton.ModelBuilder() + builder.request_state_attributes("mujoco:qfrc_actuator") + builder.add_world(make_template(wp.vec3(0.0, 0.0, 0.0))) + builder.add_world(make_template(wp.vec3(0.1, -0.05, 0.03))) + cls.model = builder.finalize() + cls.solver = SolverMuJoCo(cls.model) + + def test_world1_uses_own_com(self): + """World 1 angular qfrc must reflect its non-zero CoM, not world 0's.""" + state = self.model.state() + ctrl = self.model.control() + ctrl.mujoco.ctrl = wp.array([10.0, 10.0], dtype=wp.float32) + self.solver.step(state, state, ctrl, None, dt=0.01) + + qfrc = state.mujoco.qfrc_actuator.numpy() + dofs_per_world = self.model.joint_dof_count // self.model.world_count + + # World 0 has CoM at origin — cross product is zero + np.testing.assert_allclose(qfrc[3:6], 0.0, atol=0.01) + # World 1: tau = -cross(R * com, f_lin) where R is 90deg around X, + # com = (0.1, -0.05, 0.03), f_lin = (0, 0, 10) + # => R*com = (0.1, -0.03, -0.05), tau = (0.3, 1.0, 0.0) + # The bug would produce zeros here (reading world 0's zero CoM). + angular_1 = qfrc[dofs_per_world + 3 : dofs_per_world + 6] + np.testing.assert_allclose(angular_1, [0.3, 1.0, 0.0], atol=0.05) + + class TestActuatorLengthRangeRuntime(unittest.TestCase): """Verify actuator lengthrange updates after runtime gear changes."""