#!/usr/bin/env python3 """Independent real PhysX impulse oracle: native point load vs COM-reduced load. Three identical isotropic bodies: correct reduction, native off-center force, and deliberately double-counted lever arm (negative control). No training. """ import argparse import hashlib import math import traceback from pathlib import Path from runtime_common import finish_run, write_json def run(args): import torch from isaaclab_physx.physics import PhysxCfg from SkillFactory.legs.contact_cycle.mechanics import reduce_to_com, rotate import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sim import SimulationCfg, build_simulation_context dt, mass, radius = 0.002, 2.0, 0.1 cfg = SimulationCfg(dt=dt, device=args.device, gravity=(0, 0, 0), physics=PhysxCfg(solver_type=0, enable_external_forces_every_iteration=False)) with build_simulation_context(sim_cfg=cfg) as sim: sim._app_control_on_stop_handle = None for i in range(3): sim_utils.create_prim(f"/World/Fixture_{i}", "Xform", translation=(2*i, 0, 0)) body = RigidObject(RigidObjectCfg( prim_path="/World/Fixture_.*/Sphere", spawn=sim_utils.SphereCfg(radius=radius, rigid_props=sim_utils.RigidBodyPropertiesCfg(linear_damping=0, angular_damping=0), mass_props=sim_utils.MassPropertiesCfg(mass=mass)), )) sim.reset() pos = torch.tensor([[2*i, 0., 0.] for i in range(3)], device=args.device) quat = torch.tensor([[0.2, -0.3, 0.4, math.sqrt(0.71)]], device=args.device).expand(3, -1) body.write_root_pose_to_sim(torch.cat((pos, quat), -1)) body.write_root_velocity_to_sim(torch.zeros(3, 6, device=args.device)) body.update(0) local_f = torch.tensor([[1., -2, 3]], device=args.device).expand(3, -1) local_tau = torch.tensor([[0.02, -0.03, 0.04]], device=args.device).expand(3, -1) lever = rotate(quat, torch.tensor([[0.04, 0.03, -0.02]], device=args.device).expand(3, -1)) point = pos + lever reduced = reduce_to_com(local_f, local_tau, quat, point, pos) force = reduced[:, :3] torques = reduced[:, 3:].clone() positions = pos.clone() # native point oracle, and intentional double r x F negative control torques[1] = rotate(quat, local_tau)[1] positions[1:] = point[1:] body.permanent_wrench_composer.set_forces_and_torques_index( forces=force[:, None], torques=torques[:, None], positions=positions[:, None], is_global=True) body.write_data_to_sim() sim.step() body.update(dt) velocity = body.data.root_com_vel_w.torch.clone() inertia = 0.4 * mass * radius**2 expected = torch.cat((force * dt/mass, reduced[:, 3:] * dt/inertia), -1) native_difference = (velocity[0]-velocity[1]).abs().max().item() oracle_error = (velocity[:2]-expected[:2]).abs().max().item() negative_separation = (velocity[2]-expected[2]).abs().max().item() body.permanent_wrench_composer.reset() body.write_data_to_sim() sim.step() body.update(dt) cleared_delta = (body.data.root_com_vel_w.torch-velocity).abs().max().item() gates = {"native_point_equivalence": native_difference <= 2e-5, "newton_euler_impulse": oracle_error <= 2e-5, "double_lever_negative_control_detected": negative_separation >= 0.001, "clear_stops_acceleration": cleared_delta <= 2e-5} return {"passed": all(gates.values()), "accepted_skill": False, "gates": gates, "native_point_max_velocity_difference": native_difference, "analytical_max_velocity_error": oracle_error, "double_lever_error": negative_separation, "clear_max_velocity_change": cleared_delta, "measured_twist_world": velocity.tolist(), "expected_twist_world": expected.tolist(), "mass_kg": mass, "inertia_kg_m2": inertia, "dt_s": dt, "note": "Rigid-body force/torque API oracle only; not a Go2 tracking or sim-to-real certificate."} def main(): from isaaclab.app import AppLauncher parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--output", type=Path, required=True) AppLauncher.add_app_launcher_args(parser) args = parser.parse_args() args.output.mkdir(parents=True, exist_ok=False) root = Path(__file__).resolve().parents[1] files = [Path(__file__), root/"source/SkillFactory/SkillFactory/legs/contact_cycle/mechanics.py", root/"scripts/runtime_common.py", root/"scripts/supervise_sim.py"] write_json(args.output/"identity.json", {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in files}) write_json(args.output/"result.json", {"passed": False, "status": "running"}) app = AppLauncher(args).app result, code = {"passed": False}, 1 try: result = run(args) code = 0 if result["passed"] else 2 except BaseException: result.update(error=traceback.format_exc()) print(result["error"]) finally: finish_run(app, None, args.output/"result.json", result, code) if __name__ == "__main__": main()