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.
120 lines
5.0 KiB
Python
120 lines
5.0 KiB
Python
"""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)
|