"""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)