Enhance lifting checks for stability and orientation

[UttuG]

Refactor lifting logic to include detailed checks for object and end-effector stability during lift, including position and orientation stability.

[Copilot]

Pull request overview

This PR enhances the lifting validation logic in the Isaac Lab heuristic manipulation module by implementing comprehensive stability checks beyond the original simple Z-axis coupling test. The changes add multi-dimensional validation including XY position stability and 3D orientation stability to better detect successful object grasps during lift operations.

Key Changes:

• Added quaternion-based orientation tracking for objects and end-effector to validate grasp stability
• Implemented XY lateral slip detection with 2cm tolerance to ensure object remains fixed relative to gripper
• Added orientation stability checks (roll, pitch, yaw) with 10-degree tolerance to detect object rotation during lift

---

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

[Copilot]

[nitpick] Magic numbers should be extracted as named constants for better maintainability. Consider defining at the class or module level:

Z_COUPLING_TOLERANCE = 0.01  # meters
XY_STABILITY_TOLERANCE = 0.02  # meters
ORIENTATION_STABILITY_DEG = 10.0  # degrees

This makes the thresholds easier to tune and understand.

[Copilot]

Potential angle wrapping issue: Using torch.abs(yaw1 - yaw0) doesn't handle the discontinuity at ±π. For example, if yaw0 = -179° and yaw1 = 179°, the difference appears to be 358° when it's actually only 2°. Consider using angle normalization:

def normalize_angle_diff(angle_diff):
    # Wrap to [-π, π]
    return torch.atan2(torch.sin(angle_diff), torch.cos(angle_diff))

roll_diff = torch.abs(normalize_angle_diff(roll1 - roll0))
pitch_diff = torch.abs(normalize_angle_diff(pitch1 - pitch0))
yaw_diff = torch.abs(normalize_angle_diff(yaw1 - yaw0))

This ensures the smallest angular distance is measured.

Suggested change

	roll0, pitch0, yaw0 = quat_to_euler(obj_quat0)
	roll1, pitch1, yaw1 = quat_to_euler(obj_quat1)
	# Check angular changes (should be minimal for good grasp)
	roll_diff = torch.abs(roll1 - roll0)
	pitch_diff = torch.abs(pitch1 - pitch0)
	yaw_diff = torch.abs(yaw1 - yaw0)
	def normalize_angle_diff(angle_diff):
	"""Wrap angle difference to [-pi, pi]"""
	return torch.atan2(torch.sin(angle_diff), torch.cos(angle_diff))
	roll0, pitch0, yaw0 = quat_to_euler(obj_quat0)
	roll1, pitch1, yaw1 = quat_to_euler(obj_quat1)
	# Check angular changes (should be minimal for good grasp)
	roll_diff = torch.abs(normalize_angle_diff(roll1 - roll0))
	pitch_diff = torch.abs(normalize_angle_diff(pitch1 - pitch0))
	yaw_diff = torch.abs(normalize_angle_diff(yaw1 - yaw0))

[Copilot]

[nitpick] Consider extracting the quat_to_euler function outside of execute_and_lift_once_batch as a module-level or class method for better reusability and testability. Nested functions are harder to unit test and may be needed elsewhere for orientation validation.

Suggested change

	def quat_to_euler(quat):
	"""Convert wxyz quaternion to roll, pitch, yaw (radians)"""
	w, x, y, z = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3]
	# Roll (x-axis rotation)
	sinr_cosp = 2 * (w * x + y * z)
	cosr_cosp = 1 - 2 * (x * x + y * y)
	roll = torch.atan2(sinr_cosp, cosr_cosp)
	# Pitch (y-axis rotation)
	sinp = 2 * (w * y - z * x)
	sinp = torch.clamp(sinp, -1.0, 1.0)
	pitch = torch.asin(sinp)
	# Yaw (z-axis rotation)
	siny_cosp = 2 * (w * z + x * y)
	cosy_cosp = 1 - 2 * (y * y + z * z)
	yaw = torch.atan2(siny_cosp, cosy_cosp)
	return roll, pitch, yaw
	roll0, pitch0, yaw0 = quat_to_euler(obj_quat0)
	roll1, pitch1, yaw1 = quat_to_euler(obj_quat1)
	# Check angular changes (should be minimal for good grasp)
	roll_diff = torch.abs(roll1 - roll0)
	pitch_diff = torch.abs(pitch1 - pitch0)
	yaw_diff = torch.abs(yaw1 - yaw0)
	# Allow up to 10 degrees rotation in any axis
	orientation_stable = (roll_diff <= torch.deg2rad(torch.tensor(10.0, device=roll_diff.device))) & \
	(pitch_diff <= torch.deg2rad(torch.tensor(10.0, device=pitch_diff.device))) & \
	(yaw_diff <= torch.deg2rad(torch.tensor(10.0, device=yaw_diff.device))) # [B]
	# --- Combined success criteria ---
	lifted = z_coupling & z_lifted & xy_stable & orientation_stable # [B]
	# Detailed logging
	for b in range(min(B, 3)): # Log first 3 envs for debugging
	print(f" Env[{b}]: Z-coupling={z_coupling[b].item()}, "
	f"XY-stable={xy_stable[b].item()} (dev={xy_deviation[b].item():.4f}m), "
	f"Orient-stable={orientation_stable[b].item()} "
	# ─────────── Quaternion Utilities ───────────
	def quat_to_euler(quat):
	"""Convert wxyz quaternion to roll, pitch, yaw (radians)"""
	w, x, y, z = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3]
	# Roll (x-axis rotation)
	sinr_cosp = 2 * (w * x + y * z)
	cosr_cosp = 1 - 2 * (x * x + y * y)
	roll = torch.atan2(sinr_cosp, cosr_cosp)
	# Pitch (y-axis rotation)
	sinp = 2 * (w * y - z * x)
	sinp = torch.clamp(sinp, -1.0, 1.0)
	pitch = torch.asin(sinp)
	# Yaw (z-axis rotation)
	siny_cosp = 2 * (w * z + x * y)
	cosy_cosp = 1 - 2 * (y * y + z * z)
	yaw = torch.atan2(siny_cosp, cosy_cosp)
	return roll, pitch, yaw

[Copilot]

Variable ee_q1 is not used.

Suggested change

ee_q1 = ee_w1[:, 3:7]