Add grasp annotator

[matafela]

Description

Add grasp annotator. Example: scripts/tutorials/grasp/grasp_generator.py

TODO:

• Docs and comments
• Collision filter.

Type of change

• New feature (non-breaking change which adds functionality)

Screenshots

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation
• I have added tests that prove my fix is effective or that my feature works
• Dependencies have been updated, if applicable.

[Copilot]

Pull request overview

Adds an interactive antipodal grasp annotation workflow (Viser-based) to generate grasp poses for rigid objects, plus a simulation demo showing a UR10 + gripper grasping a mug using the annotator.

Changes:

• Introduce AntipodalSampler for sampling antipodal point pairs via Open3D raycasting.
• Introduce GraspAnnotator (Viser UI) to select a mesh region, generate antipodal pairs, and compute an approach-aligned grasp pose.
• Expose grasp annotation/pose generation on RigidObject.get_grasp_pose() and add an end-to-end mug grasp demo.

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 16 comments.

File	Description
examples/sim/demo/grasp_mug.py	New demo that spawns UR10 + gripper and uses the new grasp annotator to compute a grasp pose and execute a grasp trajectory.
embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py	New sampler that generates antipodal point pairs using Open3D ray casting and length/angle constraints.
embodichain/toolkits/graspkit/pg_grasp/antipodal_annotator.py	New Viser-based annotation tool for selecting grasp regions, caching results, and computing a grasp pose aligned to an approach direction.
embodichain/lab/sim/objects/rigid_object.py	Integrates grasp annotation into core RigidObject via get_grasp_pose().

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

SimulationManagerCfg(headless=True, ...) ignores the --headless CLI argument (and will always run headless). Use headless=args.headless so the flag actually controls window creation; otherwise sim.open_window() later can be confusing/inconsistent.

[Copilot]

create_robot(..., position=[0.0, 0.0, 0.0]) uses a mutable list as a default argument. Switch to an immutable default (e.g., a tuple) or None + assignment to avoid shared-state bugs across calls.

Suggested change

	def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot:
	def create_robot(sim: SimulationManager, position=(0.0, 0.0, 0.0)) -> Robot:

[Copilot]

create_mug() sets uid="table", which is misleading and can cause collisions/confusion with actual table assets (and other examples already use uid="table" for a table object). Rename the UID to something like "mug"/"cup" to reflect the asset being spawned.

Suggested change

	uid="table",
	uid="mug",

[Copilot]

There are duplicated/unused imports in this new script: MeshCfg is imported twice, and get_resources_data_path is imported but never used. Please remove duplicates/unused imports to keep the example clean and lint-friendly.

[Copilot]

get_raycast_result() builds hit_point_pairs as [hit_point, surface_origin] (see concatenation order), but downstream (GraspAnnotator.get_approach_grasp_poses() and _extend_hit_point_pairs()) treats index 0 as the surface/origin point and index 1 as the hit point. This mismatch can flip the grasp axis and make visualization/pose computation inconsistent. Please standardize the convention (either swap here or swap all downstream uses) and document the ordering clearly.

