orisys-tech/orisys-sdk
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1. tuned arrow mesh drawing

Browse Source 
2. centralixzed arrow config in layout.py
3. seperate mesh render and display resolution to speed up (~40 fps)
This commit is contained in:
ykcheungab
2026-06-19 14:28:28 +08:00
parent 3fd9b839a9
commit 2e5c047d1e
19 changed files with 980 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/viz/contact_arrow.py
+122 -39
View File
@@ -2,17 +2,90 @@
from __future__ import annotations
from dataclasses import dataclass, replace
from typing import Literal
import numpy as np
from orisys.finger.mesh import UVSurfaceMesh, compute_uv_surface_frames, mesh_uv_to_world_scale
from orisys.finger.mesh import UVSurfaceMesh, compute_uv_surface_frames
DEFAULT_NORMAL_FORCE_SCALE = 1.0
MIN_ARROW_LENGTH_FRACTION = 0.02
MAX_ARROW_LENGTH_FRACTION = 0.30
REFERENCE_NORMAL_FORCE = 10000.0
MIN_ARROW_THICKNESS_SCALE = 0.20
MAX_ARROW_THICKNESS_SCALE = 1.0
CONTACT_ARROW_COLOR = (1.0, 0.92, 0.10)
ForceStrengthMode = Literal["log", "power", "linear"]
@dataclass(frozen=True)
class ContactArrowStyle:
"""Visual tuning for the 3D contact normal arrow (dashboard: edit MESH_CONTACT_ARROW in layout.py)."""
normal_force_scale: float = 1.0
min_length_fraction: float = 0.02
max_length_fraction: float = 0.30
min_thickness_scale: float = 0.20
max_thickness_scale: float = 1.0
reference_normal_force: float = 10000.0
force_strength_mode: ForceStrengthMode = "log"
force_strength_gamma: float = 0.5
color: tuple[float, float, float] = (1.0, 0.92, 0.10)
DEFAULT_CONTACT_ARROW_STYLE = ContactArrowStyle()
# Backward-compatible aliases (match DEFAULT_CONTACT_ARROW_STYLE).
DEFAULT_NORMAL_FORCE_SCALE = DEFAULT_CONTACT_ARROW_STYLE.normal_force_scale
MIN_ARROW_LENGTH_FRACTION = DEFAULT_CONTACT_ARROW_STYLE.min_length_fraction
MAX_ARROW_LENGTH_FRACTION = DEFAULT_CONTACT_ARROW_STYLE.max_length_fraction
MIN_ARROW_THICKNESS_SCALE = DEFAULT_CONTACT_ARROW_STYLE.min_thickness_scale
MAX_ARROW_THICKNESS_SCALE = DEFAULT_CONTACT_ARROW_STYLE.max_thickness_scale
REFERENCE_NORMAL_FORCE = DEFAULT_CONTACT_ARROW_STYLE.reference_normal_force
FORCE_STRENGTH_MODE = DEFAULT_CONTACT_ARROW_STYLE.force_strength_mode
FORCE_STRENGTH_GAMMA = DEFAULT_CONTACT_ARROW_STYLE.force_strength_gamma
CONTACT_ARROW_COLOR = DEFAULT_CONTACT_ARROW_STYLE.color
def force_visual_strength(
force: float,
*,
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
) -> float:
"""Map normal force Fn to a unitless strength in [0, 1] for length and thickness."""
reference = style.reference_normal_force
if force <= 0.0 or reference <= 0.0:
return 0.0
ratio = force / reference
mode = style.force_strength_mode
if mode == "linear":
strength = ratio
elif mode == "power":
strength = ratio ** style.force_strength_gamma
else: # log
strength = np.log1p(ratio) / np.log1p(1.0)
return float(np.clip(strength, 0.0, 1.0))
def thickness_scale_from_force(
force: float,
*,
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
) -> float:
"""Radial scale from Fn using the shared force_strength curve."""
strength = force_visual_strength(force, style=style)
span = style.max_thickness_scale - style.min_thickness_scale
return float(style.min_thickness_scale + span * strength)
def length_from_force(
force: float,
mesh_span: float,
*,
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
) -> float:
"""Arrow length in world units from Fn and mesh extent."""
strength = force_visual_strength(force, style=style)
frac_span = style.max_length_fraction - style.min_length_fraction
fraction = style.min_length_fraction + frac_span * strength
return float(mesh_span * fraction * style.normal_force_scale)
def _mesh_span(vertices: np.ndarray) -> float:
@@ -47,20 +120,6 @@ def _rotation_matrix_z_to(direction: np.ndarray) -> np.ndarray:
return np.eye(3, dtype=np.float64) + skew + skew @ skew * (1.0 / (1.0 + dot))
def thickness_scale_from_force(force: float) -> float:
"""Map normal force to a radial scale factor (thin at low force, thicker at high force)."""
if force <= 0.0:
return 0.0
t = force / REFERENCE_NORMAL_FORCE
return float(
np.clip(
MIN_ARROW_THICKNESS_SCALE + (MAX_ARROW_THICKNESS_SCALE - MIN_ARROW_THICKNESS_SCALE) * t,
MIN_ARROW_THICKNESS_SCALE,
MAX_ARROW_THICKNESS_SCALE,
)
)
def _scale_arrow_template(
template_vertices: np.ndarray,
*,
@@ -78,8 +137,8 @@ def _scale_arrow_template(
def create_arrow_template(o3d):
"""Unit-height arrow mesh along +Z; radial size is scaled per-frame from force."""
arrow = o3d.geometry.TriangleMesh.create_arrow(
cylinder_radius=0.03,
cone_radius=0.06,
cylinder_radius=0.15,
cone_radius=0.3,
cylinder_height=0.72,
cone_height=0.28,
)
@@ -99,19 +158,26 @@ def build_contact_normal_arrow_mesh(
mesh_span: float,
template_vertices: np.ndarray,
template_triangles: np.ndarray,
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Build a yellow 3D arrow mesh pointing opposite the surface normal."""
"""Build a 3D arrow mesh pointing opposite the surface normal."""
origin = np.asarray(origin, dtype=np.float64).reshape(3)
normal = np.asarray(normal, dtype=np.float64).reshape(3)
norm = np.linalg.norm(normal)
color = np.tile(np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), (len(template_vertices), 1))
color = np.tile(np.asarray(style.color, dtype=np.float64), (len(template_vertices), 1))
radial_scale = thickness_scale_from_force(force)
radial_scale = thickness_scale_from_force(force, style=style)
if norm < 1e-8 or length <= 0.0 or radial_scale <= 0.0 or not np.all(np.isfinite(origin)):
return np.zeros_like(template_vertices), template_triangles, color
direction = -normal / norm
length = float(np.clip(length, mesh_span * MIN_ARROW_LENGTH_FRACTION, mesh_span * MAX_ARROW_LENGTH_FRACTION))
length = float(
np.clip(
length,
mesh_span * style.min_length_fraction,
mesh_span * style.max_length_fraction,
)
)
if length <= 0.0:
return np.zeros_like(template_vertices), template_triangles, color
@@ -125,13 +191,19 @@ def build_contact_normal_arrow_mesh(
return vertices, template_triangles, color
def create_arrow_mesh_geometry(o3d, template_vertices: np.ndarray, template_triangles: np.ndarray):
def create_arrow_mesh_geometry(
o3d,
template_vertices: np.ndarray,
template_triangles: np.ndarray,
*,
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
):
"""Create a persistent Open3D mesh for the contact arrow overlay."""
arrow_mesh = o3d.geometry.TriangleMesh()
arrow_mesh.vertices = o3d.utility.Vector3dVector(np.zeros_like(template_vertices))
arrow_mesh.triangles = o3d.utility.Vector3iVector(template_triangles)
arrow_mesh.vertex_colors = o3d.utility.Vector3dVector(
np.tile(np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), (len(template_vertices), 1))
np.tile(np.asarray(style.color, dtype=np.float64), (len(template_vertices), 1))
)
arrow_mesh.compute_vertex_normals()
return arrow_mesh
@@ -144,26 +216,38 @@ class ContactArrowOverlay:
self,
mesh: UVSurfaceMesh,
*,
normal_force_scale: float = DEFAULT_NORMAL_FORCE_SCALE,
style: ContactArrowStyle | None = None,
normal_force_scale: float | None = None,
enabled: bool = True,
):
import open3d as o3d
self.mesh = mesh
self.normal_force_scale = float(normal_force_scale)
base_style = style or DEFAULT_CONTACT_ARROW_STYLE
if normal_force_scale is not None:
base_style = replace(base_style, normal_force_scale=float(normal_force_scale))
self.style = base_style
self.enabled = enabled
self._mesh_span = _mesh_span(mesh.rest_vertices)
self._pixel_to_world = mesh_uv_to_world_scale(mesh.world_xyz, mesh.valid_mask)
_, _, self._surface_normals = compute_uv_surface_frames(mesh.world_xyz, mesh.valid_mask)
self._template_vertices, self._template_triangles = (
create_arrow_template(o3d) if enabled else (None, None)
)
self.arrow_mesh = (
create_arrow_mesh_geometry(o3d, self._template_vertices, self._template_triangles)
create_arrow_mesh_geometry(
o3d,
self._template_vertices,
self._template_triangles,
style=self.style,
)
if enabled
else None
)
@property
def normal_force_scale(self) -> float:
return self.style.normal_force_scale
def compute(
self,
centroid,
@@ -173,12 +257,10 @@ class ContactArrowOverlay:
if not self.enabled or self.arrow_mesh is None:
return None
style = self.style
template_vertices = self._template_vertices
template_triangles = self._template_triangles
color = np.tile(
np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64),
(len(template_vertices), 1),
)
color = np.tile(np.asarray(style.color, dtype=np.float64), (len(template_vertices), 1))
hidden = (np.zeros_like(template_vertices), template_triangles, color)
if not centroid_found or centroid is None:
@@ -204,7 +286,7 @@ class ContactArrowOverlay:
if force <= 0.0:
return hidden
length = force * self.normal_force_scale * self._pixel_to_world
length = length_from_force(force, self._mesh_span, style=style)
return build_contact_normal_arrow_mesh(
origin,
normal,
@@ -213,6 +295,7 @@ class ContactArrowOverlay:
mesh_span=self._mesh_span,
template_vertices=template_vertices,
template_triangles=template_triangles,
style=style,
)
def apply(self, arrow_vertices: np.ndarray, arrow_colors: np.ndarray) -> None: