Skip to content

Fix crashing when no grasp pose found. #232

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
matafela merged 1 commit into from
Apr 15, 2026
Merged

Fix crashing when no grasp pose found. #232

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 14 additions & 6 deletions embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -583,7 +583,7 @@ def get_grasp_poses(
approach_direction: torch.Tensor,
visualize_collision: bool = False,
visualize_pose: bool = False,
) -> tuple[torch.Tensor, torch.Tensor]:
) -> tuple[bool, torch.Tensor, float]:
"""Get grasp pose given approach direction.

Uses the antipodal point pairs stored in ``self._hit_point_pairs``
Expand All @@ -603,19 +603,20 @@ def get_grasp_poses(
after computation.

Returns:
A tuple ``(best_grasp_pose, best_open_length)`` where
``best_grasp_pose`` is a ``(4, 4)`` homogeneous matrix and
``best_open_length`` is a scalar.
is_success (bool): Whether a valid grasp pose is found.
best_grasp_pose (torch.Tensor): If a valid grasp pose is found, a tensor of shape (4, 4) representing the homogeneous transformation matrix of the best grasp pose in the world frame. Otherwise, an identity matrix.
best_open_length (float): If a valid grasp pose is found, a scalar representing the optimal gripper opening length. Otherwise, a zero tensor.

Comment on lines 605 to 609
Copy link

Copilot AI Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The return type/documentation for best_open_length is inconsistent with what the function actually returns. The type hint says float, the docstring mentions a "zero tensor" on failure, but the success path returns a 0-dim torch.Tensor (best_open_length) while failure returns a Python 0.0. Please make the return type consistent (either always return a Python float via .item() or update the type hint/docstring to torch.Tensor and return torch.tensor(0.0, device=...) on failure).

Copilot uses AI. Check for mistakes.
Raises:
RuntimeError: If :meth:`generate` or :meth:`annotate` has not
been called yet.
"""
if self._hit_point_pairs is None:
raise RuntimeError(
logger.log_warning(
"No antipodal point pairs available. "
"Call generate() or annotate() first."
)
return False, torch.eye(4, device=self.device), 0.0
Comment on lines 610 to +619
Copy link

Copilot AI Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The docstring still claims this method raises RuntimeError when _hit_point_pairs is None, but the implementation now logs a warning and returns (False, I, 0.0) instead. Please update the Raises: section (or reintroduce the exception if this is intended to remain a programmer error).

Copilot uses AI. Check for mistakes.
origin_points = self._hit_point_pairs[:, 0, :]
hit_points = self._hit_point_pairs[:, 1, :]
origin_points_ = self._apply_transform(origin_points, object_pose)
Expand All @@ -632,6 +633,10 @@ def get_grasp_poses(
valid_mask = (
positive_angle - torch.pi / 2
).abs() <= self.cfg.max_deviation_angle
if valid_mask.sum() == 0:
logger.log_warning("No valid antipodal pairs after angle filtering.")
return False, torch.eye(4, device=self.device), 0.0

valid_grasp_x = grasp_x[valid_mask]
valid_centers = centers[valid_mask]

Expand All @@ -650,6 +655,9 @@ def get_grasp_poses(
is_visual=visualize_collision,
collision_threshold=0.0,
)
if is_colliding.logical_not().sum() == 0:
logger.log_warning("No valid antipodal pairs after angle filtering.")
Copy link

Copilot AI Apr 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The warning message inside the collision-filtering failure path is incorrect/misleading: this branch triggers when all candidates collide ((~is_colliding).sum() == 0), but the log says "after angle filtering". Please change the message to reflect collision filtering (and ideally distinguish it from the earlier angle-filtering warning).

Suggested change
logger.log_warning("No valid antipodal pairs after angle filtering.")
logger.log_warning("No valid antipodal pairs after collision filtering.")

Copilot uses AI. Check for mistakes.
return False, torch.eye(4, device=self.device), 0.0
# get best grasp pose
valid_grasp_poses = valid_grasp_poses[~is_colliding]
valid_open_lengths = valid_open_lengths[~is_colliding]
Expand All @@ -674,7 +682,7 @@ def get_grasp_poses(
grasp_pose=best_grasp_pose,
open_length=best_open_length.item(),
)
return best_grasp_pose, best_open_length
return True, best_grasp_pose, best_open_length

@staticmethod
def _grasp_pose_from_approach_direction(
Expand Down
15 changes: 12 additions & 3 deletions scripts/tutorials/grasp/grasp_generator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -271,11 +271,20 @@ def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tenso
)
obj_poses = mug.get_local_pose(to_matrix=True)
grasp_xpos_list = []
for obj_pose in obj_poses:
grasp_pose, _ = grasp_generator.get_grasp_poses(

rest_xpos = robot.compute_fk(
qpos=robot.get_qpos("arm"), name="arm", to_matrix=True
)[0]
for i, obj_pose in enumerate(obj_poses):
is_success, grasp_pose, open_length = grasp_generator.get_grasp_poses(
obj_pose, approach_direction, visualize_pose=False
)
grasp_xpos_list.append(grasp_pose.unsqueeze(0))
if is_success:
grasp_xpos_list.append(grasp_pose.unsqueeze(0))
else:
logger.log_warning(f"No valid grasp pose found for {i}-th object.")
grasp_xpos_list.append(rest_xpos.unsqueeze(0))

grasp_xpos = torch.cat(grasp_xpos_list, dim=0)
cost_time = time.time() - start_time
logger.log_info(f"Get grasp pose cost time: {cost_time:.2f} seconds")
Expand Down
Loading