Suggested change

	hit_point_pairs = torch.cat(
	[hit_points[:, None, :], hit_origins[:, None, :]], dim=1
	# Convention: hit_point_pairs[..., 0, :] is the surface/origin point,
	# and hit_point_pairs[..., 1, :] is the raycast hit point.
	hit_point_pairs = torch.cat(
	[hit_origins[:, None, :], hit_points[:, None, :]], dim=1

[Copilot]

get_grasp_pose() caches _grasp_annotator on first call and reuses it thereafter, but it also takes a cfg parameter. If callers pass a different cfg later (e.g., different viser_port / sampling params), it will be silently ignored because the existing annotator isn’t updated/recreated. Consider recreating the annotator when cfg changes (or remove cfg from the method signature and configure via a setter/constructor).

Suggested change

	if hasattr(self, "_grasp_annotator") is False:
	self._grasp_annotator = GraspAnnotator(cfg=cfg)
	if hasattr(self, "_hit_point_pairs") is False or cfg.force_regenerate:
	# (Re)create grasp annotator if it does not exist yet or if the
	# configuration has changed since the last call.
	annotator_needs_update = (
	not hasattr(self, "_grasp_annotator")
	or not hasattr(self, "_grasp_annotator_cfg")
	or self._grasp_annotator_cfg != cfg
	)
	if annotator_needs_update:
	self._grasp_annotator = GraspAnnotator(cfg=cfg)
	self._grasp_annotator_cfg = cfg
	# Invalidate cached hit point pairs so they will be regenerated
	# with the new annotator / configuration.
	if hasattr(self, "_hit_point_pairs"):
	del self._hit_point_pairs
	if not hasattr(self, "_hit_point_pairs") or cfg.force_regenerate:

[Copilot]

The inline comment says the approach_direction is in the mug local frame, but GraspAnnotator.get_approach_grasp_poses() treats approach_direction as a world-frame vector (it compares directly against world-transformed point pairs). Either update the comment (if world-frame is intended) or transform the vector by the mug pose before passing it in.

[Copilot]

Both AntipodalSampler.__init__(cfg: AntipodalSamplerCfg = AntipodalSamplerCfg()) and the dataclass field defaults elsewhere use an instantiated config object as a default argument. If any code mutates cfg, that mutation will be shared across instances. Prefer cfg: AntipodalSamplerCfg | None = None + cfg = cfg or AntipodalSamplerCfg() (or field(default_factory=...) for dataclass fields).

Suggested change

	cfg: AntipodalSamplerCfg = AntipodalSamplerCfg(),
	):
	self.mesh: o3d.t.geometry.TriangleMesh | None = None
	self.cfg = cfg
	cfg: AntipodalSamplerCfg | None = None,
	):
	self.mesh: o3d.t.geometry.TriangleMesh | None = None
	self.cfg = cfg or AntipodalSamplerCfg()

[Copilot]

GraspAnnotatorCfg.antipodal_sampler_cfg: AntipodalSamplerCfg = AntipodalSamplerCfg() uses a mutable object as a dataclass default. Use field(default_factory=AntipodalSamplerCfg) to avoid sharing the same config instance across GraspAnnotatorCfg objects.

[Copilot]

Module docstring describes a "soft object" and a "pressing task", but this script demonstrates grasp annotation and a mug grasp trajectory. Please update the docstring to match the actual behavior so examples are self-describing.

Suggested change

	This script demonstrates the creation and simulation of a robot with a soft object,
	and performs a pressing task in a simulated environment.
	This script demonstrates a grasping example in simulation: it creates a robot and a
	mug object, uses grasp annotation and antipodal grasp sampling utilities, and
	executes a mug grasp trajectory in a simulated environment.

[Copilot]

Pull request overview

Copilot reviewed 6 out of 6 changed files in this pull request and generated 17 comments.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

main() calls load_convex_meshes(), but that function is not defined/imported in this module, so running main() will fail. Either implement/import it or remove the unused main() benchmark scaffold.

[Copilot]

MeshCfg is imported twice in this file. Please remove the duplicate import to avoid confusion.

Suggested change

from embodichain.lab.sim.shapes import MeshCfg

[Copilot]

This module defines main() but the if __name__ == "__main__": block below doesn't call it, and there is other example code in the file. Please consolidate to a single entry point (or remove the example code) to avoid confusion and bit-rot.

Suggested change

	from embodichain.data import get_data_path
	bottle_a_path = get_data_path("ScannedBottle/moliwulong_processed.ply")
	bottle_b_path = get_data_path("ScannedBottle/yibao_processed.ply")
	bottle_a_mesh = trimesh.load(bottle_a_path)
	bottle_b_mesh = trimesh.load(bottle_b_path)
	bottle_a_verts = torch.tensor(bottle_a_mesh.vertices, dtype=torch.float32)
	bottle_a_faces = torch.tensor(bottle_a_mesh.faces, dtype=torch.int64)
	bottle_b_verts = torch.tensor(bottle_b_mesh.vertices, dtype=torch.float32)
	bottle_b_faces = torch.tensor(bottle_b_mesh.faces, dtype=torch.int64)
	collision_checker = BatchConvexCollisionChecker(bottle_a_verts, bottle_a_faces)
	poses = torch.tensor(
	[
	[
	[1, 0, 0, 0],
	[0, 1, 0, 0],
	[0, 0, 1, 1.0],
	[0, 0, 0, 1],
	],
	[
	[1, 0, 0, 0.05],
	[0, -1, 0, 0],
	[0, 0, -1, 0],
	[0, 0, 0, 1],
	],
	]
	)
	check_cfg = BatchConvexCollisionCheckerCfg(
	debug=False,
	n_query_mesh_samples=32768,
	collsion_threshold=-0.003,
	)
	collisions, penetrations = collision_checker.query(
	bottle_b_verts, bottle_b_faces, poses, cfg=check_cfg
	)
	print("Collisions:", collisions)
	print("Penetrations:", penetrations)
	main()

