feat: checkpoint contact-cycle evaluation and RootXY PPO progress
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.
This commit is contained in:
@@ -0,0 +1,119 @@
|
||||
"""Visual-only world-frame disturbance arrows; no physics or network assets."""
|
||||
|
||||
import math
|
||||
|
||||
import torch
|
||||
|
||||
|
||||
def arrow_geometry(origins, forces, metres_per_newton=0.03):
|
||||
"""Return cylinder/cone instances along +Z, rotated using xyzw quaternions.
|
||||
|
||||
The tail is the application point and the tip is origin + scale * force.
|
||||
Zero forces are omitted (including pure torque disturbances). Inputs are
|
||||
read-only. Prototype cylinders/cones have unit radius and unit height.
|
||||
"""
|
||||
if not math.isfinite(metres_per_newton) or metres_per_newton <= 0:
|
||||
raise ValueError("Arrow scale must be finite and positive (m/N)")
|
||||
if (
|
||||
origins.ndim != 2
|
||||
or origins.shape[1] != 3
|
||||
or forces.shape != origins.shape
|
||||
or origins.device != forces.device
|
||||
or origins.dtype != forces.dtype
|
||||
or not origins.is_floating_point()
|
||||
or not torch.isfinite(origins).all()
|
||||
or not torch.isfinite(forces).all()
|
||||
):
|
||||
raise ValueError("Expected finite matching floating world positions/forces [B,3]")
|
||||
magnitude = torch.linalg.vector_norm(forces, dim=-1)
|
||||
if not torch.isfinite(magnitude).all():
|
||||
raise ValueError("Force magnitude overflow")
|
||||
active = magnitude > 1e-6
|
||||
origin, force, magnitude = origins[active], forces[active], magnitude[active]
|
||||
direction = force / magnitude[:, None]
|
||||
length = magnitude * metres_per_newton
|
||||
if not torch.isfinite(length).all():
|
||||
raise ValueError("Arrow length overflow")
|
||||
# atan2 avoids the cancellation of the [cross(z,d), 1+dot(z,d)]
|
||||
# construction near -Z; exact -Z uses a 180-degree turn around X.
|
||||
horizontal = torch.linalg.vector_norm(direction[:, :2], dim=-1)
|
||||
axis = torch.stack((-direction[:, 1], direction[:, 0], torch.zeros_like(horizontal)), dim=-1)
|
||||
axis /= horizontal.clamp_min(torch.finfo(forces.dtype).tiny)[:, None]
|
||||
axis[horizontal == 0] = axis.new_tensor([1.0, 0.0, 0.0])
|
||||
half_angle = torch.atan2(horizontal, direction[:, 2]) * 0.5
|
||||
orientation = torch.cat((axis * half_angle.sin()[:, None], half_angle.cos()[:, None]), dim=-1)
|
||||
head_length = torch.minimum(0.3 * length, length.new_tensor(0.05))
|
||||
shaft_length = length - head_length
|
||||
shaft_radius = torch.minimum(0.04 * length, length.new_tensor(0.004))
|
||||
head_radius = torch.minimum(0.12 * length, length.new_tensor(0.012))
|
||||
shaft_center = origin + direction * (shaft_length / 2)[:, None]
|
||||
head_center = origin + direction * (length - head_length / 2)[:, None]
|
||||
count = len(origin)
|
||||
return {
|
||||
"translations": torch.cat((shaft_center, head_center)),
|
||||
"orientations": torch.cat((orientation, orientation)),
|
||||
"scales": torch.cat(
|
||||
(
|
||||
torch.stack((shaft_radius, shaft_radius, shaft_length), dim=-1),
|
||||
torch.stack((head_radius, head_radius, head_length), dim=-1),
|
||||
)
|
||||
),
|
||||
"marker_indices": torch.cat(
|
||||
(
|
||||
torch.zeros(count, dtype=torch.int32, device=origins.device),
|
||||
torch.ones(count, dtype=torch.int32, device=origins.device),
|
||||
)
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def _make_markers():
|
||||
# Lazy Kit imports: pure geometry tests and non-visual runs need no renderer.
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg
|
||||
|
||||
return VisualizationMarkers(
|
||||
VisualizationMarkersCfg(
|
||||
prim_path="/Visuals/AppliedDisturbanceForce",
|
||||
markers={
|
||||
"shaft": sim_utils.CylinderCfg(
|
||||
radius=1.0,
|
||||
height=1.0,
|
||||
axis="Z",
|
||||
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.15, 0.02)),
|
||||
collision_props=None,
|
||||
rigid_props=None,
|
||||
),
|
||||
"head": sim_utils.ConeCfg(
|
||||
radius=1.0,
|
||||
height=1.0,
|
||||
axis="Z",
|
||||
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.15, 0.02)),
|
||||
collision_props=None,
|
||||
rigid_props=None,
|
||||
),
|
||||
},
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
class DisturbanceForceArrows:
|
||||
def __init__(self, metres_per_newton=0.03):
|
||||
if not math.isfinite(metres_per_newton) or metres_per_newton <= 0:
|
||||
raise ValueError("Arrow scale must be finite and positive (m/N)")
|
||||
self.metres_per_newton = metres_per_newton
|
||||
self.markers = _make_markers()
|
||||
self.markers.set_visibility(False)
|
||||
|
||||
@torch.no_grad()
|
||||
def update(self, state):
|
||||
# Actual last-applied disturbance ONLY, not guidance, total load, sensor
|
||||
# reaction, or the next requested/random wrench. root()[0] is world XYZ.
|
||||
geometry = arrow_geometry(state.root()[0], state.last_disturbance[:, :3], self.metres_per_newton)
|
||||
visible = len(geometry["translations"]) > 0
|
||||
self.markers.set_visibility(visible)
|
||||
if visible:
|
||||
self.markers.visualize(**geometry)
|
||||
|
||||
def close(self):
|
||||
self.markers.set_visibility(False)
|
||||
Reference in New Issue
Block a user