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:
examples/viz/finger_mesh_viewer.py
+4 -2
View File
@@ -4,7 +4,7 @@ from __future__ import annotations
from orisys.finger.viewer import UVMeshViewer
from .contact_arrow import ContactArrowOverlay, DEFAULT_NORMAL_FORCE_SCALE
from .contact_arrow import ContactArrowOverlay, ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE
class FingerMeshViewer(UVMeshViewer):
@@ -21,7 +21,8 @@ class FingerMeshViewer(UVMeshViewer):
value_max=None,
window_width: int = 1024,
window_height: int = 1024,
normal_force_scale: float = DEFAULT_NORMAL_FORCE_SCALE,
contact_arrow_style: ContactArrowStyle | None = None,
normal_force_scale: float | None = None,
show_contact_arrow: bool = True,
):
super().__init__(
@@ -36,6 +37,7 @@ class FingerMeshViewer(UVMeshViewer):
)
self._contact_arrow = ContactArrowOverlay(
mesh,
style=contact_arrow_style or DEFAULT_CONTACT_ARROW_STYLE,
normal_force_scale=normal_force_scale,
enabled=show_contact_arrow,
)
examples/viz/mesh_camera.py
+72 -1
View File
@@ -97,7 +97,7 @@ def apply_view_from_world_to_cam(vis, world_to_cam: np.ndarray, *, lookat: np.nd
def apply_mesh_lighting(vis) -> None:
"""Enable shaded lighting so the tactile mesh reads as a 3D surface."""
"""Enable shaded lighting on the legacy Open3D Visualizer."""
import open3d as o3d
render_option = vis.get_render_option()
@@ -108,3 +108,74 @@ def apply_mesh_lighting(vis) -> None:
if hasattr(render_option, "mesh_shade_option"):
render_option.mesh_shade_option = o3d.visualization.MeshShadeOption.Color
vis.update_renderer()
def normalize_light_direction(direction) -> np.ndarray:
d = np.asarray(direction, dtype=np.float64).reshape(3)
norm = float(np.linalg.norm(d))
if norm <= 0:
return np.array([0.0, 0.0, -1.0], dtype=np.float64)
return d / norm
def apply_filament_sun_light(renderer, *, direction, intensity: float) -> None:
"""Configure Filament sun light on an OffscreenRenderer."""
scene = renderer.scene
if not hasattr(scene, "scene"):
return
filament = scene.scene
sun_dir = normalize_light_direction(direction).tolist()
if hasattr(filament, "set_sun_light"):
filament.set_sun_light(sun_dir, [1.0, 1.0, 1.0], float(intensity))
if hasattr(filament, "enable_sun_light"):
filament.enable_sun_light(True)
def compute_vertex_normals(vertices: np.ndarray, triangles: np.ndarray) -> np.ndarray:
"""Per-vertex normals for a triangle mesh (vectorized)."""
vertices = np.asarray(vertices, dtype=np.float64)
triangles = np.asarray(triangles, dtype=np.int32)
v0 = vertices[triangles[:, 0]]
v1 = vertices[triangles[:, 1]]
v2 = vertices[triangles[:, 2]]
face_normals = np.cross(v1 - v0, v2 - v0)
normals = np.zeros_like(vertices)
np.add.at(normals, triangles[:, 0], face_normals)
np.add.at(normals, triangles[:, 1], face_normals)
np.add.at(normals, triangles[:, 2], face_normals)
lengths = np.linalg.norm(normals, axis=1)
valid = lengths > 1e-12
normals[valid] /= lengths[valid, np.newaxis]
normals[~valid] = np.array([0.0, 0.0, 1.0], dtype=np.float64)
return normals
def precompute_shade_factors(vertices: np.ndarray, triangles: np.ndarray, lighting) -> np.ndarray:
"""Per-vertex shade multipliers from static mesh geometry and layout lighting."""
normals = compute_vertex_normals(vertices, triangles)
d = normalize_light_direction(lighting.direction)
ndotl = np.clip(normals @ d, 0.0, 1.0)
return float(lighting.ambient) + float(lighting.diffuse) * ndotl
def apply_shade_factors(colors: np.ndarray, shade_factors: np.ndarray | None) -> np.ndarray:
if shade_factors is None:
return colors
return np.clip(np.asarray(colors, dtype=np.float64) * shade_factors[:, np.newaxis], 0.0, 1.0)
def shade_vertex_colors(
colors: np.ndarray,
normals: np.ndarray,
*,
direction,
ambient: float,
diffuse: float,
) -> np.ndarray:
"""Apply directional shading to vertex colors (legacy viewer path)."""
colors = np.asarray(colors, dtype=np.float64)
normals = np.asarray(normals, dtype=np.float64)
d = normalize_light_direction(direction)
ndotl = np.clip(normals @ d, 0.0, 1.0)
shade = float(ambient) + float(diffuse) * ndotl
return np.clip(colors * shade[:, np.newaxis], 0.0, 1.0)
examples/viz/offscreen_mesh.py
+60 -11
View File
@@ -10,7 +10,13 @@ from orisys.finger.mesh import UVSurfaceMesh
from orisys.finger.viewer import flow_magnitude_vertex_colors, scalar_map_vertex_colors
from .contact_arrow import ContactArrowOverlay
from .mesh_camera import apply_view_from_world_to_cam, camera_look_at_from_world_to_cam
from .mesh_camera import (
apply_filament_sun_light,
apply_shade_factors,
apply_view_from_world_to_cam,
camera_look_at_from_world_to_cam,
precompute_shade_factors,
)
def _numpy_o3d_image_to_rgb(image) -> np.ndarray:
@@ -22,13 +28,17 @@ def _numpy_o3d_image_to_rgb(image) -> np.ndarray:
return arr.astype(np.uint8)
def _make_unlit_material(o3d):
def _make_lit_material(o3d):
material = o3d.visualization.rendering.MaterialRecord()
material.shader = "defaultUnlit"
material.shader = "defaultLit"
material.base_color = (1.0, 1.0, 1.0, 1.0)
return material
def _shade_vertex_colors(mesh, colors: np.ndarray, shade_factors) -> np.ndarray:
return apply_shade_factors(colors, shade_factors)
def _mesh_to_o3d(o3d, vertices: np.ndarray, triangles: np.ndarray, colors: np.ndarray):
mesh = o3d.geometry.TriangleMesh()
mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64))
@@ -70,6 +80,8 @@ class _FilamentOffscreenBackend:
world_to_cam: np.ndarray | None,
lookat: np.ndarray,
show_contact_arrow: bool,
mesh_lighting=None,
contact_arrow_style=None,
):
import open3d as o3d
@@ -83,10 +95,26 @@ class _FilamentOffscreenBackend:
self.value_max = value_max
self.world_to_cam = world_to_cam
self.lookat = lookat
self._contact_arrow = ContactArrowOverlay(mesh, enabled=show_contact_arrow)
self.mesh_lighting = mesh_lighting
self._shade_factors = (
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
if mesh_lighting is not None
else None
)
self._contact_arrow = ContactArrowOverlay(
mesh,
enabled=show_contact_arrow,
style=contact_arrow_style,
)
self._renderer = o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
self._material = _make_unlit_material(o3d)
if mesh_lighting is not None:
apply_filament_sun_light(
self._renderer,
direction=mesh_lighting.direction,
intensity=mesh_lighting.intensity,
)
self._material = _make_lit_material(o3d)
self._mesh_added = False
self._arrow_added = False
self._apply_camera()
@@ -111,6 +139,13 @@ class _FilamentOffscreenBackend:
self.height = height
self._renderer = self._o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
if self.mesh_lighting is not None:
apply_filament_sun_light(
self._renderer,
direction=self.mesh_lighting.direction,
intensity=self.mesh_lighting.intensity,
)
self._material = _make_lit_material(self._o3d)
self._mesh_added = False
self._arrow_added = False
self._apply_camera()
@@ -172,6 +207,8 @@ class _HiddenVisualizerBackend:
world_to_cam: np.ndarray | None,
lookat: np.ndarray,
show_contact_arrow: bool,
mesh_lighting=None,
contact_arrow_style=None,
):
import open3d as o3d
@@ -185,7 +222,17 @@ class _HiddenVisualizerBackend:
self.value_max = value_max
self.world_to_cam = world_to_cam
self.lookat = lookat
self._contact_arrow = ContactArrowOverlay(mesh, enabled=show_contact_arrow)
self.mesh_lighting = mesh_lighting
self._shade_factors = (
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
if mesh_lighting is not None
else None
)
self._contact_arrow = ContactArrowOverlay(
mesh,
enabled=show_contact_arrow,
style=contact_arrow_style,
)
self.vis = None
self.tri_mesh = None
self._create_window()
@@ -202,6 +249,7 @@ class _HiddenVisualizerBackend:
value_min=self.value_min,
value_max=self.value_max,
)
colors = _shade_vertex_colors(self.mesh, colors, self._shade_factors)
self.vis = o3d.visualization.Visualizer()
self.vis.create_window(
window_name="Orisys Finger Mesh (embedded)",
@@ -222,9 +270,9 @@ class _HiddenVisualizerBackend:
render_option = self.vis.get_render_option()
render_option.mesh_show_back_face = True
render_option.background_color = np.array([0.05, 0.05, 0.08])
if hasattr(render_option, "light_on"):
render_option.light_on = False
render_option.background_color = np.array([0.05, 0.05, 0.08])
if self.world_to_cam is not None:
apply_view_from_world_to_cam(self.vis, self.world_to_cam, lookat=self.lookat)
@@ -261,11 +309,9 @@ class _HiddenVisualizerBackend:
flow=flow,
scalar_map=scalar_map,
)
vertices = self.mesh.rest_vertices
colors = _shade_vertex_colors(self.mesh, colors, self._shade_factors)
self.tri_mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64))
self.tri_mesh.vertex_colors = o3d.utility.Vector3dVector(colors.astype(np.float64))
self.tri_mesh.compute_vertex_normals()
self.vis.update_geometry(self.tri_mesh)
arrow_result = self._contact_arrow.compute(centroid, centroid_found, fnormal)
@@ -293,7 +339,6 @@ def _create_backend(mesh: UVSurfaceMesh, **kwargs):
except RuntimeError:
return _HiddenVisualizerBackend(mesh, **kwargs)
class OffscreenFingerMeshRenderer:
"""Render finger UV mesh + contact arrow to an RGB image for Qt embedding."""
@@ -310,6 +355,8 @@ class OffscreenFingerMeshRenderer:
world_to_cam: np.ndarray | None = None,
lookat: np.ndarray | None = None,
show_contact_arrow: bool = True,
mesh_lighting=None,
contact_arrow_style=None,
):
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
self._backend = _create_backend(
@@ -323,6 +370,8 @@ class OffscreenFingerMeshRenderer:
world_to_cam=world_to_cam,
lookat=lookat,
show_contact_arrow=show_contact_arrow,
mesh_lighting=mesh_lighting,
contact_arrow_style=contact_arrow_style,
)
@property