[Copilot]

Importing GraspAnnotator (and its dependencies like viser) at module import time makes embodichain.lab.sim.objects.rigid_object depend on the annotator stack even when users never call get_grasp_pose(). Consider moving these imports inside get_grasp_pose() (or guarding with a lazy/optional import) to reduce import-time overhead and optional-dep failures.

Suggested change

	from embodichain.toolkits.graspkit.pg_grasp.antipodal_annotator import (
	GraspAnnotator,
	GraspAnnotatorCfg,
	)
	try:
	from embodichain.toolkits.graspkit.pg_grasp.antipodal_annotator import (
	GraspAnnotator,
	GraspAnnotatorCfg,
	)
	except ImportError:
	logger.warning(
	"Optional dependency 'embodichain.toolkits.graspkit.pg_grasp.antipodal_annotator' "
	"could not be imported. Grasp-related functionality may be unavailable."
	)
	GraspAnnotator = None # type: ignore[assignment]
	GraspAnnotatorCfg = None # type: ignore[assignment]

[Copilot]

open_lengths contains tensors from get_approach_grasp_poses(), but visualize_grasp_pose() expects open_length: float and uses it in Open3D geometry transforms. Convert to a Python float (e.g., open_length.item()) before passing it to avoid runtime type errors in visualization.

[Copilot]

create_robot(sim, position=[...]) uses a mutable list as a default argument. This can lead to surprising behavior if the list is ever mutated; use None default and set a new list inside the function.

[Copilot]

This caches plane equations using pickle in a user-writable cache path and later loads it with pickle.load(). Pickle is unsafe for untrusted files and can lead to arbitrary code execution if the cache is tampered with. Prefer a safe serialization format (e.g., np.savez/json) or validate contents before loading.

[Copilot]

collides = penetration > threshold doesn't match the docstring's stated threshold semantics (and flips meaning depending on whether threshold is positive/negative). Please define threshold consistently (e.g., as safety margin outside hull) and update the comparison + docstring accordingly.

[Copilot]

get_raycast_result() uses self.mesh but doesn't check that it has been initialized (it will be None if sample() wasn't called first). Add a guard with a clear error message (or accept a mesh argument) to avoid confusing NoneType errors.

Suggested change

	# Ensure that the mesh has been initialized before performing raycasting
	if getattr(self, "mesh", None) is None:
	raise ValueError(
	"AntipodalSampler mesh is not initialized. "
	"Call sample() to initialize the mesh before calling get_raycast_result()."
	)

[Copilot]

Config field name collsion_threshold is misspelled. Since this is part of the public config API, please rename to collision_threshold (and keep a backwards-compatible alias if needed).

Suggested change

	class BatchConvexCollisionCheckerCfg:
	collsion_threshold: float = 0.0
	n_query_mesh_samples: int = 4096
	debug: bool = False
	class BatchConvexCollisionCheckerCfg:
	# Correctly spelled field name for the collision threshold.
	collision_threshold: float = 0.0
	# Backwards-compatible alias for the misspelled field name.
	collsion_threshold: float = 0.0
	n_query_mesh_samples: int = 4096
	debug: bool = False
	def __post_init__(self) -> None:
	"""
	Ensure backwards compatibility between the old misspelled
	`collsion_threshold` field and the new `collision_threshold` field.
	- If only `collsion_threshold` is set, propagate it to
	`collision_threshold` and log a deprecation warning.
	- If only `collision_threshold` is set, propagate it to
	`collsion_threshold` so legacy reads still work.
	"""
	# Case 1: user provided the old misspelled name.
	if (
	self.collsion_threshold != 0.0
	and self.collision_threshold == 0.0
	):
	logger.warning(
	"BatchConvexCollisionCheckerCfg: `collsion_threshold` is deprecated; "
	"please use `collision_threshold` instead."
	)
	self.collision_threshold = self.collsion_threshold
	# Case 2: user provided the new correct name; keep alias in sync.
	elif (
	self.collision_threshold != 0.0
	and self.collsion_threshold == 0.0
	):
	self.collsion_threshold = self.collision_threshold

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 14 comments.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

