9fe9d27865
Consolidate next-stage docs, archive superseded narratives, and preserve frozen document inputs. Retain invalid P2 pairs, partial RootXY training, failed attempts and nonselectable checkpoints as explicit limitations. Include supervised evaluation/training, desktop GPU admission, bounded evidence and atomic-publication monitor repairs with CPU regressions. Ignore local dotenv configuration. Validation: 1403 passed, 6 skipped for contact-cycle and related contracts/CLI tests. Full-suite legacy ABI and registration tests require Python 3.12/Torch 2.10/pxr; no runtime checks relaxed. No new physical run or accepted skill.
112 lines
5.3 KiB
Python
112 lines
5.3 KiB
Python
#!/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()
|