Conversation
There was a problem hiding this comment.
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
AntipodalSamplerfor 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.
| config = SimulationManagerCfg( | ||
| headless=True, | ||
| sim_device=args.device, | ||
| enable_rt=args.enable_rt, | ||
| physics_dt=1.0 / 100.0, | ||
| num_envs=args.num_envs, | ||
| arena_space=2.5, | ||
| ) |
There was a problem hiding this comment.
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.
| return sim | ||
|
|
||
|
|
||
| def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot: |
There was a problem hiding this comment.
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.
| 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: |
|
|
||
| def create_mug(sim: SimulationManager): | ||
| mug_cfg = RigidObjectCfg( | ||
| uid="table", |
There was a problem hiding this comment.
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.
| uid="table", | |
| uid="mug", |
| from dexsim.utility.path import get_resources_data_path | ||
|
|
||
| from embodichain.lab.sim import SimulationManager, SimulationManagerCfg | ||
| from embodichain.lab.sim.objects import Robot, RigidObject | ||
| from embodichain.lab.sim.utility.action_utils import interpolate_with_distance | ||
| from embodichain.lab.sim.shapes import MeshCfg | ||
| from embodichain.lab.sim.solvers import PytorchSolverCfg | ||
| from embodichain.data import get_data_path | ||
| from embodichain.utils import logger | ||
| from embodichain.lab.sim.cfg import ( | ||
| JointDrivePropertiesCfg, | ||
| RobotCfg, | ||
| LightCfg, | ||
| RigidBodyAttributesCfg, | ||
| RigidObjectCfg, | ||
| URDFCfg, | ||
| ) | ||
| from embodichain.lab.sim.shapes import MeshCfg | ||
| from embodichain.toolkits.graspkit.pg_grasp.antipodal_annotator import ( |
There was a problem hiding this comment.
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.
| hit_point_pairs = torch.cat( | ||
| [hit_points[:, None, :], hit_origins[:, None, :]], dim=1 |
There was a problem hiding this comment.
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.
| 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 |
| 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: |
There was a problem hiding this comment.
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).
| 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: |
| grasp_xpos = mug.get_grasp_pose( | ||
| approach_direction=torch.tensor( | ||
| [0, 0, -1], dtype=torch.float32, device=sim.device | ||
| ), # gripper approach direction in the world frame | ||
| cfg=grasp_cfg, | ||
| is_visual=True, # visualize selected grasp pose finally | ||
| ) |
There was a problem hiding this comment.
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.
| cfg: AntipodalSamplerCfg = AntipodalSamplerCfg(), | ||
| ): | ||
| self.mesh: o3d.t.geometry.TriangleMesh | None = None | ||
| self.cfg = cfg |
There was a problem hiding this comment.
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).
| 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() |
| @dataclass | ||
| class GraspAnnotatorCfg: | ||
| viser_port: int = 15531 | ||
| use_largest_connected_component: bool = False | ||
| antipodal_sampler_cfg: AntipodalSamplerCfg = AntipodalSamplerCfg() | ||
| force_regenerate: bool = False | ||
| max_deviation_angle: float = np.pi / 12 | ||
|
|
There was a problem hiding this comment.
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.
| This script demonstrates the creation and simulation of a robot with a soft object, | ||
| and performs a pressing task in a simulated environment. |
There was a problem hiding this comment.
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.
| 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. |
There was a problem hiding this comment.
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.
| convex_mesh_files = ["mesh_hull_1.obj", "mesh_hull_2.obj"] | ||
| convex_meshes = load_convex_meshes(convex_mesh_files) | ||
| if not convex_meshes: | ||
| print("No convex hulls loaded. Exiting.") | ||
| return |
There was a problem hiding this comment.
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.
| RigidObjectCfg, | ||
| URDFCfg, | ||
| ) | ||
| from embodichain.lab.sim.shapes import MeshCfg |
There was a problem hiding this comment.
MeshCfg is imported twice in this file. Please remove the duplicate import to avoid confusion.
| from embodichain.lab.sim.shapes import MeshCfg |
| 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) |
There was a problem hiding this comment.
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.
| 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() |
| from embodichain.toolkits.graspkit.pg_grasp.antipodal_annotator import ( | ||
| GraspAnnotator, | ||
| GraspAnnotatorCfg, | ||
| ) |
There was a problem hiding this comment.
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.
| 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] |
| obj_pose=poses[0], | ||
| grasp_pose=grasp_poses[0], | ||
| open_length=open_lengths[0], | ||
| ) |
There was a problem hiding this comment.
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.
| def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot: | ||
| """ | ||
| Create and configure a robot with an arm and a dexterous hand in the simulation. |
There was a problem hiding this comment.
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.
| pickle.dump(self.plane_equations, open(self.cache_path, "wb")) | ||
| else: | ||
| # load precomputed plane equations from cache | ||
| self.plane_equations = pickle.load(open(self.cache_path, "rb")) | ||
|
|
There was a problem hiding this comment.
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.
| penetration = -max_over_planes # [B, P] | ||
|
|
||
| # A point collides if its penetration exceeds the threshold | ||
| collides = penetration > threshold # [B, P] | ||
|
|
There was a problem hiding this comment.
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.
| raise ValueError( | ||
| "ray_origin and surface_origin must have the same number of rays" | ||
| ) | ||
|
|
There was a problem hiding this comment.
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.
| # 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()." | |
| ) |
| class BatchConvexCollisionCheckerCfg: | ||
| collsion_threshold: float = 0.0 | ||
| n_query_mesh_samples: int = 4096 | ||
| debug: bool = False | ||
|
|
||
|
|
There was a problem hiding this comment.
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).
| 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 |
Description
Add grasp annotator. Example:
examples/sim/demo/grasp_mug.pyTODO:
Type of change
Screenshots
Checklist
black .command to format the code base.