check_collision_single_hull()’s threshold semantics don’t match the docstring: the doc says points with signed distance to the hull interior <= threshold are colliding, but the implementation uses collides = penetration > threshold, which instead ignores shallow penetrations and never flags near-outside points for positive thresholds. Please clarify intended semantics and adjust either the doc or the condition so threshold behaves as expected.

Suggested change

	# A point collides if its penetration exceeds the threshold
	collides = penetration > threshold # [B, P]
	# A point collides if its signed distance to the hull interior is <= threshold
	collides = max_over_planes <= threshold # [B, P]

[Copilot]

Importing GraspAnnotator at module import time pulls in heavy optional UI dependencies (e.g., viser, trimesh, open3d) whenever RigidObject is imported, even if grasp annotation is never used. To reduce baseline dependencies and import overhead (and avoid failures in minimal/headless installs), consider moving these imports inside get_grasp_pose() and raising a clear error if optional deps are missing.

[Copilot]

open_lengths appears to represent the full gripper opening (it is computed as the distance between antipodal points and used as ±open_length/2 in visualize_grasp_pose). Here each finger is offset by open_length, which makes the effective opening 2*open_length and can over-report collisions. Consider offsetting each finger by open_length / 2 (and updating naming/docs accordingly).

Suggested change

	open_lengths_repeat = open_lengths[:, None].repeat(1, 3)
	left_finger_poses = grasp_poses.clone()
	left_finger_poses[:, :3, 3] -= left_finger_poses[:, :3, 0] * open_lengths_repeat
	right_finger_poses = grasp_poses.clone()
	right_finger_poses[:, :3, 3] += (
	right_finger_poses[:, :3, 0] * open_lengths_repeat
	half_open_lengths_repeat = (open_lengths[:, None] * 0.5).repeat(1, 3)
	left_finger_poses = grasp_poses.clone()
	left_finger_poses[:, :3, 3] -= (
	left_finger_poses[:, :3, 0] * half_open_lengths_repeat
	)
	right_finger_poses = grasp_poses.clone()
	right_finger_poses[:, :3, 3] += (
	right_finger_poses[:, :3, 0] * half_open_lengths_repeat

[Copilot]

box_surface_grid()’s docstring says the number of samples along an edge is round(L * dense) + 1 (dense = samples per unit length), but the implementation uses round(L / dense) + 1 (dense = spacing). Please align the docstring and variable naming with the actual behavior to avoid callers passing the wrong units.

[Copilot]

get_approach_grasp_poses() can crash when no valid antipodal pairs remain after filtering (e.g., valid_mask is empty or all grasps collide): downstream ops like .max(), .argmin(), and normalizations on empty tensors will raise. Add explicit checks after each filtering stage and return a clear error/None (and have callers handle it) when no feasible grasp is found.

[Copilot]

The cache file is written/read without file locking. If multiple processes/threads construct BatchConvexCollisionChecker for the same mesh concurrently, the cache can be corrupted and subsequent loads will fail. Consider writing to a temp file and atomically renaming, and/or using a file lock around cache writes/reads.

[Copilot]

AntipodalSamplerCfg’s doc says setting max_angle=0 disables random disturbance, but _random_rotate_unit_vectors() still samples a non-zero theta (and the current formula becomes problematic when max_angle < eps). Add an early return when max_angle <= 0 (or clamp the sampling range) so the behavior matches the config contract.

[Copilot]

The cost normalization divides by center_distance.max() / valid_open_lengths.max() without guarding for the max being 0 (or the tensors being length 1 with zero variance). This can produce NaNs/infs and make grasp selection unstable. Add an epsilon clamp (e.g., max().clamp_min(eps)) or conditional handling.

[Copilot]

The gripper point-cloud templates are created on the default box_surface_grid() device (CPU). If grasp_poses are on CUDA, transform_points_batch() will error due to device mismatch. Create templates on cfg.device (or grasp_poses.device) and ensure dtype/device match the incoming poses.

[Copilot]

Log message typo: "retult" → "result".

Suggested change

	f"Found existing antipodal retult. Loading cached antipodal pairs from {cache_path}"
	f"Found existing antipodal result. Loading cached antipodal pairs from {cache_path}"

[Copilot]

Pull request overview

Copilot reviewed 12 out of 12 changed files in this pull request and generated 16 comments.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

get_approach_grasp_poses() doesn't handle the case where no grasps survive filtering/collision checking. If valid_mask is empty or all candidates collide, the code will hit .max() on empty tensors and argmin will error. Please add an explicit check and return a sentinel/raise a clear exception when no valid grasp can be found.

[Copilot]

The main() CLI wiring is inconsistent with the GraspAnnotator API: GraspAnnotator requires vertices and triangles in __init__, but main() instantiates it with only cfg, and then calls tool.annotate(vertices=..., triangles=...) even though annotate() takes no parameters. As written, running this module as a script will crash; please align main() with the actual class signature.

Suggested change

	cfg = GraspAnnotatorCfg(
	force_regenerate=True,
	)
	tool = GraspAnnotator(cfg=cfg)
	hit_point_pairs = tool.annotate(
	vertices=torch.from_numpy(vertices).float(),
	triangles=torch.from_numpy(triangles).long(),
	)
	vertices_tensor = torch.from_numpy(vertices).float()
	triangles_tensor = torch.from_numpy(triangles).long()
	cfg = GraspAnnotatorCfg(
	force_regenerate=True,
	)
	tool = GraspAnnotator(
	cfg=cfg,
	vertices=vertices_tensor,
	triangles=triangles_tensor,
	)
	hit_point_pairs = tool.annotate()

[Copilot]

New collision-checking logic (convex decomposition caching + Warp kernel query) is introduced here but there are no automated tests covering it. Since the repo already has graspkit tests (tests/toolkits/test_pg_grasp.py), please add unit tests for BatchConvexCollisionChecker.query_batch_points() (e.g., on a simple box mesh with known inside/outside points) to prevent regressions.

[Copilot]

The tutorial references scripts/tutorials/grasp/grasp_generator.py via multiple literalinclude directives, but there is no such script in the repo (only grasp_mug.py exists under scripts/tutorials/grasp). This will break the Sphinx build; either add the referenced script or update the doc to include the correct filename/path.

[Copilot]

The inverse transform for obj_pose is computed incorrectly: after setting inv_obj_pose[:3,:3] = R^T, the translation should be -R^T @ t, but the code uses -(t @ R), which is not equivalent for column-vector convention. This will yield wrong gripper point clouds in object frame and can invalidate collision results.

Suggested change

	inv_obj_pose[:3, 3] = -obj_pose[:3, 3] @ obj_pose[:3, :3]
	inv_obj_pose[:3, 3] = -(obj_pose[:3, :3].T @ obj_pose[:3, 3])

[Copilot]

query() iterates with for normals, offsets in self.plane_equations:, but self.plane_equations is a dict (with keys plane_equations / plane_equation_counts) as set in __init__. This loop will iterate over the dict keys and crash when trying to unpack. Either remove query() if it's obsolete, or update it to use the packed tensors in self.plane_equations[...] (or store the original list separately).

[Copilot]

box_surface_grid()'s docstring defines dense as "samples per unit length" with a formula round(L * dense) + 1, but the implementation uses round(L / dense) + 1. Please either fix the implementation or update the docstring/parameter name (e.g., treat it as a spacing) so users configure it correctly.

Suggested change

	nx = max(2, round(sx / dense) + 1)
	ny = max(2, round(sy / dense) + 1)
	nz = max(2, round(sz / dense) + 1)
	nx = max(2, round(sx * dense) + 1)
	ny = max(2, round(sy * dense) + 1)
	nz = max(2, round(sz * dense) + 1)

[Copilot]

Type annotations are misleading here: grasp_poses: tuple[torch.Tensor] = [] and open_lengths: tuple[torch.Tensor] = [] are initialized as lists and later appended to. Prefer list[torch.Tensor] (or build tensors directly) to avoid type confusion for readers and static checkers.

Suggested change

	grasp_poses: tuple[torch.Tensor] = []
	open_lengths: tuple[torch.Tensor] = []
	grasp_poses: list[torch.Tensor] = []
	open_lengths: list[torch.Tensor] = []

[Copilot]

PR description mentions an example examples/sim/demo/grasp_mug.py, but no such file exists in the repo; this PR adds scripts/tutorials/grasp/grasp_mug.py instead. Please either update the PR description (and docs) or add the example at the stated path to avoid confusion for users.

[Copilot]

_get_gripper_pc() offsets finger poses by open_lengths along the gripper x-axis, but open_lengths is described/used elsewhere as the jaw opening width (distance between contact points). Typically each finger should be offset by open_length/2 from the centerline; otherwise the modeled gripper opening becomes 2× too large and collision checking will be inaccurate.

[Copilot]

Pull request overview

Copilot reviewed 12 out of 12 changed files in this pull request and generated 10 comments.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

get_grasp_poses() is annotated/ documented as returning a single (4,4) tensor (and “None if no valid grasp”), but it actually returns (best_grasp_pose, best_open_length) and will crash if valid_mask is empty or if all grasps are filtered out as colliding (e.g. .max() on empty tensors). Please (1) fix the return type/docstring, and (2) add explicit handling for “no valid grasps” with a clear exception or a None/sentinel return.

[Copilot]

New collision-checking code paths (BatchConvexCollisionChecker.query_batch_points, SimpleGripperCollisionChecker.query, and box_surface_grid) aren’t covered by unit tests, and there is already a tests/toolkits/test_pg_grasp.py suite for graspkit. Adding a small deterministic test (e.g., a box/sphere mesh with a known colliding/non-colliding point cloud) would prevent regressions in the distance sign convention and point-cloud sampling density.

[Copilot]

box_surface_grid() computes grid resolution using round(sx / dense), but the docstring defines dense as “samples per unit length” and the formula should scale with size (round(sx * dense)). As written, increasing dense makes the point cloud sparser, which breaks collision-check fidelity.

Suggested change

	nx = max(2, round(sx / dense) + 1)
	ny = max(2, round(sy / dense) + 1)
	nz = max(2, round(sz / dense) + 1)
	nx = max(2, round(sx * dense) + 1)
	ny = max(2, round(sy * dense) + 1)
	nz = max(2, round(sz * dense) + 1)

[Copilot]

The convex decomposition cache is pickling torch.Tensors that are created on self.device (which may be CUDA). Pickling CUDA tensors can make the cache non-portable (e.g., loading on CPU-only machines) and can be brittle across device indices. Prefer saving CPU tensors / numpy arrays (or torch.save with map_location='cpu') and moving to self.device after loading.

Suggested change

	pickle.dump(self.plane_equations, open(self.cache_path, "wb"))
	# Save CPU tensors to make the cache portable across devices
	plane_equations_cpu = {
	"plane_equations": plane_equations.cpu(),
	"plane_equation_counts": plane_equations_counts.cpu(),
	}
	pickle.dump(plane_equations_cpu, open(self.cache_path, "wb"))

[Copilot]

The object-pose inverse is computed incorrectly. For a standard homogeneous transform [R t; 0 1], the inverse translation should be -Rᵀ @ t, not -t @ R. This will place the gripper point cloud in the wrong frame and invalidate collision results. Consider using the existing embodichain.utils.math.pose_inv() helper to avoid convention mistakes.

Suggested change

	inv_obj_pose[:3, 3] = -obj_pose[:3, 3] @ obj_pose[:3, :3]
	inv_obj_pose[:3, 3] = -obj_pose[:3, :3].T @ obj_pose[:3, 3]

[Copilot]

The main() example is currently broken: GraspAnnotator is instantiated without the required vertices/triangles args, and annotate() is called with vertices=/triangles= keyword args even though annotate() takes no parameters. Update main() to match the current GraspAnnotator API (or adjust the class API to match the intended CLI).

Suggested change

	force_regenerate=True,
	)
	tool = GraspAnnotator(cfg=cfg)
	hit_point_pairs = tool.annotate(
	vertices=torch.from_numpy(vertices).float(),
	triangles=torch.from_numpy(triangles).long(),
	)
	viser_port=args.port,
	use_largest_connected_component=args.largest_component,
	force_regenerate=True,
	)
	tool = GraspAnnotator(
	cfg=cfg,
	vertices=torch.from_numpy(vertices).float(),
	triangles=torch.from_numpy(triangles).long(),
	)
	hit_point_pairs = tool.annotate()

[Copilot]

In the is_visual block, vertices, triangles, and scale are recomputed but never used (the visualizer uses self._grasp_annotator’s stored mesh). This is dead code and can be removed to avoid confusion and extra work on large meshes.

Suggested change

	vertices = self._entities[0].get_vertices()
	triangles = self._entities[0].get_triangles()
	scale = self._entities[0].get_body_scale()
	vertices = vertices * scale

[Copilot]

BatchConvexCollisionCheckerCfg exposes collsion_threshold (typo) as part of the public config API. This is easy to miss and forces callers to use the misspelling. Rename to collision_threshold (and consider a backwards-compatible alias if needed).

Suggested change

	@dataclass
	class BatchConvexCollisionCheckerCfg:
	""" Configuration for BatchConvexCollisionChecker."""
	collsion_threshold: float = 0.0
	""" Collision threshold in meters. A point is considered colliding if its signed distance to the hull interior is <= this threshold. This allows for a margin of error in collision checking, where a small positive threshold can be used to consider points near the surface as colliding, and a small negative threshold can be used to allow for slight penetration without considering it a collision."""
	n_query_mesh_samples: int = 4096
	""" Number of points to sample from the query mesh surface for collision checking. A higher number of samples can provide a more accurate collision check at the cost of increased computation time. The optimal number may depend on the complexity of the mesh and the required precision of collision detection."""
	debug: bool = False
	""" Whether to visualize the collision checking results for debugging purposes. If set to True, the code will generate visualizations of the query points colored by their collision status (e.g., red for colliding points and green for non-colliding points) along with the original mesh. This can help in understanding and verifying the collision checking process, especially during development and testing."""
	@dataclass(init=False)
	class BatchConvexCollisionCheckerCfg:
	""" Configuration for BatchConvexCollisionChecker."""
	collision_threshold: float = 0.0
	""" Collision threshold in meters. A point is considered colliding if its signed distance to the hull interior is <= this threshold. This allows for a margin of error in collision checking, where a small positive threshold can be used to consider points near the surface as colliding, and a small negative threshold can be used to allow for slight penetration without considering it a collision."""
	n_query_mesh_samples: int = 4096
	""" Number of points to sample from the query mesh surface for collision checking. A higher number of samples can provide a more accurate collision check at the cost of increased computation time. The optimal number may depend on the complexity of the mesh and the required precision of collision detection."""
	debug: bool = False
	""" Whether to visualize the collision checking results for debugging purposes. If set to True, the code will generate visualizations of the query points colored by their collision status (e.g., red for colliding points and green for non-colliding points) along with the original mesh. This can help in understanding and verifying the collision checking process, especially during development and testing."""
	def __init__(
	self,
	collision_threshold: float = 0.0,
	n_query_mesh_samples: int = 4096,
	debug: bool = False,
	collsion_threshold: Union[float, None] = None,
	):
	"""
	Initialize the configuration.
	Parameters
	----------
	collision_threshold:
	Preferred, correctly spelled collision threshold in meters.
	n_query_mesh_samples:
	Number of points to sample from the query mesh surface.
	debug:
	Whether to enable debugging visualizations.
	collsion_threshold:
	Deprecated alias for ``collision_threshold`` kept for backward compatibility.
	"""
	# Resolve threshold value, preferring the deprecated alias if explicitly provided.
	if collsion_threshold is not None and collsion_threshold != collision_threshold:
	logger.warning(
	"BatchConvexCollisionCheckerCfg: both 'collision_threshold' and "
	"'collsion_threshold' were provided; using value from deprecated "
	"'collsion_threshold'."
	)
	effective_threshold = collsion_threshold
	elif collsion_threshold is not None:
	# Only deprecated alias provided
	logger.warning(
	"BatchConvexCollisionCheckerCfg: 'collsion_threshold' is deprecated; "
	"use 'collision_threshold' instead."
	)
	effective_threshold = collsion_threshold
	else:
	effective_threshold = collision_threshold
	self.collision_threshold = effective_threshold
	# Backward-compatible attribute for any code still referencing the typo.
	self.collsion_threshold = effective_threshold
	self.n_query_mesh_samples = n_query_mesh_samples
	self.debug = debug

[Copilot]

_random_rotate_unit_vectors() does not actually disable disturbance when max_angle=0 (it still samples a small non-zero angle because of the eps term and because the sampling range becomes [0, eps]). Either update the docstring/config expectations, or explicitly short-circuit when max_angle <= 0 so the function returns the input vectors unchanged.

[Copilot]

Any is imported but unused in this module. Consider removing it to keep the kernel module minimal and avoid unused-import noise in editors/static analysis.

Suggested change

from typing import Any