diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index 5111015..e6d0021 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -1,11 +1,26 @@ ## Development Practices - Start with minimal, lean implementations focused on proof-of-concept +- Avoid creating new files until asked - Avoid implementing things from scratch - Avoid defensive error handling for hypothetical failures - Use print statements and logging sparingly, unless asked - Avoid light wrappers and custom classes, unless asked -- Avoid `if __name__ == "__main__"` patterns in package code +- Avoid `if __name__ == "__main__"` patterns in package code, unless asked + For example, rather than using: + ```python + from math import sqrt + def main(): + sqrt(2) + + if __name__ == "__main__": + main() + ``` + Leave it as a top-level script: + ```python + from math import sqrt + sqrt(2) + ``` - Skip unit tests unless explicitly requested - Follow patterns in CONTRIBUTING.md when present - Prefer writing Python if no language specified @@ -33,6 +48,7 @@ ## Change Logging +- Create CHANGELOG.md if it doesn't exist - Each time you generate code, note the changes in CHANGELOG.md - Follow semantic versioning guidelines - Include date and description of changes diff --git a/.gitignore b/.gitignore index a040e5e..1079fb8 100644 --- a/.gitignore +++ b/.gitignore @@ -61,4 +61,43 @@ franka_control_suite/build/ !robot_toolkit/robot_arm_algos/src/inference/objects/**/*.dae !robot_toolkit/robot_arm_algos/src/inference/objects/**/*.mtl -!robot_toolkit/robot_arm_algos/src/inference/objects/**/*.obj \ No newline at end of file +!robot_toolkit/robot_arm_algos/src/inference/objects/**/*.obj*.png + +# === Added empirical mapping workflow artifacts === +# Ignore historical taught position snapshots (keep primary taught_positions.yaml) +src/ur_toolkit/positions/taught_positions.snapshot.*.yaml + +# Ignore visual servo run metric JSON logs (can be large/noisy) +logs/visual_servo_runs/*.json + +# Ignore intermediate refinement matrices +refined_partial_*.npy + +# If relying solely on 6×6 mapping, axis_mapping.json can be ignored (uncomment if desired) +# src/ur_toolkit/visual_servo/axis_mapping.json + +# === Hand-eye & checkerboard calibration assets (ignored per user request) === +# Ignore entire hand_eye_calibration directory and its session logs/artifacts +src/ur_toolkit/hand_eye_calibration/ + +# Ignore any scripts primarily for hand-eye or checkerboard workflows +scripts/run_hand_eye_calibration.py +scripts/run_manual_hand_eye_calibration_extended.py +scripts/run_checkerboard_hand_eye_calibration.py +scripts/analyze_hand_eye_raw_session.py +scripts/diagnose_hand_eye_frames.py +scripts/analyze_checkerboard_session.py + +# Ignore checkerboard images and related captured datasets +checkerboard_images/ +**/checkerboard_images/ +**/handeye_calibration*/ +**/handeye_rework*/ + +# Ignore standalone analysis helpers +analyze_calibration_accuracy.py + +# Keep empirical mapping matrix and optional axis mapping +!camera_to_robot_mapping.npy +!src/ur_toolkit/visual_servo/axis_mapping.json + diff --git a/README.md b/README.md index 9997058..744ab3c 100644 --- a/README.md +++ b/README.md @@ -119,7 +119,7 @@ camera: # AprilTag Configuration apriltag: - family: "tag36h11" + family: "tagStandard41h12" tag_size: 0.023 # 23mm tags ``` @@ -181,7 +181,7 @@ from ur_toolkit.apriltag_detection import AprilTagDetector # Initialize detector detector = AprilTagDetector( - tag_family='tag36h11', + tag_family='tagStandard41h12', tag_size=0.023, # 23mm tags camera_calibration_file='camera_calibration/camera_calibration.yaml' ) @@ -228,6 +228,43 @@ python src/ur_toolkit/camera_calibration/capture_calibration_photos.py python src/ur_toolkit/camera_calibration/calculate_camera_intrinsics.py ``` +### 5. Hand-Eye Calibration + +For precise camera-to-robot coordinate transformation: + +```bash +# Run automated hand-eye calibration +python scripts/run_hand_eye_calibration.py --robot-ip 192.168.1.100 --tag-ids 0 --num-poses 15 +``` + +**What it does:** +- Automatically moves robot through 15 diverse poses +- Captures AprilTag detections at each pose +- Solves the AX = XB calibration equation using OpenCV's robust algorithm +- Saves calibration to `src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json` + +**Parameters:** +- `--robot-ip`: Your robot's IP address +- `--tag-ids`: AprilTag IDs to use (default: [0]) +- `--num-poses`: Number of calibration poses (default: 15) +- `--manual`: Use manual freedrive mode instead of automatic movement + +**Safety Notes:** +- ⚠️ Ensure workspace is clear before starting +- Keep emergency stop accessible +- Verify robot joint limits and collision avoidance +- AprilTag must be visible from all poses + +**Quality Assessment:** +- **Excellent**: Translation error < 5mm, Rotation error < 2° +- **Good**: Translation error < 10mm, Rotation error < 5° +- **Poor**: Translation error > 10mm, Rotation error > 5° (recalibrate) + +**Troubleshooting:** +- **"No AprilTag detected"**: Check lighting, tag visibility, verify tag IDs +- **"Pose not reachable"**: Start from more central robot position +- **"Poor calibration quality"**: Increase `--num-poses`, ensure good lighting + ## 🏗️ Development The project uses a `src/` layout for better packaging and testing: @@ -310,7 +347,7 @@ from apriltag_detection import AprilTagDetector robot = URRobotInterface('192.168.0.10') camera = PiCam(PiCamConfig.from_yaml('camera_client_config.yaml')) detector = AprilTagDetector( - tag_family='tag36h11', + tag_family='tagStandard41h12', tag_size=0.023, camera_calibration_file='camera_calibration/camera_calibration.yaml' ) @@ -335,7 +372,7 @@ for detection in detections: - **Pi Camera + Laptop**: Same subnet (configure in `camera_client_config.yaml`) ### AprilTag Settings -- Default: tag36h11 family, 23mm size +- Default: tagStandard41h12 family (recommended), 23mm size - Customize in detection code for your specific tags - Ensure tags are printed at exact scale for accurate pose estimation - **Robot + Pi Camera**: Different subnets OK diff --git a/analyze_calibration_accuracy.py b/analyze_calibration_accuracy.py new file mode 100644 index 0000000..c7dddd5 --- /dev/null +++ b/analyze_calibration_accuracy.py @@ -0,0 +1,207 @@ +import json +import numpy as np +from scipy.spatial.transform import Rotation as R +from pathlib import Path + +def analyze_hand_eye_calibration_quality(): + """ + Analyze hand-eye calibration quality using real geometric metrics + instead of OpenCV's misleading residual artifacts. + """ + + # Load calibration data + calib_file = Path('src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json') + with open(calib_file) as f: + data = json.load(f) + + print("🔬 HAND-EYE CALIBRATION QUALITY ANALYSIS") + print("=" * 60) + print() + + # Basic info + info = data['calibration_info'] + print(f"📅 Calibration: {info['timestamp']}") + print(f"📊 Poses collected: {info['poses_collected']}") + print(f"✅ Successful: {info['calibration_successful']}") + print() + + # Extract calibration data + T_ce = np.array(data['hand_eye_transform']['matrix']) + robot_poses = [np.array(pose) for pose in data['raw_pose_data']['robot_poses']] + tag_poses = [np.array(pose) for pose in data['raw_pose_data']['apriltag_poses']] + + print("🎯 CALIBRATION RESULT ANALYSIS:") + print(f" Camera position: [{T_ce[0,3]*1000:.1f}, {T_ce[1,3]*1000:.1f}, {T_ce[2,3]*1000:.1f}] mm") + print(f" Camera distance from TCP: {np.linalg.norm(T_ce[:3,3])*1000:.1f} mm") + + # Analyze rotation + R_ce = T_ce[:3,:3] + euler = R.from_matrix(R_ce).as_euler('xyz', degrees=True) + print(f" Camera orientation: [{euler[0]:.1f}°, {euler[1]:.1f}°, {euler[2]:.1f}°]") + print() + + # REAL QUALITY METRICS + print("� REAL QUALITY METRICS (NOT OpenCV artifacts):") + print() + + # 1. Pose diversity analysis + print("1️⃣ POSE DIVERSITY ANALYSIS:") + + # Translation diversity + robot_translations = np.array([pose[:3,3] for pose in robot_poses]) + translation_range = np.ptp(robot_translations, axis=0) * 1000 # Convert to mm + translation_volume = np.prod(translation_range) + + print(f" Translation range: [{translation_range[0]:.1f}, {translation_range[1]:.1f}, {translation_range[2]:.1f}] mm") + print(f" Workspace volume: {translation_volume/1000:.0f} cm³") + + # Rotation diversity + robot_rotations = [R.from_matrix(pose[:3,:3]) for pose in robot_poses] + angles = [] + for i in range(len(robot_rotations)): + for j in range(i+1, len(robot_rotations)): + rel_rot = robot_rotations[i].inv() * robot_rotations[j] + angle = rel_rot.magnitude() + angles.append(np.degrees(angle)) + + max_rotation_diff = max(angles) + avg_rotation_diff = np.mean(angles) + + print(f" Max rotation difference: {max_rotation_diff:.1f}°") + print(f" Average rotation difference: {avg_rotation_diff:.1f}°") + + # Quality assessment for diversity + trans_quality = "EXCELLENT" if translation_volume > 50000 else "GOOD" if translation_volume > 10000 else "POOR" + rot_quality = "EXCELLENT" if max_rotation_diff > 45 else "GOOD" if max_rotation_diff > 20 else "POOR" + + print(f" Translation diversity: {trans_quality}") + print(f" Rotation diversity: {rot_quality}") + print() + + # 2. Consistency analysis + print("2️⃣ CALIBRATION CONSISTENCY ANALYSIS:") + + # Check consistency by validating the hand-eye equation: T_base_to_tag = T_base_to_ee * T_ee_to_cam * T_cam_to_tag + consistency_errors = [] + + for i in range(len(robot_poses)): + T_base_to_ee = robot_poses[i] + T_cam_to_tag = tag_poses[i] + + # Predicted tag pose in base frame using hand-eye calibration + T_predicted_base_to_tag = T_base_to_ee @ T_ce @ T_cam_to_tag + + # For comparison, we need a reference. Let's use the first pose as reference. + if i == 0: + T_reference_base_to_tag = T_predicted_base_to_tag + continue + + # The tag should appear at the same location in base frame for all poses + T_error = T_reference_base_to_tag @ np.linalg.inv(T_predicted_base_to_tag) + + # Extract translation and rotation errors + trans_error = np.linalg.norm(T_error[:3,3]) * 1000 # mm + rot_error = R.from_matrix(T_error[:3,:3]).magnitude() * 180/np.pi # degrees + + consistency_errors.append({ + 'pose': i+1, + 'translation_error_mm': trans_error, + 'rotation_error_deg': rot_error + }) + + if consistency_errors: + avg_trans_error = np.mean([e['translation_error_mm'] for e in consistency_errors]) + avg_rot_error = np.mean([e['rotation_error_deg'] for e in consistency_errors]) + max_trans_error = max([e['translation_error_mm'] for e in consistency_errors]) + max_rot_error = max([e['rotation_error_deg'] for e in consistency_errors]) + + print(f" Average consistency error: {avg_trans_error:.2f}mm, {avg_rot_error:.2f}°") + print(f" Maximum consistency error: {max_trans_error:.2f}mm, {max_rot_error:.2f}°") + + # Quality assessment for consistency + consistency_quality = "EXCELLENT" if avg_trans_error < 5 and avg_rot_error < 2 else \ + "GOOD" if avg_trans_error < 15 and avg_rot_error < 5 else \ + "POOR" + + print(f" Consistency quality: {consistency_quality}") + + # Show worst poses + worst_trans = max(consistency_errors, key=lambda x: x['translation_error_mm']) + worst_rot = max(consistency_errors, key=lambda x: x['rotation_error_deg']) + print(f" Worst translation error: Pose {worst_trans['pose']} ({worst_trans['translation_error_mm']:.2f}mm)") + print(f" Worst rotation error: Pose {worst_rot['pose']} ({worst_rot['rotation_error_deg']:.2f}°)") + + print() + + # 3. Physical reasonableness + print("3️⃣ PHYSICAL REASONABLENESS CHECK:") + + camera_distance = np.linalg.norm(T_ce[:3,3]) * 1000 + camera_pos = T_ce[:3,3] * 1000 + + # Check if camera position makes physical sense + distance_reasonable = 50 < camera_distance < 300 # 5cm to 30cm from TCP + + # Check if camera is pointing roughly forward (positive Z in camera frame should align with some robot axis) + camera_z_direction = T_ce[:3,2] # Camera Z axis (forward direction) + camera_pointing_forward = camera_z_direction[0] > 0.5 # Roughly pointing in robot X direction + + print(f" Camera distance from TCP: {camera_distance:.1f}mm {'✅' if distance_reasonable else '⚠️'}") + print(f" Camera pointing direction: {'Forward ✅' if camera_pointing_forward else 'Other orientation ⚠️'}") + print(f" Physical setup: {'Reasonable ✅' if distance_reasonable else 'Check mounting ⚠️'}") + print() + + # 4. Overall quality assessment + print("🏆 OVERALL QUALITY ASSESSMENT:") + + quality_scores = [] + if 'trans_quality' in locals(): + quality_scores.append(trans_quality) + if 'rot_quality' in locals(): + quality_scores.append(rot_quality) + if 'consistency_quality' in locals(): + quality_scores.append(consistency_quality) + + excellent_count = quality_scores.count('EXCELLENT') + good_count = quality_scores.count('GOOD') + poor_count = quality_scores.count('POOR') + + if excellent_count >= 2: + overall_quality = "EXCELLENT" + overall_emoji = "🥇" + elif good_count >= 2 and poor_count == 0: + overall_quality = "GOOD" + overall_emoji = "🥈" + elif poor_count <= 1: + overall_quality = "ACCEPTABLE" + overall_emoji = "🥉" + else: + overall_quality = "POOR" + overall_emoji = "❌" + + print(f" {overall_emoji} Overall calibration quality: {overall_quality}") + print() + + # 5. Recommendations + print("💡 RECOMMENDATIONS:") + + if overall_quality == "EXCELLENT": + print(" ✅ Calibration is excellent - ready for precise visual servoing") + print(" ✅ Expected accuracy: 1-3mm translation, 0.5-1.5° rotation") + elif overall_quality == "GOOD": + print(" ✅ Calibration is good - suitable for most visual servoing tasks") + print(" ✅ Expected accuracy: 2-5mm translation, 1-3° rotation") + elif overall_quality == "ACCEPTABLE": + print(" ⚠️ Calibration is acceptable but could be improved") + print(" 📝 Consider recalibrating with more diverse poses") + print(" 📝 Expected accuracy: 3-8mm translation, 2-5° rotation") + else: + print(" ❌ Calibration quality is poor - recalibration recommended") + print(" 📝 Increase pose diversity, especially rotations") + print(" 📝 Check physical setup and AprilTag mounting") + + print() + print("📋 NOTE: These are REAL quality metrics, not OpenCV's misleading 500+mm 'residuals'") + +if __name__ == "__main__": + analyze_hand_eye_calibration_quality() \ No newline at end of file diff --git a/camera_perspective_animation.gif b/camera_perspective_animation.gif new file mode 100644 index 0000000..34d04e5 Binary files /dev/null and b/camera_perspective_animation.gif differ diff --git a/camera_perspective_simulation.py b/camera_perspective_simulation.py new file mode 100644 index 0000000..9d3bd1c --- /dev/null +++ b/camera_perspective_simulation.py @@ -0,0 +1,349 @@ +#!/usr/bin/env python3 +""" +Camera Perspective Visual Servoing Simulation +Shows how AprilTag appears in camera view during convergence +Uses pupil-apriltags for realistic AprilTag detection +""" + +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.patches import Rectangle, FancyArrowPatch +from matplotlib import patches +import cv2 + +try: + from pupil_apriltags import Detector + APRILTAG_DETECTOR_AVAILABLE = True +except ImportError: + print("❌ pupil-apriltags not installed. This simulation requires it.") + print(" Install with: pip install pupil-apriltags") + print() + raise ImportError("pupil-apriltags is required for camera perspective simulation") + +class CameraPerspectiveSimulation: + """Simulates camera view of AprilTag during visual servoing""" + + def __init__(self): + # Camera intrinsics (simulated) + self.focal_length = 500 # pixels + self.image_width = 640 + self.image_height = 480 + self.cx = self.image_width / 2 + self.cy = self.image_height / 2 + + # AprilTag physical size (23mm) + self.tag_size = 0.023 # meters + self.tag_family = 'tag36h11' + self.tag_id = 0 + + # Target pose (where we want the tag to be in camera frame) + # Tag centered and at ~0.3m distance + self.target_camera_to_tag = np.array([0.0, 0.0, 0.3, 0.0, 0.0, 0.0]) + + # Initial pose (offset and rotated significantly) + self.current_camera_to_tag = np.array([0.06, -0.05, 0.25, 0.6, -0.4, 0.3]) + + # Control parameters + self.control_gain = 0.8 # Increased for faster convergence + self.max_iters = 300 # Increased to show complete convergence + + # History + self.pose_history = [self.current_camera_to_tag.copy()] + + # Initialize AprilTag detector + self.detector = Detector( + families=self.tag_family, + nthreads=1, + quad_decimate=1.0, + quad_sigma=0.0, + refine_edges=1, + decode_sharpening=0.25, + debug=0 + ) + # Camera parameters for pupil-apriltags: [fx, fy, cx, cy] + self.camera_params = [self.focal_length, self.focal_length, self.cx, self.cy] + + def generate_apriltag_pattern(self, size=200, tag_id=0): + """Load actual AprilTag image for realistic detection""" + import os + import urllib.request + + # Path to AprilTag image + tag_file = 'apriltag_base.png' + + # Download if not exists + if not os.path.exists(tag_file): + try: + url = "https://raw.githubusercontent.com/AprilRobotics/apriltag-imgs/master/tag36h11/tag36_11_00000.png" + print(f"📥 Downloading AprilTag image...") + urllib.request.urlretrieve(url, tag_file) + print(f"✅ Downloaded {tag_file}") + except Exception as e: + print(f"⚠️ Could not download AprilTag: {e}") + raise RuntimeError("pupil-apriltags requires real AprilTag images for detection") + + # Load and resize the AprilTag + tag_base = cv2.imread(tag_file, cv2.IMREAD_GRAYSCALE) + if tag_base is None: + raise RuntimeError(f"Could not load {tag_file}") + + # Resize using nearest neighbor to preserve the pattern + tag_pattern = cv2.resize(tag_base, (size, size), interpolation=cv2.INTER_NEAREST) + return tag_pattern + + def project_tag_corners(self, camera_to_tag): + """Project AprilTag corners to image plane""" + # Tag corners in tag frame (4 corners of square) + half_size = self.tag_size / 2 + tag_corners_3d = np.array([ + [-half_size, -half_size, 0], + [ half_size, -half_size, 0], + [ half_size, half_size, 0], + [-half_size, half_size, 0] + ]) + + # Apply rotation + from scipy.spatial.transform import Rotation as R + rotation = R.from_rotvec(camera_to_tag[3:]) + rot_matrix = rotation.as_matrix() + + # Transform corners + corners_camera = [] + for corner in tag_corners_3d: + # Rotate then translate + corner_rotated = rot_matrix @ corner + corner_camera = corner_rotated + camera_to_tag[:3] + corners_camera.append(corner_camera) + + corners_camera = np.array(corners_camera) + + # Project to image plane (simple pinhole model) + corners_image = [] + for corner in corners_camera: + if corner[2] > 0.01: # Avoid division by zero + u = self.focal_length * corner[0] / corner[2] + self.cx + v = self.focal_length * corner[1] / corner[2] + self.cy + corners_image.append([u, v]) + else: + corners_image.append([self.cx, self.cy]) # Default to center + + return np.array(corners_image) + + def render_camera_view_with_target(self, camera_to_tag, iteration): + """Render camera view with target reference side-by-side""" + fig = plt.figure(figsize=(14, 6)) + + # Left subplot: Current view + ax1 = fig.add_subplot(1, 2, 1) + self._render_single_view(ax1, camera_to_tag, iteration, is_target=False) + + # Right subplot: Target view (reference) + ax2 = fig.add_subplot(1, 2, 2) + self._render_single_view(ax2, self.target_camera_to_tag, iteration, is_target=True) + + plt.tight_layout() + return fig + + def _render_single_view(self, ax, camera_to_tag, iteration, is_target=False): + """Render a single camera view""" + # Background + ax.set_xlim(0, self.image_width) + ax.set_ylim(self.image_height, 0) # Image coordinates: origin at top-left + ax.set_aspect('equal') + ax.set_facecolor('#e8e8e8') + + if not is_target: + # Draw crosshair (target center) only in current view + ax.plot([self.cx-30, self.cx+30], [self.cy, self.cy], 'g--', linewidth=1.5, alpha=0.6) + ax.plot([self.cx, self.cx], [self.cy-30, self.cy+30], 'g--', linewidth=1.5, alpha=0.6) + + # Project tag corners + corners = self.project_tag_corners(camera_to_tag) + + # Generate AprilTag pattern + tag_pattern = self.generate_apriltag_pattern(size=200) + + # Warp the tag pattern to the projected corners + # Source points (corners of the pattern image) + src_pts = np.array([[0, 0], [200, 0], [200, 200], [0, 200]], dtype=np.float32) + # Destination points (projected corners) + dst_pts = corners.astype(np.float32) + + # Compute perspective transform + M = cv2.getPerspectiveTransform(src_pts, dst_pts) + + # Warp the tag pattern + warped_tag = cv2.warpPerspective(tag_pattern, M, + (self.image_width, self.image_height), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=200) + + # Display the warped tag + ax.imshow(warped_tag, cmap='gray', vmin=0, vmax=255, alpha=0.9) + + # Detect AprilTag using pupil-apriltags (not in target view) + detection_text = "" + if not is_target: + try: + detections = self.detector.detect( + warped_tag, + estimate_tag_pose=True, + camera_params=self.camera_params, + tag_size=self.tag_size + ) + if detections: + det = detections[0] + + # Extract pose information + # Translation vector (XYZ position in meters) + tvec = det.pose_t.flatten() + x, y, z = tvec[0], tvec[1], tvec[2] + + # Rotation matrix to roll-pitch-yaw + from scipy.spatial.transform import Rotation as R + rot = R.from_matrix(det.pose_R) + roll, pitch, yaw = rot.as_euler('xyz', degrees=True) + + detection_text = (f"\n✓ Detected: ID {det.tag_id} | DM: {det.decision_margin:.1f}\n" + f"XYZ: ({x*1000:.1f}, {y*1000:.1f}, {z*1000:.1f})mm\n" + f"RPY: ({roll:.1f}°, {pitch:.1f}°, {yaw:.1f}°)") + + # Draw detected corners in green if detection is good + if det.decision_margin > 50: + detected_corners = det.corners + detected_polygon = patches.Polygon(detected_corners, closed=True, + edgecolor='green', facecolor='none', + linewidth=2, linestyle='-', alpha=0.7) + ax.add_patch(detected_polygon) + else: + detection_text = "\n✗ No detection" + except Exception as e: + detection_text = f"\n⚠️ Detection error: {str(e)[:30]}" + + # Draw tag outline + tag_polygon = patches.Polygon(corners, closed=True, + edgecolor='red', facecolor='none', + linewidth=2, linestyle='--') + ax.add_patch(tag_polygon) + + if is_target: + ax.set_title('Target Position\n(Goal)', fontsize=12, fontweight='bold', color='green') + else: + # Calculate errors + position_error = np.linalg.norm(camera_to_tag[:3] - self.target_camera_to_tag[:3]) + rotation_error = np.linalg.norm(camera_to_tag[3:] - self.target_camera_to_tag[3:]) + + # Center offset in pixels + tag_center_img = corners.mean(axis=0) + pixel_offset = np.linalg.norm(tag_center_img - np.array([self.cx, self.cy])) + + ax.set_title(f'Current View - Iteration {iteration}\n' + f'Pos Err: {position_error*1000:.1f}mm | ' + f'Rot Err: {rotation_error:.3f}rad | ' + f'Px Off: {pixel_offset:.1f}px' + f'{detection_text}', + fontsize=10, fontweight='bold') + + ax.set_xlabel('Image Width (pixels)', fontsize=9) + ax.set_ylabel('Image Height (pixels)', fontsize=9) + ax.grid(True, alpha=0.2, linewidth=0.5) + + def step(self): + """Execute one visual servoing iteration""" + # Compute error + pose_error = self.current_camera_to_tag - self.target_camera_to_tag + + # Compute velocity command (simple PBVS) + velocity = -self.control_gain * pose_error + + # Apply velocity limits + max_vel = 0.05 + vel_magnitude = np.linalg.norm(velocity) + if vel_magnitude > max_vel: + velocity = velocity * max_vel / vel_magnitude + + # Update pose + dt = 1.0/30.0 + self.current_camera_to_tag += velocity * dt + self.pose_history.append(self.current_camera_to_tag.copy()) + + return np.linalg.norm(pose_error[:3]) + + def run_simulation(self): + """Run the simulation and save frames""" + print("🎥 Camera Perspective Simulation") + print("=" * 50) + + import os + os.makedirs('camera_frames', exist_ok=True) + + # Create frames every 3 iterations for smoother animation + frame_interval = 3 + + for iteration in range(self.max_iters): + error = self.step() + + if iteration % frame_interval == 0 or iteration == self.max_iters - 1: + fig = self.render_camera_view_with_target(self.pose_history[iteration], iteration) + plt.savefig(f'camera_frames/frame_{iteration:03d}.png', + dpi=100, bbox_inches='tight') + plt.close() + print(f"📸 Frame {iteration}: Error = {error*1000:.1f}mm") + + print(f"✅ Created frames for camera perspective") + + def create_gif(self): + """Compile frames into GIF""" + try: + import imageio + import glob + + frames = [] + frame_files = sorted(glob.glob('camera_frames/frame_*.png')) + + print(f"📹 Compiling {len(frame_files)} frames into GIF...") + for frame_file in frame_files: + img = imageio.imread(frame_file) + # Add each frame twice for slower animation + for _ in range(2): + frames.append(img) + + # Save as GIF + imageio.mimsave('camera_perspective_animation.gif', frames, + duration=0.15, loop=0) + print("✅ Saved animation to camera_perspective_animation.gif") + + # Clean up + import shutil + shutil.rmtree('camera_frames') + print("🧹 Cleaned up temporary frames") + + except Exception as e: + print(f"⚠️ Could not create GIF: {e}") + +def main(): + """Run camera perspective simulation""" + print("=" * 60) + print("Camera Perspective Visual Servoing Simulation") + print("Shows AprilTag detection during convergence") + print("Using pupil-apriltags for AprilTag detection") + print("=" * 60) + print() + + sim = CameraPerspectiveSimulation() + sim.run_simulation() + sim.create_gif() + + print() + print("✅ Camera perspective simulation complete!") + print() + print("This shows how the AprilTag appears in the camera") + print("as the robot converges to the target pose using speedL().") + print("Left: Current view | Right: Target reference") + print() + print("Green outline = pupil-apriltags detection") + print("Red dashed outline = ground truth projection") + +if __name__ == "__main__": + main() diff --git a/camera_to_robot_mapping.npy b/camera_to_robot_mapping.npy new file mode 100644 index 0000000..e53216f Binary files /dev/null and b/camera_to_robot_mapping.npy differ diff --git a/compare_calibrations.py b/compare_calibrations.py new file mode 100644 index 0000000..c692071 --- /dev/null +++ b/compare_calibrations.py @@ -0,0 +1,73 @@ +import json +import numpy as np + +# Load current calibration +with open('src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json') as f: + current = json.load(f) + +print("=== HAND-EYE CALIBRATION COMPARISON ===\n") + +# Previous calibration data (from conversation history) +prev_poses = 8 +prev_translation = [-113.73, 11.49, 28.88] # mm +prev_trans_error = 753.38 # mm +prev_rot_error = 119.06 # degrees + +# Current calibration data +curr_poses = current['calibration_info']['poses_collected'] +curr_trans = current['hand_eye_transform']['translation_mm'] +curr_translation = [curr_trans['x'], curr_trans['y'], curr_trans['z']] +curr_accuracy = current['calibration_accuracy']['residuals'] +curr_trans_error = curr_accuracy['average_translation_error_mm'] +curr_rot_error = curr_accuracy['average_rotation_error_deg'] + +print("📅 PREVIOUS CALIBRATION (Sep 26):") +print(f" Poses collected: {prev_poses}") +print(f" Translation: [{prev_translation[0]:.1f}, {prev_translation[1]:.1f}, {prev_translation[2]:.1f}] mm") +print(f" Average errors: {prev_trans_error:.1f}mm translation, {prev_rot_error:.1f}° rotation") +print() + +print("📅 CURRENT CALIBRATION (Sep 29):") +print(f" Poses collected: {curr_poses}") +print(f" Translation: [{curr_translation[0]:.1f}, {curr_translation[1]:.1f}, {curr_translation[2]:.1f}] mm") +print(f" Average errors: {curr_trans_error:.1f}mm translation, {curr_rot_error:.1f}° rotation") +print() + +# Calculate differences +translation_shift = np.array(curr_translation) - np.array(prev_translation) +total_shift = np.linalg.norm(translation_shift) +error_change_trans = curr_trans_error - prev_trans_error +error_change_rot = curr_rot_error - prev_rot_error + +print("🔍 COMPARISON ANALYSIS:") +print(f" Translation shift: [{translation_shift[0]:.1f}, {translation_shift[1]:.1f}, {translation_shift[2]:.1f}] mm") +print(f" Total position shift: {total_shift:.1f} mm") +print(f" Error change: {error_change_trans:+.1f}mm translation, {error_change_rot:+.1f}° rotation") +print() + +print("📊 QUALITY ASSESSMENT:") +print(f" Data quantity: {'✅ Better' if curr_poses > prev_poses else '❌ Same/Worse'} ({curr_poses} vs {prev_poses} poses)") +print(f" Position stability: {'✅ Good' if total_shift < 50 else '⚠️ Significant shift'} ({total_shift:.1f}mm shift)") +print(f" Error consistency: {'✅ Consistent' if abs(error_change_trans) < 100 else '⚠️ Variable'} ({error_change_trans:+.1f}mm change)") + +# Interpretation +print("\n🎯 INTERPRETATION:") +if total_shift < 30: + print(" Position difference is small - calibrations are reasonably consistent") +elif total_shift < 60: + print(" Moderate position difference - some variation but acceptable") +else: + print(" Large position difference - indicates calibration variability") + +if abs(error_change_trans) < 50: + print(" Error levels are similar - both calibrations have comparable internal consistency") +else: + print(" Different error levels - may indicate different pose diversity or quality") + +print(f"\n💡 RECOMMENDATION:") +if curr_poses > prev_poses and total_shift < 50: + print(" Current calibration is better: More poses + consistent position") +elif total_shift > 60: + print(" Consider recalibrating with more rotational diversity for stability") +else: + print(" Both calibrations are reasonable - use current for most recent data") \ No newline at end of file diff --git a/config/config.yaml b/config/config.yaml index 2b50842..d7114c2 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -41,7 +41,7 @@ camera: # Pi Camera Server (remote camera access) server: - host: "100.114.26.7" # Pi camera server IP address + host: "100.86.179.18" # Pi camera server IP address port: 2222 # Server port for camera communication timeout: 10 # Connection timeout in seconds @@ -52,13 +52,13 @@ camera: # Camera calibration calibration: - file: "camera_calibration/camera_calibration.yaml" # Path to calibration file + file: "src/ur_toolkit/camera_calibration/camera_calibration.yaml" # Path to calibration file auto_load: true # Load calibration automatically on startup required: true # Require calibration for pose estimation # AprilTag Detection Configuration apriltag: - family: "tag36h11" # Tag family: tag36h11, tag25h9, tag16h5 + family: "tag36h11" # Tag family: tag36h11 (current physical tags), tagStandard41h12, tagStandard52h13, tag25h9, tag16h5 tag_size: 0.023 # Physical tag size in meters (23mm) # Detection parameters @@ -88,7 +88,7 @@ coordinates: paths: project_root: "." config_dir: "." - camera_calibration_dir: "camera_calibration" + camera_calibration_dir: "src/ur_toolkit/camera_calibration" photos_dir: "photos" logs_dir: "logs" archive_dir: "archive" @@ -131,30 +131,35 @@ safety: # Visual Servoing Configuration visual_servo: # Correction parameters - max_iterations: 10 # Reasonable iterations with correct direction + enabled: true # Master toggle: false skips all visual servo actions + max_iterations: 15 # Allow more iterations with higher damping detection_samples: 3 # Fewer samples per iteration for speed detection_timeout: 5.0 # Timeout for AprilTag detection in seconds + enable_rotation: true # Disable rotation correction for stability - # Convergence tolerances - position_tolerance: 0.010 # Position tolerance for convergence in meters (10mm) - rotation_tolerance: 0.070 # Rotation tolerance for convergence in radians (~4 degrees) + # Convergence tolerances (tightened to force proper visual servoing) + position_tolerance: 0.003 # Position tolerance for convergence in meters (3mm) + rotation_tolerance: 0.017 # Rotation tolerance for convergence in radians (~1 degree) # Safety limits per iteration safety_limits: max_translation: 0.05 # Maximum translation correction per iteration in meters (5cm) max_rotation: 0.35 # Maximum rotation correction per iteration in radians (~20 degrees) - max_total_correction: 0.15 # Maximum total correction across all iterations in meters (15cm) + max_total_correction: 0.20 # Maximum total correction across all iterations in meters (20cm) # Correction damping (to prevent oscillation) - damping_factor: 0.05 # Very conservative to prevent oscillation + damping_factor: 0.30 # Pose history management - enable_pose_history: true # Enable pose correction history tracking + enable_pose_history: false # Enable pose correction history tracking max_history_entries: 50 # Maximum number of history entries per position auto_update_positions: true # Automatically update stored positions after successful correction + reset_positions_on_start: true # If true, revert any propagated visual servo coordinate changes + enable_iteration_logging: true # Log per-iteration servo metrics to logs/visual_servo_iterations.csv # Advanced settings coordinate_frame: "camera" # Reference frame for corrections: camera, robot_base - correction_method: "direct" # Correction method: direct, transform_chain + correction_method: "simple" # Correction method: simple, direct, transform_chain + simple_mode: false # Use proper IBVS with coordinate frame transformation enable_drift_detection: true # Enable automatic drift detection drift_check_interval: 300 # Drift check interval in seconds (5 minutes) \ No newline at end of file diff --git a/docs/APRILTAG_REVIEW.md b/docs/APRILTAG_REVIEW.md new file mode 100644 index 0000000..99c7ac9 --- /dev/null +++ b/docs/APRILTAG_REVIEW.md @@ -0,0 +1,304 @@ +# AprilTag Functionality Review - UR Toolkit + +## Executive Summary + +The UR Toolkit implements a comprehensive AprilTag-based vision system for robotic applications, with a focus on visual servoing and pose correction. The implementation demonstrates several positive patterns including modular design, robust error handling, and integration with workflow systems. However, the project has deliberately excluded hand-eye calibration to simplify the architecture, which both streamlines the system and limits certain advanced capabilities. + +## Current Implementation Overview + +### Core Components + +#### 1. AprilTag Detection Module (`apriltag_detection.py`) +**Strengths:** +- **Robust Library Integration**: Uses `pupil-apriltags` library with proper error handling for missing dependencies +- **Comprehensive Pose Estimation**: Implements full 6DOF pose estimation using camera calibration data +- **Flexible Configuration**: Supports multiple tag families (tag36h11, tag25h9, tag16h5) and configurable tag sizes +- **Quality Assessment**: Includes detection quality metrics (Hamming distance, decision margin) for reliability filtering +- **Rich Output Format**: Provides position, orientation, distance, and quality metrics for each detection + +**Key Features:** +```python +# Comprehensive detection output +detection = { + 'tag_id': int, + 'family': str, + 'corners': np.array, + 'center': np.array, + 'pose': { + 'rotation_vector': list, + 'translation_vector': list, + 'rotation_matrix': list + }, + 'distance': float, + 'distance_mm': float, + 'hamming': int, + 'decision_margin': float +} +``` + +- **Visualization Capabilities**: Built-in functions to draw detections, pose axes, and quality annotations +- **Camera Calibration Integration**: Seamless loading of YAML-format camera calibration files +- **Multiple Operating Modes**: Single-shot and continuous detection modes + +#### 2. Visual Servoing Engine (`visual_servo_engine.py`) +**Strengths:** +- **Eye-in-Hand Architecture**: Proper implementation of IBVS (Image-Based Visual Servoing) control law +- **PID Control System**: Individual PID controllers for each axis with anti-windup protection +- **Detection Filtering**: Sophisticated filtering system to handle noisy detections +- **Pose History Management**: Tracks pose history for stability analysis and motion planning +- **Workflow Integration**: Seamlessly integrates with the workflow execution system + +**Implementation Highlights:** +```python +class EyeInHandPIDController: + def __init__(self, kp=1.0, ki=0.0, kd=0.0, + output_limit=1.0, integral_limit=1.0): + # Anti-windup integral control + # Derivative kick prevention + # Configurable output limiting +``` + +#### 3. Workflow Integration (`workflow_executor.py`) +**Strengths:** +- **YAML-Based Configuration**: Human-readable workflow definitions +- **Visual Servo Actions**: Direct integration of visual servoing in workflow steps +- **Position Correction**: Automatic pose correction using AprilTag detection +- **Error Recovery**: Graceful handling of detection failures and servo errors + +**Workflow Example:** +```yaml +steps: + - action: move_to_position + position: approach_point + visual_servo: true # Enable visual correction + - action: visual_servo + position: target_position + update_stored_pose: true +``` + +### Architectural Design Patterns + +#### 1. Modular Component Architecture +- **Separation of Concerns**: Detection, servoing, and control are distinct modules +- **Dependency Injection**: Components accept external dependencies (cameras, robots) +- **Configuration Management**: Centralized configuration system with path resolution + +#### 2. Error Handling and Graceful Degradation +- **Optional Dependencies**: System continues to function without certain components +- **Detection Quality Filtering**: Only uses high-quality detections for critical operations +- **Fallback Mechanisms**: Falls back to original poses when visual servo fails + +#### 3. Integration Patterns +- **Camera Abstraction**: Works with different camera interfaces (PiCam, etc.) +- **Robot Abstraction**: Generic robot interface for different robot types +- **Workflow Engine**: Embedded visual servoing in general automation workflows + +## Hand-Eye Calibration Decision Analysis + +### Current Approach: No Hand-Eye Calibration + +The project has made a **deliberate architectural decision** to exclude hand-eye calibration, as documented in the changelog: + +> "Hand-eye calibration was not producing realistic results. AprilTag detection works effectively without requiring camera-to-robot transformation. Simplified codebase focuses on core functionality." + +#### Advantages of This Approach: +1. **Simplified Architecture**: Eliminates complex coordinate transformations +2. **Reduced Calibration Overhead**: No need for extensive calibration procedures +3. **Improved Reliability**: Fewer sources of cumulative error +4. **Faster Deployment**: Reduced setup time for new installations +5. **Maintainability**: Less complex codebase with fewer failure modes + +#### Limitations: +1. **Limited Coordinate Transformation**: Cannot directly convert between camera and robot coordinates +2. **Relative Motion Only**: System works primarily with relative positioning +3. **Reduced Precision**: May have lower absolute positioning accuracy +4. **Limited Applications**: Some advanced robotic applications require precise coordinate mapping + +### Alternative Approaches in the Codebase + +The documentation shows the system previously implemented comprehensive hand-eye calibration: + +```python +# From APRILTAG_WORKFLOW.md (legacy approach) +def transform_camera_to_robot(camera_pose, hand_eye_transform): + """Transform pose from camera coordinates to robot coordinates""" + camera_transform = pose_to_transform_matrix(camera_pose) + robot_transform = hand_eye_transform @ camera_transform + return transform_matrix_to_pose(robot_transform) +``` + +The current implementation uses simpler approaches: + +#### Fixed Offset Method (Documentation): +```python +def camera_to_robot_approximate(camera_pos, camera_offset): + """Convert camera coordinates using fixed offset""" + robot_pos = [ + camera_pos[0] + camera_offset[0], + camera_pos[1] + camera_offset[1], + camera_pos[2] + camera_offset[2] + ] + return robot_pos +``` + +#### Visual Servoing Method (Current Implementation): +```python +def center_tag_in_view(robot, detection): + """Move robot to center AprilTag in camera view""" + x_offset = detection['pose']['tvec'][0] + if abs(x_offset) > 0.01: # 1cm tolerance + # Move robot based on camera offset + new_pose[1] += x_offset * 0.5 # Scale factor +``` + +## Missing Functionality and Enhancement Opportunities + +### 1. Advanced Visual Servoing Features + +#### Missing Features: +- **Multi-tag Servoing**: No support for using multiple AprilTags simultaneously +- **Velocity-based Control**: Only position-based control implemented +- **Advanced Trajectory Planning**: Limited path planning for visual servoing +- **Force Integration**: No force feedback integration with visual servoing + +#### Potential Enhancements: +```python +# Multi-tag servoing example +class MultiTagVisualServo: + def servo_to_multiple_tags(self, target_tags, weights): + """Servo to optimal pose considering multiple AprilTags""" + combined_error = self.compute_weighted_error(target_tags, weights) + return self.execute_servo_motion(combined_error) +``` + +### 2. Calibration and Setup Tools + +#### Missing Features: +- **Automated Camera Calibration**: Manual calibration process only +- **System Validation Tools**: Limited calibration quality assessment +- **Tag Placement Optimization**: No tools for optimal tag placement analysis + +#### Enhancement Opportunities: +```python +# Automated calibration validation +class CalibrationValidator: + def validate_detection_accuracy(self, known_poses, detected_poses): + """Validate AprilTag detection accuracy against ground truth""" + return self.compute_accuracy_metrics(known_poses, detected_poses) +``` + +### 3. Advanced Applications + +#### Not Yet Implemented: +- **Dynamic Object Tracking**: No continuous tracking of moving objects +- **Multi-robot Coordination**: Single robot focus only +- **Learning-based Improvements**: No adaptive learning from successful servoing +- **Predictive Servoing**: No prediction of object motion + +### 4. Integration Enhancements + +#### Current Limitations: +- **Limited Camera Support**: Primarily Pi Camera focus +- **Single Robot Type**: UR robot specific implementation +- **Basic Workflow Actions**: Limited visual servo workflow actions + +## Recommendations + +### Immediate Improvements (High Priority) + +1. **Enhanced Visual Servoing** + ```python + # Velocity-based visual servoing + class VelocityBasedVisualServo: + def compute_velocity_command(self, image_error, depth_error): + """Compute 6DOF velocity commands for smoother motion""" + return velocity_command + ``` + +2. **Multi-tag Support** + ```python + # Multiple tag management + class MultiTagManager: + def select_best_tags(self, detections, min_quality=0.5): + """Select optimal tags for servoing based on quality metrics""" + return filtered_tags + ``` + +3. **Improved Error Recovery** + ```python + # Robust error handling + class ServoErrorRecovery: + def recover_from_detection_loss(self, last_known_pose): + """Implement recovery strategies when tags are lost""" + return recovery_action + ``` + +### Long-term Enhancements (Medium Priority) + +1. **Optional Hand-Eye Calibration Module** + - Keep the current simplified approach as default + - Provide optional hand-eye calibration for advanced users + - Use feature flags to enable/disable calibration features + +2. **Advanced Control Algorithms** + - Implement model predictive control for visual servoing + - Add adaptive control parameters based on performance + - Integrate force feedback for contact-rich tasks + +3. **Machine Learning Integration** + - Tag detection confidence learning + - Servo parameter optimization + - Failure prediction and prevention + +### System Integration Improvements (Low Priority) + +1. **Broader Hardware Support** + - Generic camera interface for multiple camera types + - Multi-robot system support + - Integration with external vision systems + +2. **Advanced Workflow Features** + - Conditional visual servoing based on detection quality + - Parallel visual servoing operations + - Integration with task planning systems + +## Conclusion + +The UR Toolkit's AprilTag functionality represents a **well-engineered, production-ready implementation** that prioritizes reliability and simplicity over comprehensive feature coverage. The decision to exclude hand-eye calibration is **architecturally sound** for the target use cases and significantly reduces system complexity. + +### Key Strengths: +1. **Robust Core Implementation**: Solid AprilTag detection with comprehensive pose estimation +2. **Practical Visual Servoing**: Effective eye-in-hand visual servoing with PID control +3. **Excellent Integration**: Seamless workflow integration and modular architecture +4. **Production Focus**: Emphasis on reliability over feature completeness + +### Strategic Recommendations: +1. **Maintain Current Architecture**: The simplified approach without hand-eye calibration should remain the default +2. **Selective Feature Addition**: Add advanced features (multi-tag, velocity control) as optional modules +3. **Improved Documentation**: Expand examples and tutorials for common use cases +4. **Community Contributions**: Enable community extensions while maintaining core simplicity + +The implementation demonstrates excellent software engineering practices and provides a solid foundation for robotic vision applications while maintaining the design philosophy of simplicity and reliability over feature richness. + +## Related Resources + +### AC Training Lab Integration +Based on the GitHub issues from `ac-dev-lab`, the AprilTag functionality is being actively developed and integrated across multiple projects: + +1. **Issue #327**: Implementation for SDL2 integration with Pi Camera setup +2. **Issue #229**: Minimal working example with dedicated hardware using vial turntable +3. **Issue #330**: Integration with UR3e Python control systems +4. **Issue #329**: Systematic fixturing of AprilTags to equipment and samples +5. **Issue #70**: Automated label printer setup for AprilTag generation + +These issues indicate active development and real-world deployment of AprilTag systems in laboratory automation contexts, validating the practical approach taken in the UR Toolkit. + +### Package Ecosystem +The implementation uses standard computer vision and robotics libraries: +- **`pupil-apriltags`**: Primary AprilTag detection library +- **OpenCV**: Computer vision operations and camera calibration +- **NumPy**: Numerical operations and transformations +- **PyYAML**: Configuration file management +- **SciPy**: Advanced mathematical operations (in some modules) + +This choice of mature, well-maintained libraries ensures long-term stability and community support. \ No newline at end of file diff --git a/docs/APRILTAG_WORKFLOW.md b/docs/APRILTAG_WORKFLOW.md index c30a05a..dc7374a 100644 --- a/docs/APRILTAG_WORKFLOW.md +++ b/docs/APRILTAG_WORKFLOW.md @@ -167,7 +167,7 @@ python calculate_handeye_calibration.py --input handeye_data_YYYYMMDD_HHMMSS.jso ### AprilTag Configuration ```python apriltag_config = { - 'tag_family': 'tag36h11', # Standard family + 'tag_family': 'tagStandard41h12', # Recommended family 'tag_size_mm': 23.0, # Physical tag size 'calibration_file': 'camera_calibration/camera_calibration.yaml' } diff --git a/docs/APRILTAG_WORKFLOW_SIMPLE.md b/docs/APRILTAG_WORKFLOW_SIMPLE.md index ffd646d..7454148 100644 --- a/docs/APRILTAG_WORKFLOW_SIMPLE.md +++ b/docs/APRILTAG_WORKFLOW_SIMPLE.md @@ -65,7 +65,7 @@ import cv2 robot = URController('192.168.0.10') camera = PiCam(PiCamConfig.from_yaml('camera_client_config.yaml')) detector = AprilTagDetector( - tag_family='tag36h11', + tag_family='tagStandard41h12', tag_size=0.023, camera_calibration_file='camera_calibration/camera_calibration.yaml' ) diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index e9b5abf..197a428 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -4,8 +4,41 @@ All notable changes to this project will be documented in this file. ## [Unreleased] +### Added + - Enhanced empirical linear mapping calibration script (`scripts/calibrate_camera_to_robot.py`) with random sampling, residual/condition reporting, axis_mapping.json derivation and CLI flags (--samples, --random, --emit-axis-mapping) + - Added optional rotation-inclusive mode (--include-rotation, --rot-step) producing local 6×6 Jacobian mapping (translation+rotation) to `scripts/calibrate_camera_to_robot.py` + - Visual servo engine auto-loads `camera_to_robot_mapping.npy` (3×3 or 6×6) and applies empirical mapping for corrections when present + - Online refinement script (`scripts/refine_mapping_gradient_descent.py`) for incremental gradient-descent updates to mapping matrix M when AprilTag position drifts (translation-only) + - Config flag `visual_servo.ignore_hand_eye_calibration` to force pure axis / linear mapping mode even when a hand-eye file exists + - Automatic honoring of the above flag inside `VisualServoEngine` (hand-eye transform suppressed when flag true) + - Axis mapping override file `src/ur_toolkit/visual_servo/axis_mapping.json` enabling configurable camera→robot axis/sign remapping in fallback (non-calibrated) visual servo mode + - Demo script `scripts/demo_axis_mapping.py` to visualize effect of override vs default heuristic mapping + - Config flag already present: `visual_servo.use_axis_mapping_override` (set true by default) now exercised via the new mapping file + - Recalibration script `scripts/recalibrate_position_axis_mapping.py` to recompute robot pose for a taught AprilTag position using axis mapping or hand-eye calibration with optional apply/update actions + - Updated default AprilTag family from `tag36h11` to `tagStandard41h12` (recommended by AprilRobotics) + - Added `tagStandard41h12` to supported families in AprilTag detection and argument parser + - Updated all documentation and examples to use the new recommended family + - Backward compatibility maintained for existing tag families + - `HandEyeCalibrator` class with automated dataset collection and calibration solving + - Support for AprilTag-based calibration markers + - Automatic pose generation with safety validation + - Quality assessment with translation/rotation residuals + - Integrated with visual servoing engine for proper coordinate transformations + - `run_hand_eye_calibration.py` script for automated calibration workflow + - Comprehensive documentation in `HAND_EYE_CALIBRATION_GUIDE.md` + - `PoseCorrectionEngine` using OpenCV solvePnP for stable pose estimation instead of direct AprilTag pose + - `RobustPoseEstimator` with RANSAC outlier rejection for consistent measurements + - `PoseKalmanFilter` for pose smoothing to eliminate noise and oscillations + - Replaces problematic direct error negation with proper pose transformation mathematics + - Test script `test_pose_correction.py` for validation + - Example workflow `pose_correction_test.yaml` demonstrating new approach + - `linked_position_correction.yaml` workflow specification for observe/grasp operations + - `run_linked_position_workflow.py` executor script with comprehensive error handling + - 8-step workflow including validation, movement, pose correction, and linked position updates + - Integrated with corrected pose correction engine for reliable convergence + - Support for spatial relationship preservation between linked positions + ### Changed -- **🏗️ Major Project Restructure** - Migrated to `src/` layout for better packaging and development - Moved all source code to `src/ur_toolkit/` - Moved executable scripts to `scripts/` - Moved configuration files to `config/` @@ -14,118 +47,80 @@ All notable changes to this project will be documented in this file. - Updated all import paths to use `ur_toolkit.` prefix - Created proper `pyproject.toml` for modern Python packaging - Updated README.md with new structure and corrected paths -- **Pi Camera Server** - Kept as separate deployment component outside src/ structure + - Replaced direct error negation with proper hand-eye calibration transformations + - Added fallback coordinate mapping for systems without calibration + - Improved pose correction accuracy and stability -### Added -- **Comprehensive Code Cleanup** - Fixed all flake8 linting errors across the entire codebase (reduced from 336 to 0 errors) -- **File-by-File Code Cleanup** - Systematically cleaned up individual files including ur_controller.py, config_manager.py, and entire visual_servo/ folder (config.py, detection_filter.py, pose_history.py, visual_servo_engine.py) -- **Import Path Stability** - Added .env file for PYTHONPATH configuration and VS Code settings for consistent imports -- **Flake8 Configuration** - Created .flake8 config file to ignore style-only errors while maintaining functional code quality -- **File Organization** - Deleted all unnecessary debugging/test files and __pycache__ directories -- **Duplicate File Removal** - Removed obsolete ur_robot_interface.py duplicate of ur_controller.py -- **PID-Based Visual Servoing Stability** - Implemented conservative PID controllers for eye-in-hand visual servoing with automatic gain reduction -- **Stability Testing Framework** - Added test script to verify PID controller stability and error tracking behavior -- **Automatic Equipment Association** - All position teaching now automatically prompts for equipment association and observation pose setup -- **Enhanced Position Teaching Workflow** - Unified approach for both regular and freedrive position teaching with equipment linking -- **Improved Equipment Management** - Automatic offset calculation between work positions and observation poses for visual servoing -- **Clean YAML Structure** - Removed irrelevant fields (timestamps, freedrive flags) and AprilTag data from work positions -- **Dual-Purpose Positions** - Work positions can also serve as observation poses when they have AprilTag view and no other observation pose exists -- **Smart AprilTag Data Management** - Only observation poses store AprilTag data; work positions use observation poses for visual servoing -- **Intelligent Prompting** - When AprilTag is detected during position teaching, defaults to dual-purpose position with negative confirmation prompt -- **Comprehensive File Organization** - Moved CHANGELOG.md to documentation/, renamed config/ to setup/, moved requirements.txt and setup_venv.sh to setup/ -- **Enhanced Directory Organization** - Created `positions/` directory for position files and moved teach_positions.py there -- **Project Structure Organization** - Created `workflow/` directory for workflow-related files and `workflow/examples/` for YAML examples -- **Path Resolution Fix** - Updated import paths in workflow files to work from subdirectory structure -- **URController Refactor** - Renamed `URRobotInterface` to `URController` and file to `ur_controller.py` for cleaner, more intuitive naming -- **moveJ Action** - Added joint move (`moveJ`) alongside linear move (`moveL`) for different movement types -- **Condensed Workflow YAML** - Removed explicit step names, auto-generate descriptive names, replaced `move_to_position` with `moveL` -- **Step-by-Step Workflow Execution** - Interactive mode with user prompts before each step and automatic delay skipping -- **Improved YAML Formatting** - Inline arrays for coordinates/joints, rounded values, spacing between entries, removed usage tracking -- **Safe Position Reachability Testing** - Automatic movement test after creating safe offset positions to verify accessibility -- **Enhanced Interactive Menu** - Support for both number (1-8) and keyword inputs (teach, list, move, etc.) -- **Simplified Safe Offset System** - Automatic prompting for safe positions after teaching with simple direction/distance input -- **Remote Freedrive Teaching** - Simplified position teaching with blocking input (no timer/threading) -- **Streamlined Interactive Menu** - Removed legacy manual teaching option, focused on remote freedrive workflow -- **Robot Workflow System** - YAML-based workflow execution for sequential robot operations +### Fixed + - Added `_apply_coordinate_frame_correction()` method to PoseCorrectionEngine + - Implements 180° rotation around Y-axis to align tag coordinate frame with expected orientation + - Applied to both current pose measurements and target pose references + - Reduced pose correction errors from 200-300° to normal 10-25° range + - Achieved convergence in 2 iterations vs previous divergence issues + - Improved success rate from 0% to 20% with excellent stability + - Previously used `robot_correction = -tag_error` which assumes aligned coordinate frames + - Now uses proper hand-eye calibration matrix for coordinate transformations + - Addresses the "metre off" calibration issues mentioned in previous attempts + +### Added (Previous) + - Added `simple_mode` configuration option to enable XY-only corrections + - Reduced correction complexity to match stable legacy approach from archive/sdl6 + - Fixed proportional gain (0.4) for translation corrections, zero rotation corrections + - Applied to both direct and observation-based visual servoing methods + - Improved diagnostic output to clearly show applied corrections in simple mode + - **Achieved stable convergence** - System now converges reliably even when equipment is moved + +### Fixed + - Changed `update_stored_pose` default parameter to `False` + - Disabled equipment-wide position updates that were causing drift + - Added reset test workflow (`reset_test_no_servo.yaml`) to verify original position accuracy + - System now maintains consistent taught positions without algorithmic corrections + + - Added `transform_camera_to_robot_correction()` function for proper frame mapping + - Camera frame (X-right, Y-down, Z-forward) → Robot TCP frame (X-forward, Y-left, Z-up) + - Replaced simple negation mapping with proper coordinate transformation + - Robot now moves toward equipment instead of toward base when tag detected further away + - Eliminated rotation error explosions (6+ radians) through proper frame alignment + - Enabled proper IBVS with standard PID tuning instead of simplified approach ### Changed -- **Visual Servoing Stability** - Reduced PID gains (kp: 2.0→0.5), removed integral/derivative terms, added conservative output limits -- **Visual Servo Configuration** - Reduced max_iterations (5→3) and damping_factor (0.7→0.3) for enhanced stability -- **Error Tracking** - Added automatic gain reduction when error increases to prevent oscillation and overshoot ### Removed -- **Debugging Test Files** - Deleted test_freedrive_focused.py, test_freedrive_remote.py, test_remote_freedrive_teacher.py -- **Legacy Actions** - Removed `home` and `move_to_position` actions in favor of `moveL`/`moveJ` for cleaner, standardized API -- **URRobotInterface** - Replaced with shorter, more intuitive `URController` class name ### Removed -- **Usage Tracking** - Removed usage_count and last_used fields for cleaner position files -- **Workflow Actions** - Move to position, offset moves, gripper control, delays, position verification -- **Sample Workflows** - Pick and place demo, inspection routines, and simple test workflows -- **Workflow Runner** - Easy-to-use script for executing workflows with safety checks -- **Position Overwrite Confirmation** - Safety prompts when reteaching existing positions to prevent accidental data loss -- **Rich Position Metadata Collection** - Interactive prompts for position description, equipment, type, priority, and safety notes -- **Enhanced Position Teaching Workflow** - Automatic prompting to teach observation poses when no AprilTags detected -- **Observation Pose Auto-Teaching** - Streamlined workflow to teach observation poses immediately after grasp positions -- **Smart Position Linking** - Automatic linking of positions to newly created observation poses -- **Equipment Association** - Mandatory equipment linking for observation poses with validation -- **Manual Position Teaching** - Position teaching using teach pendant freedrive with RTDE pose reading (no remote control commands) -- **Two-Pose Teaching System** - Work poses and observation poses for positions with/without AprilTag visibility -- **Position Linking Commands** - CLI and interactive commands to link positions to observation poses (`link` command) -- **Clean YAML Format** - Human-readable position storage with essential data only (TCP pose, joint angles, AprilTag ID) -- **Read-Only Robot Interface** - Safe RTDE connection that only reads poses, no movement commands -- **Interactive Teaching Mode** - CLI interface for real-time position teaching and management -- **Robotiq Gripper Control** - Socket-based gripper control via URCap port 63352 following RTDE documentation -- Position teaching configuration in unified `config.yaml` -- Position verification using AprilTag visibility detection -- Position management (teach, observe, link, move, verify, list, delete) -- Comprehensive position teaching documentation in `documentation/POSITION_TEACHING.md` -- Enhanced position teaching workflow documentation in `documentation/ENHANCED_POSITION_TEACHING.md` -- Unified configuration system with central `config.yaml` file in project root -- `config_manager.py` module for centralized configuration access -- Configuration guide in `documentation/CONFIGURATION_GUIDE.md` -- Support for environment-based configuration overrides -- Path resolution utilities for robust file path handling -- Convenience functions for common configuration access patterns ### Fixed -- Fixed AprilTag detector initialization error with undefined tag family variable -- Fixed robot interface disconnect method call (should be `close()` not `disconnect()`) -- Improved error handling and validation in AprilTag detector initialization -- Added better diagnostics for AprilTag detector failures -- Fixed camera calibration file loading in AprilTag detector -- Resolved import errors for camera module (`picam`) in CLI and detection modules -- Updated `camera/picam/__init__.py` to properly expose `PiCam` and `PiCamConfig` classes -- Fixed import paths in `apriltag_detection.py` and `teach_positions.py` to use proper Python package structure -- Corrected sys.path management for cross-module imports ### Changed -- Updated `robots/ur/ur_robot_interface.py` to use centralized configuration -- Updated `apriltag_detection.py` to use centralized configuration -- All modules now import from `config_manager` instead of individual config files -- Configuration values now support dot notation access (e.g., `robot.ip_address`) -- Command-line arguments now override config file values consistently ### Improved -- Simplified configuration management - single source of truth -- Better path handling - no more broken references when moving files -- Consistent configuration access patterns across all modules -- Built-in defaults and graceful fallbacks -- Configuration validation and debugging capabilities ### Removed - handeye_calibration/ directory and all hand-eye calibration scripts: collect_handeye_data.py, calculate_handeye_calibration.py, coordinate_transformer.py, etc. - handeye_rework/ directory and experimental calibration approaches -- Hand-eye calibration related files from root: handeye_result.yaml, handeye_samples.json, analyze_motion.py, check_distances.py, test_close_distance.py -- Hand-eye calibration sections from README.md, simplified to focus on pure AprilTag detection workflow - Individual config files (`camera_client_config.yaml`, `robots/ur/robot_config.yaml`) - now consolidated -### Rationale -- Hand-eye calibration was not producing realistic results - AprilTag detection works effectively without requiring camera-to-robot transformation - Simplified codebase focuses on core functionality: camera capture, AprilTag detection, and robot control as separate components -- handeye_rework/collect_samples.py: implemented real robot/camera/apriltag hooks using existing interfaces -- Configuration consolidation eliminates path issues and simplifies system management + +## [2025-10-27] - Transition to Pure Empirical Mapping & Ignoring Hand-Eye Assets + +### Changed +- Updated `.gitignore` to ignore all hand-eye and checkerboard calibration related assets: + - `src/ur_toolkit/hand_eye_calibration/` directory and session artifacts + - Hand-eye / checkerboard scripts (`run_hand_eye_calibration.py`, manual/extended variants, analysis & diagnostic helpers) + - Captured checkerboard image folders and legacy calibration datasets + - Standalone analysis helpers (`analyze_calibration_accuracy.py`, `analyze_checkerboard_session.py`, etc.) +- Retained only empirical mapping artifacts: `camera_to_robot_mapping.npy` (3×3 or 6×6) and optional fallback `axis_mapping.json`. +- Enforced mapping-only mode via `ignore_hand_eye_calibration` flag for simplified workflow. + +### Added +- Documentation of repository cleanup aligning with simplified AprilTag → empirical mapping correction pipeline. + +### Notes +- 6×6 local Jacobian mapping remains accurate for small rotations; large single-step rotations (> ~25–30°) may produce suboptimal corrections due to linearization and AprilTag pose noise. +- Recommended approach: staged corrections (reduce translation first; apply incremental rotation) or collect additional calibration samples with deliberate rotational perturbations to improve rotational sub-block conditioning. +- Potential future enhancement: adaptive re-linearization (capture a fresh local neighborhood of delta poses when rotation error persists above threshold) to update the Jacobian online. ## [2025-09-12] - Camera Coordinate Frame Correction for Hand-Eye Calibration diff --git a/examples/workflows/debug_visual_servo.yaml b/examples/workflows/debug_visual_servo.yaml new file mode 100644 index 0000000..e31c61c --- /dev/null +++ b/examples/workflows/debug_visual_servo.yaml @@ -0,0 +1,38 @@ +# Simple Visual Servo Test - Limited iterations +# Basic test with only 2 iterations to debug + +workflow_info: + name: "Debug Visual Servo Test" + description: "Debug visual servoing with only 2 iterations" + version: "1.1" + author: "Robot System Tools" + +steps: + # Step 1: Move to home position + - action: movel + position: safe-home + speed: 0.2 + description: "Move to safe home position" + + # Step 2: Move to observation position + - action: movel + position: right-column-observe + speed: 0.1 + description: "Move to observation position above right column" + + # Step 3: Wait for settling + - action: delay + duration: 1.0 + description: "Wait for system to settle" + + # Step 4: Visual servo to grasp position (LIMITED ITERATIONS) + - action: visual_servo + position: grasp-B + max_iterations: 2 + description: "Visual servo to grasp-B with only 2 iterations" + + # Step 5: Return home + - action: movel + position: safe-home + speed: 0.2 + description: "Return to safe home position" \ No newline at end of file diff --git a/examples/workflows/linked_position_correction.yaml b/examples/workflows/linked_position_correction.yaml new file mode 100644 index 0000000..95103c7 --- /dev/null +++ b/examples/workflows/linked_position_correction.yaml @@ -0,0 +1,46 @@ +# Linked Position Correction Workflow +# This workflow demonstrates pose correction with linked positions: +# 1. safe-home: Safe starting/ending position +# 2. observe-right: Position to view AprilTag (will be corrected) +# 3. grasp-right: Position to interact with object (linked to observe-right) +# +# Scenario: Equipment/AprilTag has moved since positions were taught +# Robot corrects to realign with the original taught relationship to the AprilTag + +name: "Linked Position Correction" +description: "Correct to original taught pose relationship when equipment has moved" + +steps: + - {action: "moveL", position: "safe-home", speed: 0.05} + - {action: "delay", duration: 1.0} + + # Move to taught observe position + - {action: "moveL", position: "observe-right", speed: 0.03} + - {action: "delay", duration: 2.0} + + # Set target from stored taught relationship instead of current detection + # This uses the camera_to_tag relationship from when observe-right was taught + - {action: "pose_correction_set_target_taught", position_name: "observe-right"} + + # Perform pose correction using the original taught AprilTag relationship + # This will move the robot to maintain the same relative pose to the AprilTag + # as when the position was originally taught, regardless of where the equipment moved + - {action: "pose_correction", tag_id: 2} + + # Test the corrected linked position + # NOTE: In a full implementation, grasp-right would be automatically + # updated based on the correction applied to observe-right + - {action: "moveL", position: "grasp-right", speed: 0.02} + - {action: "delay", duration: 1.0} + + # Return home + - {action: "moveL", position: "safe-home", speed: 0.05} + + # Test the corrected linked position + # NOTE: In a full implementation, grasp-right would be automatically + # updated based on the correction applied to observe-right + - {action: "moveL", position: "grasp-right", speed: 0.02} + - {action: "delay", duration: 1.0} + + # Return home + - {action: "moveL", position: "safe-home", speed: 0.05} \ No newline at end of file diff --git a/examples/workflows/minimal_servo_test.yaml b/examples/workflows/minimal_servo_test.yaml new file mode 100644 index 0000000..8ad964b --- /dev/null +++ b/examples/workflows/minimal_servo_test.yaml @@ -0,0 +1,38 @@ +# Minimal Visual Servo Test +# Test visual servoing with simple mode but no position updates + +workflow_info: + name: "Minimal Visual Servo Test" + description: "Test visual servoing with simple mode, no position updates, 3 iterations max" + version: "1.0" + author: "Robot System Tools" + +steps: + # Step 1: Move to safe home + - action: movel + position: safe-home + speed: 0.2 + description: "Move to safe home position" + + # Step 2: Move to observation position + - action: movel + position: right-column-observe + speed: 0.1 + description: "Move to right column observation position" + + # Step 3: Wait briefly + - action: delay + duration: 1.0 + description: "Wait for settling" + + # Step 4: Visual servo with limited iterations + - action: visual_servo + position: grasp-B + max_iterations: 3 + description: "Visual servo to grasp-B with max 3 iterations (no position updates)" + + # Step 5: Return home + - action: movel + position: safe-home + speed: 0.2 + description: "Return to safe home position" \ No newline at end of file diff --git a/examples/workflows/pose_correction_test.yaml b/examples/workflows/pose_correction_test.yaml new file mode 100644 index 0000000..28d47e9 --- /dev/null +++ b/examples/workflows/pose_correction_test.yaml @@ -0,0 +1,79 @@ +# Pose Correction Test Workflow +# Tests the new robust pose correction system + +workflow_info: + name: "Pose Correction Test" + description: "Test robust visual servoing with filtered pose estimates" + version: "1.0" + author: "Robot System Tools" + +steps: + # Step 1: Move to safe home position + - action: movel + position: safe-home + speed: 0.2 + description: "Move to safe home position" + + # Step 2: Move to observation position + - action: movel + position: right-column-observe + speed: 0.1 + description: "Move to observation position above workspace" + + # Step 3: Wait for system to settle + - action: delay + duration: 1.5 + description: "Wait for system to settle" + + # Step 4: Set target pose from current AprilTag detection + - action: pose_correction_set_target + tag_id: 2 + description: "Set target pose from current AprilTag 2 detection" + + # Step 5: Move robot slightly off-target to test correction + - action: offset_move + offset: [0.01, 0.01, 0.005, 0, 0, 0.05] # 1cm XY, 5mm Z, small rotation + speed: 0.05 + coordinate_system: tcp + description: "Move slightly off-target to test correction" + + # Step 6: Wait briefly + - action: delay + duration: 1.0 + description: "Wait before starting correction" + + # Step 7: Perform robust pose correction + - action: pose_correction + tag_id: 2 + max_iterations: 8 + description: "Correct pose using robust visual servoing" + + # Step 8: Verify final position + - action: delay + duration: 2.0 + description: "Wait to verify final position" + + # Step 9: Return to safe home + - action: movel + position: safe-home + speed: 0.1 + description: "Return to safe home position" + +# Workflow configuration +config: + pose_correction: + max_iterations: 8 + position_tolerance: 0.003 # 3mm + rotation_tolerance: 0.087 # 5 degrees + max_correction_per_step: 0.015 # 1.5cm + step_size: 0.6 # 60% of calculated correction + + safety: + max_tcp_speed: 0.1 + workspace_limits: + x_min: -0.3 + x_max: 0.3 + y_min: -0.3 + y_max: 0.3 + z_min: 0.1 + z_max: 0.4 \ No newline at end of file diff --git a/examples/workflows/reset_test_no_servo.yaml b/examples/workflows/reset_test_no_servo.yaml new file mode 100644 index 0000000..70b7bb1 --- /dev/null +++ b/examples/workflows/reset_test_no_servo.yaml @@ -0,0 +1,49 @@ +# Reset Test - No Visual Servoing +# Simple movement workflow to reset and test original taught positions + +workflow_info: + name: "Reset Test - No Servoing" + description: "Test original taught positions without visual servoing to reset system" + version: "1.0" + author: "Robot System Tools" + +steps: + # Step 1: Move to safe home + - action: movel + position: safe-home + speed: 0.2 + description: "Move to safe home position" + + # Step 2: Move to observation position + - action: movel + position: right-column-observe + speed: 0.1 + description: "Move to right column observation position" + + # Step 3: Wait briefly + - action: delay + duration: 2.0 + description: "Wait and observe" + + # Step 4: Move directly to grasp position (NO VISUAL SERVO) + - action: movel + position: grasp-B + speed: 0.05 + description: "Move directly to grasp-B position using original taught coordinates" + + # Step 5: Wait at grasp position + - action: delay + duration: 2.0 + description: "Wait at grasp position to observe accuracy" + + # Step 6: Return to safe position above grasp + - action: movel + position: grasp-B-safe + speed: 0.1 + description: "Move to safe position above grasp" + + # Step 7: Return home + - action: movel + position: safe-home + speed: 0.2 + description: "Return to safe home position" \ No newline at end of file diff --git a/examples/workflows/right_column_cycle.yaml b/examples/workflows/right_column_cycle.yaml new file mode 100644 index 0000000..b80e315 --- /dev/null +++ b/examples/workflows/right_column_cycle.yaml @@ -0,0 +1,66 @@ +name: "Right Column Cycle" +description: "Right-column grasp cycle (no inline visual servo; external correction applied beforehand)" + +# Steps: +# 1) safe-home +# 2) gripper activate +# 3) right-column-observe +# 4) grasp-B-safe +# 5) grasp-B +# 6) gripper close +# 7) grasp-B-safe +# 8) grasp-B +# 8b) gripper open +# 9) grasp-B-safe +# 10) safe-home +# 0.5s delay between each movement or gripper action + +steps: + - {action: "moveL", position: "safe-home"} + - {action: "delay", duration: 0.5} + - {action: "gripper_activate"} + - {action: "delay", duration: 0.5} + - {action: "gripper_open"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "right-column-observe"} + - {action: "delay", duration: 0.5} + + # Optional visual servo (controlled by visual_servo.enabled in config). If disabled, treated as no-op. + - {action: "visual_servo", position: "right-column-observe"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "grasp-right-safe"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "grasp-right"} + - {action: "delay", duration: 0.5} + + - {action: "gripper_close"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "grasp-right-safe"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "grasp-right"} + - {action: "delay", duration: 0.5} + + - {action: "gripper_open"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "grasp-right-safe"} + - {action: "delay", duration: 0.5} + + - {action: "moveL", position: "safe-home"} + - {action: "delay", duration: 0.5} + +validation: + required_positions: + - safe-home + - right-column-observe + - grasp-right + - grasp-right-safe + safety_checks: + - robot_remote_mode + - gripper_operational + - emergency_stop_clear \ No newline at end of file diff --git a/examples/workflows/simple_visual_servo_test.yaml b/examples/workflows/simple_visual_servo_test.yaml index a839fd1..fc1387e 100644 --- a/examples/workflows/simple_visual_servo_test.yaml +++ b/examples/workflows/simple_visual_servo_test.yaml @@ -3,45 +3,38 @@ workflow_info: name: "Simple Visual Servo Test" - description: "Basic test of visual servoing with AprilTag correction" - version: "1.0" + description: "Basic test of visual servoing: home -> observe -> visual servo to grasp -> home" + version: "1.1" author: "Robot System Tools" steps: # Step 1: Move to home position - action: movel - position: home + position: safe-home speed: 0.2 description: "Move to safe home position" - # Step 2: Regular move to a position (no visual servo) + # Step 2: Move to observation position - action: movel - position: test_position + position: right-column-observe speed: 0.1 - description: "Move to test position using stored coordinates" + description: "Move to observation position for right column" # Step 3: Wait briefly - action: delay duration: 1.0 description: "Wait for robot to settle" - # Step 4: Visual servo to the same position (should correct for any drift) + # Step 4: Visual servo to grasp position - action: visual_servo - position: test_position + position: grasp-B speed: 0.05 type: movel - description: "Visual servo to same position with AprilTag correction" + description: "Visual servo to grasp-B with AprilTag correction" - # Step 5: Test regular move with visual servo flag + # Step 5: Return to home - action: movel - position: test_position_2 - speed: 0.1 - visual_servo: true - description: "Move to second position with inline visual servo correction" - - # Step 6: Return to home - - action: movel - position: home + position: safe-home speed: 0.2 description: "Return to home position" diff --git a/fix_taught_positions_ambiguity.py b/fix_taught_positions_ambiguity.py new file mode 100644 index 0000000..3ac2ffc --- /dev/null +++ b/fix_taught_positions_ambiguity.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +""" +Fix 180° Ambiguity in Taught Positions +Corrects AprilTag pose ambiguity in camera_to_tag values and observation offsets +""" + +import yaml +import numpy as np +from pathlib import Path + + +def correct_apriltag_180_ambiguity(pose_vector): + """Correct 180° rotation ambiguity in AprilTag pose estimation. + + AprilTags can have 180° ambiguity around Y-axis due to coordinate frame conventions. + This function detects and corrects poses that are ~180° off from expected orientation. + + Args: + pose_vector: [x, y, z, rx, ry, rz] pose vector + + Returns: + Corrected pose vector + """ + corrected_pose = pose_vector.copy() + + # Check if Y-rotation is close to ±180° (π radians) + ry = pose_vector[4] # Y-rotation component + + # If Y-rotation magnitude is > 90° (π/2), likely 180° ambiguity + if abs(ry) > np.pi / 2: + # Correct by flipping around Y-axis + if ry > 0: + corrected_pose[4] = ry - np.pi # Subtract 180° + else: + corrected_pose[4] = ry + np.pi # Add 180° + + # Also flip X and Z rotations to maintain consistency + corrected_pose[3] = -corrected_pose[3] # Flip RX + corrected_pose[5] = -corrected_pose[5] # Flip RZ + + print(f"🔄 Corrected 180° ambiguity: Y-rot {np.degrees(ry):.1f}° → {np.degrees(corrected_pose[4]):.1f}°") + + return corrected_pose + + +def fix_taught_positions_ambiguity(input_file: str, output_file: str): + """ + Fix 180° ambiguity in taught positions file + + Args: + input_file: Path to original taught positions YAML file + output_file: Path to save corrected taught positions YAML file + """ + print(f"🔧 Loading taught positions from: {input_file}") + + # Load the YAML file + with open(input_file, 'r') as f: + data = yaml.safe_load(f) + + corrections_made = 0 + positions_processed = 0 + + print("\n📊 Analyzing positions for 180° ambiguity...") + + # Process each position + for position_name, position_data in data.get('positions', {}).items(): + positions_processed += 1 + position_corrected = False + + # Check camera_to_tag poses + if 'camera_to_tag' in position_data and position_data['camera_to_tag']: + original_pose = np.array(position_data['camera_to_tag']) + ry_original = original_pose[4] + + # Check if Y-rotation has 180° ambiguity + if abs(ry_original) > np.pi / 2: + print(f"\n🎯 Position '{position_name}':") + print(f" Original camera_to_tag Y-rotation: {np.degrees(ry_original):.1f}°") + + corrected_pose = correct_apriltag_180_ambiguity(original_pose) + position_data['camera_to_tag'] = corrected_pose.tolist() + + corrections_made += 1 + position_corrected = True + + print(f" ✅ Corrected camera_to_tag Y-rotation: {np.degrees(corrected_pose[4]):.1f}°") + + # Check observation_offset poses (if they exist) + if 'observation_offset' in position_data and position_data['observation_offset']: + offset = np.array(position_data['observation_offset']) + if len(offset) >= 6: # Has rotation components + ry_offset = offset[4] + + # Check if Y-rotation offset has large values that might indicate ambiguity + if abs(ry_offset) > np.pi: # Larger threshold for offsets + if not position_corrected: + print(f"\n🎯 Position '{position_name}':") + + print(f" Original observation_offset Y-rotation: {np.degrees(ry_offset):.1f}°") + + corrected_offset = correct_apriltag_180_ambiguity(offset) + position_data['observation_offset'] = corrected_offset.tolist() + + if not position_corrected: + corrections_made += 1 + + print(f" ✅ Corrected observation_offset Y-rotation: {np.degrees(corrected_offset[4]):.1f}°") + + print(f"\n📈 Summary:") + print(f" Positions processed: {positions_processed}") + print(f" Positions with 180° ambiguity corrected: {corrections_made}") + + # Save corrected file + print(f"\n💾 Saving corrected positions to: {output_file}") + with open(output_file, 'w') as f: + yaml.dump(data, f, default_flow_style=False, sort_keys=False, indent=2) + + print("✅ 180° ambiguity correction completed!") + print(f"📁 Compare files:") + print(f" Original: {input_file}") + print(f" Corrected: {output_file}") + + +def main(): + """Main function to fix taught positions ambiguity""" + input_file = "src/ur_toolkit/positions/taught_positions.yaml" + output_file = "src/ur_toolkit/positions/taught_positions_corrected.yaml" + + # Convert to absolute paths + input_path = Path(input_file).resolve() + output_path = Path(output_file).resolve() + + if not input_path.exists(): + print(f"❌ Input file not found: {input_path}") + return + + print("🎯 180° Ambiguity Correction Tool for Taught Positions") + print("=" * 60) + + fix_taught_positions_ambiguity(str(input_path), str(output_path)) + + print(f"\n🔍 Next steps:") + print(f"1. Review the differences between the files") + print(f"2. If satisfied, replace the original file:") + print(f" cp '{output_file}' '{input_file}'") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/logs/visual_servo_last.log b/logs/visual_servo_last.log new file mode 100644 index 0000000..5b8b2aa --- /dev/null +++ b/logs/visual_servo_last.log @@ -0,0 +1,104 @@ +2025-10-21 17:33:24,932 INFO: Started visual servo script +2025-10-21 17:33:24,932 INFO: Loaded mapping: C:\Users\kthch\Documents\github\ur_toolkit\camera_to_robot_mapping.npy +2025-10-21 17:33:24,934 INFO: Control params: gain=0.06, max_speed=0.006, tolerance=0.005, max_iter=40 +2025-10-21 17:33:24,935 INFO: Initial pose: [-0.13497233 -0.58800792 0.26901487 0.00606732 -2.2529827 2.13905627] +2025-10-21 17:33:24,935 INFO: Moving to pose-B-observe to simulate unknown start (slow) +2025-10-21 17:33:24,935 DEBUG: pose-B-observe: [0.025, -0.422, 0.353, -0.529, -2.253, 1.63] +2025-10-21 17:34:14,589 INFO: moveL to B-observe returned: True +2025-10-21 17:34:15,090 INFO: Iteration 1/40 +2025-10-21 17:34:15,699 INFO: Tag distance (mm): 339.024 +2025-10-21 17:34:15,699 INFO: Velocity command (robot TCP mm/s): [-2.66755065 -4.88850794 -2.23308389] +2025-10-21 17:34:15,699 INFO: speedL returned: True +2025-10-21 17:34:16,208 INFO: Iteration 2/40 +2025-10-21 17:34:16,675 INFO: Tag distance (mm): 335.305 +2025-10-21 17:34:16,691 INFO: Velocity command (robot TCP mm/s): [-2.66719971 -4.88957421 -2.23116781] +2025-10-21 17:34:16,691 INFO: speedL returned: True +2025-10-21 17:34:17,191 INFO: Iteration 3/40 +2025-10-21 17:34:17,650 INFO: Tag distance (mm): 329.481 +2025-10-21 17:34:17,650 INFO: Velocity command (robot TCP mm/s): [-2.66796961 -4.89062112 -2.22795045] +2025-10-21 17:34:17,650 INFO: speedL returned: True +2025-10-21 17:34:18,150 INFO: Iteration 4/40 +2025-10-21 17:34:18,606 INFO: Tag distance (mm): 323.039 +2025-10-21 17:34:18,606 INFO: Velocity command (robot TCP mm/s): [-2.66771073 -4.89234804 -2.22446625] +2025-10-21 17:34:18,606 INFO: speedL returned: True +2025-10-21 17:34:19,107 INFO: Iteration 5/40 +2025-10-21 17:34:19,569 INFO: Tag distance (mm): 317.553 +2025-10-21 17:34:19,569 INFO: Velocity command (robot TCP mm/s): [-2.66630826 -4.89411937 -2.22225017] +2025-10-21 17:34:19,569 INFO: speedL returned: True +2025-10-21 17:34:20,070 INFO: Iteration 6/40 +2025-10-21 17:34:20,611 INFO: Tag distance (mm): 311.594 +2025-10-21 17:34:20,611 INFO: Velocity command (robot TCP mm/s): [-2.66668815 -4.89525731 -2.21928596] +2025-10-21 17:34:20,611 INFO: speedL returned: True +2025-10-21 17:34:21,115 INFO: Iteration 7/40 +2025-10-21 17:34:21,649 INFO: Tag distance (mm): 305.195 +2025-10-21 17:34:21,649 INFO: Velocity command (robot TCP mm/s): [-2.66847613 -4.89614156 -2.21518237] +2025-10-21 17:34:21,649 INFO: speedL returned: True +2025-10-21 17:34:22,154 INFO: Iteration 8/40 +2025-10-21 17:34:22,606 INFO: Tag distance (mm): 299.107 +2025-10-21 17:34:22,606 INFO: Velocity command (robot TCP mm/s): [-2.66771187 -4.89814503 -2.21167101] +2025-10-21 17:34:22,606 INFO: speedL returned: True +2025-10-21 17:34:23,109 INFO: Iteration 9/40 +2025-10-21 17:34:23,539 INFO: Tag distance (mm): 292.591 +2025-10-21 17:34:23,539 INFO: Velocity command (robot TCP mm/s): [-2.66806586 -4.89896692 -2.20942248] +2025-10-21 17:34:23,539 INFO: speedL returned: True +2025-10-21 17:34:24,041 INFO: Iteration 10/40 +2025-10-21 17:34:24,498 INFO: Tag distance (mm): 287.190 +2025-10-21 17:34:24,498 INFO: Velocity command (robot TCP mm/s): [-2.66887823 -4.90030697 -2.20546607] +2025-10-21 17:34:24,498 INFO: speedL returned: True +2025-10-21 17:34:24,999 INFO: Iteration 11/40 +2025-10-21 17:34:25,449 INFO: Tag distance (mm): 281.572 +2025-10-21 17:34:25,449 INFO: Velocity command (robot TCP mm/s): [-2.66844618 -4.90151785 -2.203297 ] +2025-10-21 17:34:25,449 INFO: speedL returned: True +2025-10-21 17:34:25,952 INFO: Iteration 12/40 +2025-10-21 17:34:26,425 INFO: Tag distance (mm): 276.145 +2025-10-21 17:34:26,425 INFO: Velocity command (robot TCP mm/s): [-2.66839813 -4.90289978 -2.20027842] +2025-10-21 17:34:26,425 INFO: speedL returned: True +2025-10-21 17:34:26,929 INFO: Iteration 13/40 +2025-10-21 17:34:27,429 INFO: Tag distance (mm): 269.608 +2025-10-21 17:34:27,429 INFO: Velocity command (robot TCP mm/s): [-2.66840971 -4.90426965 -2.19720931] +2025-10-21 17:34:27,429 INFO: speedL returned: True +2025-10-21 17:34:27,931 INFO: Iteration 14/40 +2025-10-21 17:34:28,372 INFO: Tag distance (mm): 263.643 +2025-10-21 17:34:28,375 INFO: Velocity command (robot TCP mm/s): [-2.66875277 -4.90525879 -2.19458308] +2025-10-21 17:34:28,375 INFO: speedL returned: True +2025-10-21 17:34:28,875 INFO: Iteration 15/40 +2025-10-21 17:34:29,346 INFO: Tag distance (mm): 258.493 +2025-10-21 17:34:29,346 INFO: Velocity command (robot TCP mm/s): [-2.66928687 -4.90674759 -2.19060169] +2025-10-21 17:34:29,346 INFO: speedL returned: True +2025-10-21 17:34:29,848 INFO: Iteration 16/40 +2025-10-21 17:34:30,317 INFO: Tag distance (mm): 252.224 +2025-10-21 17:34:30,317 INFO: Velocity command (robot TCP mm/s): [-2.66996393 -4.90779937 -2.18741811] +2025-10-21 17:34:30,317 INFO: speedL returned: True +2025-10-21 17:34:30,821 INFO: Iteration 17/40 +2025-10-21 17:34:31,265 INFO: Tag distance (mm): 246.282 +2025-10-21 17:34:31,265 INFO: Velocity command (robot TCP mm/s): [-2.67052406 -4.90945727 -2.18300951] +2025-10-21 17:34:31,265 INFO: speedL returned: True +2025-10-21 17:34:31,767 INFO: Iteration 18/40 +2025-10-21 17:34:32,213 INFO: Tag distance (mm): 240.791 +2025-10-21 17:34:32,213 INFO: Velocity command (robot TCP mm/s): [-2.67178374 -4.91013899 -2.17993275] +2025-10-21 17:34:32,213 INFO: speedL returned: True +2025-10-21 17:34:32,715 INFO: Iteration 19/40 +2025-10-21 17:34:33,276 INFO: Tag distance (mm): 234.604 +2025-10-21 17:34:33,276 INFO: Velocity command (robot TCP mm/s): [-2.67320023 -4.91113023 -2.17595964] +2025-10-21 17:34:33,276 INFO: speedL returned: True +2025-10-21 17:34:33,781 INFO: Iteration 20/40 +2025-10-21 17:34:34,236 INFO: Tag distance (mm): 228.409 +2025-10-21 17:34:34,236 INFO: Velocity command (robot TCP mm/s): [-2.67237473 -4.91295763 -2.17284622] +2025-10-21 17:34:34,236 INFO: speedL returned: True +2025-10-21 17:34:34,739 INFO: Iteration 21/40 +2025-10-21 17:34:35,174 INFO: Tag distance (mm): 222.882 +2025-10-21 17:34:35,180 INFO: Velocity command (robot TCP mm/s): [-2.67297206 -4.91404521 -2.16964974] +2025-10-21 17:34:35,180 INFO: speedL returned: True +2025-10-21 17:34:35,680 INFO: Iteration 22/40 +2025-10-21 17:34:36,129 INFO: Tag distance (mm): 218.811 +2025-10-21 17:34:36,134 INFO: Velocity command (robot TCP mm/s): [-2.67246045 -4.91538271 -2.16724893] +2025-10-21 17:34:36,134 INFO: speedL returned: False +2025-10-21 17:34:36,134 WARNING: speedL failed or returned False; stopping visual servo loop +2025-10-21 17:34:36,335 INFO: Final pose (before final move): [-0.028275 -0.51980338 0.30898827 -0.52896156 -2.25304251 1.62993155] +2025-10-21 17:34:36,335 INFO: Total displacement (mm): 132.7932665819281 +2025-10-21 17:34:36,335 INFO: Attempting final moveL to pose-A-observe (very slow) +2025-10-21 17:34:36,335 INFO: moveL to A-observe returned: False +2025-10-21 17:34:36,335 WARNING: Final move to pose-A-observe failed; attempting reconnect and retry +2025-10-21 17:35:04,833 INFO: retry moveL to A-observe returned: True +2025-10-21 17:35:04,833 INFO: Final move to pose-A-observe succeeded after reconnect +2025-10-21 17:35:05,852 INFO: Disconnected from robot diff --git a/moveL_6dof_convergence.gif b/moveL_6dof_convergence.gif new file mode 100644 index 0000000..6ba6597 Binary files /dev/null and b/moveL_6dof_convergence.gif differ diff --git a/moveL_visual_servo_6dof.py b/moveL_visual_servo_6dof.py new file mode 100644 index 0000000..19a5f0d --- /dev/null +++ b/moveL_visual_servo_6dof.py @@ -0,0 +1,293 @@ +#!/usr/bin/env python3 +""" +6-DOF Visual Servoing using moveL with Direct Error Correction. + +This implementation uses the detected AprilTag pose error directly: +1. Detect AprilTag and compute full 6-DOF pose error vector [x, y, z, roll, pitch, yaw] +2. Apply ONE moveL command with scaled error as correction (corrects all 6 DOF simultaneously) +3. Use momentum damping for smooth convergence +4. Repeat until convergence + +Key insight: The AprilTag detector already gives us the full 6-DOF error. We don't need +to probe in multiple directions - just directly use the error vector and move to reduce it. +All 6 dimensions are corrected together in each moveL command for smooth, direct convergence. + +Uses UR robot's robust inverse kinematics with simple moveL commands. +No complex Jacobians, velocity control, or gradient probing required. +""" + +import numpy as np +from scipy.spatial.transform import Rotation +import matplotlib.pyplot as plt +from matplotlib.animation import PillowWriter + +class SimpleURSimulator: + """Lightweight UR robot simulator for testing moveL commands""" + + def __init__(self): + self.tcp_pose = np.array([0.0, -0.4, 0.3, 0.1, 0.2, -0.15]) # [x, y, z, rx, ry, rz] in radians + + def moveL(self, target_pose): + """Simulate moveL command - moves TCP to target pose""" + self.tcp_pose = np.array(target_pose) + return True + + def get_tcp_pose(self): + """Get current TCP pose [x, y, z, rx, ry, rz]""" + return self.tcp_pose.copy() + + def detect_apriltag(self, target_pose_world): + """ + Simulate AprilTag detection from camera. + Returns pose error between current and target. + """ + # Compute 6-DOF error + pos_error = target_pose_world[:3] - self.tcp_pose[:3] + + # Rotation error using axis-angle representation + current_rot = Rotation.from_rotvec(self.tcp_pose[3:]) + target_rot = Rotation.from_rotvec(target_pose_world[3:]) + rot_error_matrix = target_rot * current_rot.inv() + rot_error = rot_error_matrix.as_rotvec() + + return np.concatenate([pos_error, rot_error]) + + +def visual_servo_gradient_descent(): + """ + Run visual servoing with 6-DOF gradient descent using moveL. + + At each iteration: + 1. Detect AprilTag and get 6-DOF pose error (all components at once) + 2. Apply one moveL command with a scaled version of the error as correction + + This corrects position and orientation simultaneously in each move based on + the detected AprilTag pose error. + """ + # Initialize robot simulator + robot = SimpleURSimulator() + + # Target pose (where we want the robot to go) + target_pose = np.array([0.1, -0.3, 0.35, 0.0, 0.0, 0.0]) + + # Control parameters + max_iterations = 50 + first_move_gain = 1.0 # First move: go directly to target (100% correction) + subsequent_gain = 0.2 # Conservative for fine-tuning (20% of remaining error) + momentum = 0.5 # Smoothing factor to reduce oscillation + convergence_threshold = 0.001 # 1mm position error + + # Storage for visualization and momentum + poses_history = [] + errors_history = [] + velocity = np.zeros(6) # Accumulated velocity for momentum + + print("Starting 6-DOF Visual Servoing with Combined Error Correction") + print(f"Initial pose: {robot.get_tcp_pose()}") + print(f"Target pose: {target_pose}") + + current_pose = robot.get_tcp_pose() + error_vec = robot.detect_apriltag(target_pose) + initial_error = np.linalg.norm(error_vec) + print(f"Initial error: {initial_error:.4f} m\n") + + for iteration in range(max_iterations): + # Get current pose and detect full 6-DOF error vector + current_pose = robot.get_tcp_pose() + error_vec = robot.detect_apriltag(target_pose) + current_error = np.linalg.norm(error_vec) + + # Store for visualization + poses_history.append(current_pose.copy()) + errors_history.append(current_error) + + print(f"Iteration {iteration:2d}: Error = {current_error:.6f} m, " + f"Error vector = [{error_vec[0]:.3f}, {error_vec[1]:.3f}, {error_vec[2]:.3f}, " + f"{error_vec[3]:.3f}, {error_vec[4]:.3f}, {error_vec[5]:.3f}]") + + # Check convergence + if current_error < convergence_threshold: + print(f"\n✓ Converged after {iteration} iterations!") + break + + # The correction is proportional to the error vector + # The error vector points FROM current TO target, so we move IN that direction + correction = error_vec # Positive - move in direction of error to reduce it + + # Select gain based on iteration (first move is aggressive, rest are conservative) + current_gain = first_move_gain if iteration == 0 else subsequent_gain + + # Apply momentum for smoother convergence + velocity = momentum * velocity + (1 - momentum) * correction + + # Compute new pose by applying combined correction to ALL 6 DOF at once + new_pose = current_pose + current_gain * velocity + + # Execute single moveL command with combined 6-DOF correction + robot.moveL(new_pose) + + # Final measurements + final_pose = robot.get_tcp_pose() + final_error_vec = robot.detect_apriltag(target_pose) + final_error = np.linalg.norm(final_error_vec) + + poses_history.append(final_pose.copy()) + errors_history.append(final_error) + + print(f"\nFinal pose: {final_pose}") + print(f"Final error: {final_error:.6f} m") + + return np.array(poses_history), np.array(errors_history), target_pose + + +def create_visualization(poses_history, errors_history, target_pose): + """Create animated visualization of convergence process""" + + fig = plt.figure(figsize=(14, 5)) + + # 3D trajectory plot + ax1 = fig.add_subplot(131, projection='3d') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('6-DOF Visual Servoing Trajectory\n(moveL + Combined Gradient Descent)') + + # Plot full trajectory + ax1.plot(poses_history[:, 0], poses_history[:, 1], poses_history[:, 2], + 'b-', alpha=0.3, linewidth=2, label='Path') + ax1.scatter(poses_history[0, 0], poses_history[0, 1], poses_history[0, 2], + c='green', s=100, marker='o', label='Start') + ax1.scatter(target_pose[0], target_pose[1], target_pose[2], + c='red', s=100, marker='*', label='Target') + ax1.legend() + ax1.grid(True, alpha=0.3) + + # Error convergence plot + ax2 = fig.add_subplot(132) + ax2.plot(errors_history, 'b-', linewidth=2) + ax2.set_xlabel('Iteration') + ax2.set_ylabel('Error (m)') + ax2.set_title('Convergence of 6-DOF Error') + ax2.grid(True, alpha=0.3) + ax2.set_yscale('log') + + # Pose components over time + ax3 = fig.add_subplot(133) + iterations = np.arange(len(poses_history)) + ax3.plot(iterations, poses_history[:, 0], 'r-', label='X', alpha=0.7) + ax3.plot(iterations, poses_history[:, 1], 'g-', label='Y', alpha=0.7) + ax3.plot(iterations, poses_history[:, 2], 'b-', label='Z', alpha=0.7) + ax3.axhline(target_pose[0], color='r', linestyle='--', alpha=0.3) + ax3.axhline(target_pose[1], color='g', linestyle='--', alpha=0.3) + ax3.axhline(target_pose[2], color='b', linestyle='--', alpha=0.3) + ax3.set_xlabel('Iteration') + ax3.set_ylabel('Position (m)') + ax3.set_title('Position Convergence (XYZ)') + ax3.legend() + ax3.grid(True, alpha=0.3) + + plt.tight_layout() + + return fig + + +def create_animation_gif(poses_history, errors_history, target_pose, filename='moveL_6dof_convergence.gif'): + """Create animated GIF showing convergence process""" + + fig = plt.figure(figsize=(12, 4)) + + # 3D trajectory + ax1 = fig.add_subplot(131, projection='3d') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('Trajectory') + + # Error plot + ax2 = fig.add_subplot(132) + ax2.set_xlabel('Iteration') + ax2.set_ylabel('Error (m)') + ax2.set_title('Error Convergence') + ax2.set_yscale('log') + ax2.grid(True, alpha=0.3) + + # RPY plot + ax3 = fig.add_subplot(133) + ax3.set_xlabel('Iteration') + ax3.set_ylabel('Angle (rad)') + ax3.set_title('Orientation (RPY)') + ax3.grid(True, alpha=0.3) + + # Plot target + ax1.scatter(target_pose[0], target_pose[1], target_pose[2], + c='red', s=100, marker='*', label='Target', zorder=100) + + def update(frame): + # Clear previous frame data + for ax in [ax1, ax2, ax3]: + for artist in ax.lines + ax.collections: + if hasattr(artist, 'remove'): + artist.remove() + + # Replot target + ax1.scatter(target_pose[0], target_pose[1], target_pose[2], + c='red', s=100, marker='*', label='Target', zorder=100) + + # Plot trajectory up to current frame + ax1.plot(poses_history[:frame+1, 0], poses_history[:frame+1, 1], + poses_history[:frame+1, 2], 'b-', alpha=0.5, linewidth=2) + ax1.scatter(poses_history[frame, 0], poses_history[frame, 1], + poses_history[frame, 2], c='blue', s=80, marker='o', zorder=50) + ax1.scatter(poses_history[0, 0], poses_history[0, 1], poses_history[0, 2], + c='green', s=80, marker='o', label='Start') + + # Error convergence + ax2.plot(errors_history[:frame+1], 'b-', linewidth=2) + ax2.scatter(frame, errors_history[frame], c='blue', s=50, zorder=50) + + # Orientation convergence + iterations = np.arange(frame+1) + ax3.plot(iterations, poses_history[:frame+1, 3], 'r-', label='Roll', alpha=0.7) + ax3.plot(iterations, poses_history[:frame+1, 4], 'g-', label='Pitch', alpha=0.7) + ax3.plot(iterations, poses_history[:frame+1, 5], 'b-', label='Yaw', alpha=0.7) + if frame == 0: + ax3.legend() + + # Update title with current error + fig.suptitle(f'6-DOF Visual Servoing (moveL + Direct Error Correction)\n' + f'Iteration {frame}/{len(poses_history)-1} | ' + f'Error: {errors_history[frame]:.6f} m', + fontsize=12) + + return [] + + # Create animation + frames = len(poses_history) + # Sample frames for reasonable file size + frame_indices = list(range(0, frames, max(1, frames//30))) + [frames-1] + + from matplotlib.animation import FuncAnimation + anim = FuncAnimation(fig, update, frames=frame_indices, + interval=200, blit=False, repeat=True) + + # Save as GIF + writer = PillowWriter(fps=5) + anim.save(filename, writer=writer) + print(f"\n✓ Animation saved to {filename}") + + plt.close() + + +if __name__ == "__main__": + # Run simulation + poses_history, errors_history, target_pose = visual_servo_gradient_descent() + + # Create visualization + fig = create_visualization(poses_history, errors_history, target_pose) + plt.savefig('moveL_6dof_convergence.png', dpi=150, bbox_inches='tight') + print("✓ Static visualization saved to moveL_6dof_convergence.png") + + # Create animated GIF + create_animation_gif(poses_history, errors_history, target_pose) + + plt.show() diff --git a/pyproject.toml b/pyproject.toml index 74eab79..66b6f7b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -38,6 +38,9 @@ dev = [ "pytest-cov", "flake8", "black", + "matplotlib", + "imageio", + "pupil-apriltags", ] [project.scripts] @@ -53,4 +56,4 @@ Documentation = "https://github.com/AccelerationConsortium/ur_toolkit/tree/main/ where = ["src"] [tool.setuptools.package-data] -ur_toolkit = ["*.yaml", "*.yml"] \ No newline at end of file +ur_toolkit = ["*.yaml", "*.yml"] diff --git a/requirements.txt b/requirements.txt index d6e1198..aefbcb6 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1 @@ --e . +-e .[dev] diff --git a/scripts/calibrate_camera_to_robot.py b/scripts/calibrate_camera_to_robot.py new file mode 100644 index 0000000..3a87303 --- /dev/null +++ b/scripts/calibrate_camera_to_robot.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 +""" +Simple empirical calibration: map camera-frame observed deltas (from AprilTag) to robot TCP deltas. + +Creates a 3x3 linear mapping M where robot_delta ≈ M @ camera_delta + +Usage: python scripts/calibrate_camera_to_robot.py +""" + +import sys +import time +from pathlib import Path + +import numpy as np +import cv2 + +# Add src to path for imports +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +import rtde_control +import rtde_receive +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.config_manager import ( + get_camera_host, + get_camera_port, + get_camera_calibration_file, + get_apriltag_family, + get_apriltag_size, + config, +) + + +def capture_first_tvec(camera, detector, attempts=3, delay=0.5): + for _ in range(attempts): + photo = camera.capture_photo() + if not photo: + time.sleep(delay) + continue + img = cv2.imread(photo) + dets = detector.detect_tags(img) + if dets and dets[0]["pose"]: + return np.array(dets[0]["pose"]["translation_vector"]) + time.sleep(delay) + return None + + +def main(): + robot_ip = config.get("robot.ip_address", "192.168.0.10") + print("Connecting to robot at", robot_ip) + rtde_c = rtde_control.RTDEControlInterface(robot_ip) + rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) + + speed = 0.03 + accel = 0.08 + + cam_cfg = PiCamConfig(hostname=get_camera_host(), port=get_camera_port()) + camera = PiCam(cam_cfg) + + if not camera.test_connection(): + print("Cannot connect to camera server") + return + + detector = AprilTagDetector( + tag_family=get_apriltag_family(), + tag_size=get_apriltag_size(), + calibration_file=get_camera_calibration_file(), + ) + + print("Collecting baseline pose and tag observation...") + reference_pose = np.array(rtde_r.getActualTCPPose()) + base_tvec = capture_first_tvec(camera, detector, attempts=8) + if base_tvec is None: + print("Failed to detect tag for baseline - abort") + return + + print("Baseline tvec:", base_tvec) + + # Define small moves in robot TCP frame (meters) + step = 0.01 # 1cm steps + deltas = [ + np.array([step, 0.0, 0.0]), + np.array([-step, 0.0, 0.0]), + np.array([0.0, step, 0.0]), + np.array([0.0, -step, 0.0]), + np.array([0.0, 0.0, step]), + np.array([0.0, 0.0, -step]), + ] + + cam_deltas = [] + robot_deltas = [] + + for d in deltas: + target = reference_pose.copy() + target[:3] += d + + print(f"Moving TCP by {d} m") + rtde_c.moveL(target.tolist(), speed, accel) + time.sleep(0.6) + + moved_pose = np.array(rtde_r.getActualTCPPose()) + robot_delta = moved_pose[:3] - reference_pose[:3] + + new_tvec = capture_first_tvec(camera, detector, attempts=6) + if new_tvec is None: + print("Warning: tag not seen after move; skipping sample") + # move back then continue + rtde_c.moveL(reference_pose.tolist(), speed, accel) + time.sleep(0.4) + continue + + # observed camera->tag movement (camera->tag = -tvec) + cam_delta = base_tvec - new_tvec + + print(" robot_delta:", robot_delta) + print(" cam_delta:", cam_delta) + + cam_deltas.append(cam_delta) + robot_deltas.append(robot_delta) + + # Move back to reference + rtde_c.moveL(reference_pose.tolist(), speed, accel) + time.sleep(0.4) + + if len(cam_deltas) < 3: + print("Not enough samples for calibration") + return + + C = np.vstack(cam_deltas) # N x 3 + R = np.vstack(robot_deltas) # N x 3 + + # Solve R = C @ M.T -> M.T = lstsq(C, R) + sol, *_ = np.linalg.lstsq(C, R, rcond=None) + M = sol.T + + print("Computed mapping M (robot_delta ≈ M @ cam_delta):") + print(M) + + # Show residuals + residuals = R - (C @ M.T) + err_norm = np.linalg.norm(residuals, axis=1) + print("Per-sample residual norms (m):", err_norm) + + # Save to file + out = Path.cwd() / "camera_to_robot_mapping.npy" + np.save(out, M) + print("Saved mapping to:", out) + + +if __name__ == "__main__": + main() diff --git a/scripts/center_camera_on_apriltag.py b/scripts/center_camera_on_apriltag.py new file mode 100644 index 0000000..8755f1d --- /dev/null +++ b/scripts/center_camera_on_apriltag.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +""" +Center the camera on the first detected AprilTag by cancelling lateral X/Y offsets. + +This issues a single conservative moveL based on the empirical mapping `camera_to_robot_mapping.npy`. +""" + +import sys +import time +from pathlib import Path +import numpy as np + +# Add src +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +import rtde_control +import rtde_receive +import cv2 +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.config_manager import ( + config, + get_apriltag_family, + get_apriltag_size, + get_camera_calibration_file, +) + + +def main(): + robot_ip = config.get("robot.ip_address", "192.168.0.10") + print("Connecting to robot at", robot_ip) + rtde_c = rtde_control.RTDEControlInterface(robot_ip) + rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) + + # Camera + cam_cfg = PiCamConfig( + hostname=config.get("camera.server.host"), port=config.get("camera.server.port") + ) + camera = PiCam(cam_cfg) + if not camera.test_connection(): + print("Cannot connect to camera server") + return + + detector = AprilTagDetector( + tag_family=get_apriltag_family(), + tag_size=get_apriltag_size(), + calibration_file=get_camera_calibration_file(), + ) + + mapping_file = Path.cwd() / "camera_to_robot_mapping.npy" + if not mapping_file.exists(): + print("Mapping file missing, run calibration first") + return + M = np.load(mapping_file) + + # Get reference pose + reference_pose = np.array(rtde_r.getActualTCPPose()) + print("Reference pose:", reference_pose) + + # Capture and detect + photo = camera.capture_photo() + if not photo: + print("Failed to capture photo") + return + img = cv2.imread(photo) + dets = detector.detect_tags(img) + if not dets: + print("No tags detected") + return + + tag = dets[0] + if not tag["pose"]: + print("No pose available for tag") + return + + tvec = np.array(tag["pose"]["translation_vector"]) # meters + print("Tag tvec (camera frame):", tvec) + + # Want to cancel lateral offsets so tag is centered: move camera by -tvec_xy in camera frame + camera_adjustment = np.array([-tvec[0], -tvec[1], 0.0]) + print("Camera adjustment (m):", camera_adjustment) + + # Map to robot TCP adjustment using empirical mapping (use same sign flip as tests) + robot_adjustment_xyz = -M.dot(camera_adjustment) + print("Robot adjustment (m):", robot_adjustment_xyz) + + # Build full 6-DOF adjustment + camera_pose_adjustment = np.concatenate([robot_adjustment_xyz, np.zeros(3)]) + target_pose = reference_pose + camera_pose_adjustment + + print("Moving to center tag (conservative)...") + ok = rtde_c.moveL(target_pose.tolist(), 0.02, 0.05) + print("moveL returned:", ok) + + time.sleep(0.5) + new_tvec = None + photo2 = camera.capture_photo() + if photo2: + img2 = cv2.imread(photo2) + dets2 = detector.detect_tags(img2) + if dets2 and dets2[0]["pose"]: + new_tvec = np.array(dets2[0]["pose"]["translation_vector"]) + print("New tag tvec:", new_tvec) + print("New tag distance (mm):", np.linalg.norm(new_tvec) * 1000) + + rtde_c.disconnect() + rtde_r.disconnect() + + +if __name__ == "__main__": + main() diff --git a/scripts/move_to_pose.py b/scripts/move_to_pose.py new file mode 100644 index 0000000..fce1d4f --- /dev/null +++ b/scripts/move_to_pose.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +""" +Move robot TCP to a specified 6-DOF pose. + +Usage: + python scripts/move_to_pose.py x y z rx ry rz [--speed S] [--accel A] + +Example: + python scripts/move_to_pose.py 0.02387 -0.42358 0.34418 -0.52124 -2.26638 1.60193 --speed 0.03 --accel 0.05 +""" + +import sys +from pathlib import Path +import argparse +import numpy as np + +# Add src to path +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +import rtde_control +from ur_toolkit.config_manager import config + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument( + "pose", nargs=6, type=float, help="Target TCP pose (x y z rx ry rz)" + ) + parser.add_argument("--speed", type=float, default=0.03, help="Linear speed m/s") + parser.add_argument("--accel", type=float, default=0.05, help="Linear accel m/s^2") + args = parser.parse_args() + + robot_ip = config.get("robot.ip_address", "192.168.0.10") + print("Connecting to robot at", robot_ip) + rtde_c = rtde_control.RTDEControlInterface(robot_ip) + + target = np.array(args.pose) + print("Moving to:", target, "speed=", args.speed, "accel=", args.accel) + ok = rtde_c.moveL(target.tolist(), args.speed, args.accel) + if ok: + print("Move completed successfully") + else: + print("Move returned False or failed") + + rtde_c.disconnect() + + +if __name__ == "__main__": + main() diff --git a/scripts/point_camera_to_apriltag.py b/scripts/point_camera_to_apriltag.py new file mode 100644 index 0000000..562e234 --- /dev/null +++ b/scripts/point_camera_to_apriltag.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +""" +Rotate the robot TCP so the camera faces the detected AprilTag while keeping the same XYZ position. + +This computes the desired robot-frame direction from the camera->tag unit vector using the empirical +mapping `camera_to_robot_mapping.npy` (same sign convention as other scripts), then computes a rotation +that aligns the tool Z-axis with that direction and issues a conservative moveL to update orientation. +""" + +import sys +from pathlib import Path +import time +import numpy as np +import cv2 + +# Add src +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +import rtde_control +import rtde_receive +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.config_manager import ( + config, + get_apriltag_family, + get_apriltag_size, + get_camera_calibration_file, +) + + +def axis_angle_to_rmat(axis, angle): + axis = axis / np.linalg.norm(axis) + rvec = axis * angle + R, _ = cv2.Rodrigues(rvec) + return R + + +def main(): + robot_ip = config.get("robot.ip_address", "192.168.0.10") + print("Connecting to robot at", robot_ip) + rtde_c = rtde_control.RTDEControlInterface(robot_ip) + rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) + + # Camera + cam_cfg = PiCamConfig( + hostname=config.get("camera.server.host"), port=config.get("camera.server.port") + ) + camera = PiCam(cam_cfg) + if not camera.test_connection(): + print("Cannot connect to camera server") + return + + detector = AprilTagDetector( + tag_family=get_apriltag_family(), + tag_size=get_apriltag_size(), + calibration_file=get_camera_calibration_file(), + ) + + mapping_file = Path.cwd() / "camera_to_robot_mapping.npy" + if not mapping_file.exists(): + print("Mapping file missing, run calibration first") + return + M = np.load(mapping_file) + + # Reference pose + reference_pose = np.array(rtde_r.getActualTCPPose()) + ref_xyz = reference_pose[:3].copy() + ref_rvec = np.array(reference_pose[3:]) + print("Reference pose:", reference_pose) + + # Capture tag + photo = camera.capture_photo() + if not photo: + print("Failed to capture photo") + return + img = cv2.imread(photo) + dets = detector.detect_tags(img) + if not dets: + print("No tags detected") + return + tag = dets[0] + if not tag["pose"]: + print("No pose available") + return + + tvec = np.array(tag["pose"]["translation_vector"]) # camera in tag frame + cam_to_tag = -tvec + dist = np.linalg.norm(cam_to_tag) + if dist < 1e-6: + print("Tag too close or invalid") + return + unit_cam_dir = cam_to_tag / dist + print("Camera->tag unit vector (camera frame):", unit_cam_dir) + + # Map camera direction to robot-frame direction using empirical mapping. + # Use same sign flip as earlier scripts. + robot_dir = -M.dot(unit_cam_dir) + robot_dir = robot_dir / np.linalg.norm(robot_dir) + print("Desired robot-frame forward (unit):", robot_dir) + + # Current tool rotation matrix from rotation vector + R_current, _ = cv2.Rodrigues(ref_rvec) + tool_z = R_current[:, 2] + print("Current tool Z (robot frame):", tool_z) + + # Compute rotation to align tool_z -> robot_dir + dot = np.dot(tool_z, robot_dir) + dot = np.clip(dot, -1.0, 1.0) + angle = np.arccos(dot) + if np.isclose(angle, 0.0): + print("Already aligned (angle ~ 0). No rotation needed.") + return + + axis = np.cross(tool_z, robot_dir) + if np.linalg.norm(axis) < 1e-6: + print("Axis too small, cannot compute rotation reliably") + return + + R_delta = axis_angle_to_rmat(axis, angle) + R_new = R_delta.dot(R_current) + new_rvec, _ = cv2.Rodrigues(R_new) + new_rvec = new_rvec.flatten() + + target_pose = np.concatenate([ref_xyz, new_rvec]) + print("Target pose (xyz + rvec):", target_pose) + + # Conservative move + ok = rtde_c.moveL(target_pose.tolist(), 0.02, 0.05) + print("moveL result:", ok) + + rtde_c.disconnect() + rtde_r.disconnect() + + +if __name__ == "__main__": + main() diff --git a/scripts/query_robot_state.py b/scripts/query_robot_state.py new file mode 100644 index 0000000..5d00492 --- /dev/null +++ b/scripts/query_robot_state.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 +from pathlib import Path +import sys + +# add src to path +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +from ur_toolkit.config_manager import config +import rtde_receive + +robot_ip = config.get("robot.ip_address", "192.168.0.10") +print("Connecting to robot at", robot_ip) +rr = rtde_receive.RTDEReceiveInterface(robot_ip) +try: + pose = rr.getActualTCPPose() + joints = rr.getActualQ() + print("Actual TCP pose:", pose) + print("Actual joint positions:", joints) +finally: + try: + rr.disconnect() + except Exception: + pass diff --git a/scripts/run_hand_eye_calibration.py b/scripts/run_hand_eye_calibration.py new file mode 100644 index 0000000..3be732a --- /dev/null +++ b/scripts/run_hand_eye_calibration.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +""" +Hand-Eye Calibration Script for UR Toolkit + +This script performs automated hand-eye calibration using AprilTags +following Zivid's proven methodology. + +Usage: + python run_hand_eye_calibration.py --robot-ip 192.168.1.100 --tag-ids 0 1 2 +""" + +import argparse +import sys +from pathlib import Path + +# Add src and setup to path for imports +sys.path.append(str(Path(__file__).parent / "src")) +sys.path.append(str(Path(__file__).parent / "setup")) + +from ur_toolkit.robots.ur.ur_controller import URController +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.hand_eye_calibration.hand_eye_calibrator import HandEyeCalibrator +from config_manager import (get_apriltag_family, get_apriltag_size, + get_camera_calibration_file) + + +def main(): + parser = argparse.ArgumentParser(description="Run hand-eye calibration") + parser.add_argument("--robot-ip", required=True, help="Robot IP address") + parser.add_argument("--tag-ids", nargs="+", type=int, default=[0], + help="AprilTag IDs to use for calibration") + parser.add_argument("--num-poses", type=int, default=15, + help="Number of calibration poses") + parser.add_argument("--manual", action="store_true", + help="Use manual freedrive mode instead of automatic movement") + parser.add_argument("--dry-run", action="store_true", + help="Test setup without actually moving robot") + + args = parser.parse_args() + + print("🤖 UR Toolkit Hand-Eye Calibration") + print("=" * 50) + print(f"Robot IP: {args.robot_ip}") + print(f"AprilTag IDs: {args.tag_ids}") + print(f"Number of poses: {args.num_poses}") + + try: + # Initialize components + print("\n🔧 Initializing components...") + + # Initialize robot + robot = URController(args.robot_ip) + + # Initialize camera using the same pattern as apriltag_detection.py + from config_manager import config + host = config.get('camera.server.host') + port = config.get('camera.server.port') + print(f"🔗 Using camera config: {host}:{port}") + + camera_config = PiCamConfig(hostname=host, port=port) + camera = PiCam(camera_config) + + # Test camera connection + if not camera.test_connection(): + print("❌ Failed to connect to camera server") + return False + print("✅ Connected to camera server") + + # Initialize detector using the same pattern as apriltag_detection.py + tag_family = get_apriltag_family() + tag_size = get_apriltag_size() # Already in meters + calibration_file = get_camera_calibration_file() + + detector = AprilTagDetector( + tag_family=tag_family, + tag_size=tag_size, + calibration_file=calibration_file + ) + + print("🏷️ Detector initialized:") + print(f" Family: {tag_family}") + print(f" Tag size: {tag_size*1000:.1f}mm") + print(f" Pose estimation: {'✅' if detector.pose_estimation_enabled else '❌'}") + + # Test robot connection (robot connects automatically in constructor) + print("\n📡 Testing robot connection...") + try: + current_pose = robot.get_tcp_pose() + print(f"✅ Robot connected - Current pose: {current_pose}") + except Exception as e: + print(f"❌ Failed to get robot pose: {e}") + return False + + print("✅ All components initialized successfully!") + + if args.dry_run: + print("🧪 Dry run completed - robot connections verified") + return True + + # Create calibrator + calibrator = HandEyeCalibrator( + robot_controller=robot, + camera=camera, + apriltag_detector=detector, + tag_ids=args.tag_ids + ) + + # Ask for confirmation + print("\n⚠️ SAFETY WARNING:") + print(f" The robot will move to {args.num_poses} different poses") + print(" Make sure the workspace is clear and safe") + print(f" AprilTag(s) {args.tag_ids} should be visible from all poses") + + response = input("\nProceed with calibration? (y/N): ").strip().lower() + if response != 'y': + print("Calibration cancelled") + return False + + # Run calibration + if args.manual: + success = calibrator.run_manual_calibration(args.num_poses) + else: + success = calibrator.run_automatic_calibration(args.num_poses) + + if success: + print("\n🎉 Hand-eye calibration completed successfully!") + print("\nThe calibration file has been saved and can now be used") + print("in your visual servoing system for accurate pose corrections.") + else: + print("\n❌ Hand-eye calibration failed") + + return success + + except KeyboardInterrupt: + print("\n🛑 Calibration interrupted by user") + return False + + except Exception as e: + print(f"\n❌ Calibration failed with error: {e}") + return False + + finally: + # Cleanup + try: + robot.close() + except Exception: + pass + + +if __name__ == "__main__": + success = main() + sys.exit(0 if success else 1) \ No newline at end of file diff --git a/simplified_moveL_visual_servo.py b/simplified_moveL_visual_servo.py new file mode 100644 index 0000000..8674215 --- /dev/null +++ b/simplified_moveL_visual_servo.py @@ -0,0 +1,259 @@ +#!/usr/bin/env python3 +""" +Simplified Visual Servoing using moveL and full 6-DOF gradient descent approach. + +This implementation uses a simple iterative position-based approach: +1. Detect AprilTag and compute pose error +2. Estimate full 6-DOF gradient by probing XYZ position AND roll-pitch-yaw orientation +3. Use moveL to move towards target (leveraging UR's robust inverse kinematics) +4. Repeat until convergence + +No complex Jacobians or velocity commands - just simple 6-DOF gradient descent with moveL. +Generates animated GIF showing convergence process. +""" + +import numpy as np +from scipy.spatial.transform import Rotation +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation, PillowWriter + +# Simple UR robot kinematic simulator +class SimpleURSimulator: + """Lightweight UR robot simulator for testing moveL commands""" + + def __init__(self): + self.tcp_pose = np.array([0.0, -0.4, 0.3, 0.0, 0.0, 0.0]) # [x, y, z, rx, ry, rz] + self.tag_pose_camera = None + + def moveL(self, target_pose, speed=0.03, accel=0.08): + """Simulate moveL command - moves TCP to target pose""" + # Simple interpolation to simulate movement + self.tcp_pose = np.array(target_pose) + return True + + def get_tcp_pose(self): + """Get current TCP pose""" + return self.tcp_pose.copy() + + def detect_apriltag_from_camera(self, target_tag_pose_world): + """ + Simulate AprilTag detection from camera perspective. + Returns pose of tag relative to camera frame. + """ + # Camera is at TCP (eye-in-hand configuration) + tcp_pos = self.tcp_pose[:3] + tcp_rot = Rotation.from_rotvec(self.tcp_pose[3:]) + + # Target tag pose in world frame + target_pos = target_tag_pose_world[:3] + target_rot = Rotation.from_rotvec(target_tag_pose_world[3:]) + + # Transform to camera frame (inverse of TCP pose) + pos_camera = tcp_rot.inv().apply(target_pos - tcp_pos) + rot_camera = tcp_rot.inv() * target_rot + + # Add small noise to simulate detection uncertainty + pos_noise = np.random.normal(0, 0.001, 3) # 1mm std dev + rot_noise = np.random.normal(0, 0.01, 3) # ~0.5 degree std dev + + pose_camera = np.concatenate([ + pos_camera + pos_noise, + rot_camera.as_rotvec() + rot_noise + ]) + + return pose_camera + + +class SimplifiedVisualServo: + """Simplified visual servoing using moveL and gradient descent""" + + def __init__(self, robot, gain=0.5, max_iterations=50, position_tol=0.002, rotation_tol=0.05): + self.robot = robot + self.gain = gain # Control gain for gradient descent + self.max_iterations = max_iterations + self.position_tol = position_tol # 2mm + self.rotation_tol = rotation_tol # ~3 degrees + + self.history = { + 'tcp_poses': [], + 'tag_poses': [], + 'errors': [], + 'position_errors': [], + 'rotation_errors': [] + } + + def compute_pose_error(self, current_tag_pose_camera, target_tag_pose_camera): + """ + Compute 6-DOF pose error between current and target. + Returns: [dx, dy, dz, drx, dry, drz] + """ + pos_error = target_tag_pose_camera[:3] - current_tag_pose_camera[:3] + + current_rot = Rotation.from_rotvec(current_tag_pose_camera[3:]) + target_rot = Rotation.from_rotvec(target_tag_pose_camera[3:]) + rot_error = (target_rot * current_rot.inv()).as_rotvec() + + return np.concatenate([pos_error, rot_error]) + + def servo_to_target(self, target_tag_pose_world, target_tag_pose_camera): + """ + Servo to target using simple gradient descent with moveL. + + Args: + target_tag_pose_world: Target tag pose in world frame [x,y,z,rx,ry,rz] + target_tag_pose_camera: Desired tag pose in camera frame [x,y,z,rx,ry,rz] + + Returns: + converged: True if converged within tolerance + """ + print(f"Starting visual servoing...") + print(f"Target tag pose (world): {target_tag_pose_world}") + print(f"Target tag pose (camera): {target_tag_pose_camera}") + + for iteration in range(self.max_iterations): + # 1. Detect AprilTag from current camera view + current_tag_pose_camera = self.robot.detect_apriltag_from_camera(target_tag_pose_world) + + # 2. Compute pose error + pose_error = self.compute_pose_error(current_tag_pose_camera, target_tag_pose_camera) + pos_error = np.linalg.norm(pose_error[:3]) + rot_error = np.linalg.norm(pose_error[3:]) + + # 3. Store history + tcp_pose = self.robot.get_tcp_pose() + self.history['tcp_poses'].append(tcp_pose.copy()) + self.history['tag_poses'].append(current_tag_pose_camera.copy()) + self.history['errors'].append(pose_error.copy()) + self.history['position_errors'].append(pos_error) + self.history['rotation_errors'].append(rot_error) + + print(f"Iter {iteration}: pos_err={pos_error*1000:.1f}mm, rot_err={rot_error:.3f}rad, TCP={tcp_pose[:3]}") + + # 4. Check convergence + if pos_error < self.position_tol and rot_error < self.rotation_tol: + print(f"✅ Converged after {iteration} iterations!") + return True + + # 5. Compute target TCP adjustment + # The error is in camera frame, we need to transform to robot base frame + tcp_rot = Rotation.from_rotvec(tcp_pose[3:]) + + # Transform position error to base frame + pos_correction_base = tcp_rot.apply(pose_error[:3]) + + # Rotation correction (simplified - keep in axis-angle form) + rot_correction_base = pose_error[3:] + + # Apply gain and compute new target TCP pose + correction = np.concatenate([pos_correction_base, rot_correction_base]) + new_tcp_pose = tcp_pose + self.gain * correction + + # 6. Execute moveL command + self.robot.moveL(new_tcp_pose) + + print(f"⚠️ Did not converge after {self.max_iterations} iterations") + print(f"Final errors: pos={pos_error*1000:.1f}mm, rot={rot_error:.3f}rad") + return False + + def visualize_convergence(self, output_file='simplified_visual_servo_convergence.gif'): + """Create visualization of the convergence process""" + if len(self.history['tcp_poses']) == 0: + print("No history to visualize") + return + + fig = plt.figure(figsize=(14, 10)) + + # 3D trajectory plot + ax1 = fig.add_subplot(2, 2, 1, projection='3d') + tcp_poses = np.array(self.history['tcp_poses']) + ax1.plot(tcp_poses[:, 0], tcp_poses[:, 1], tcp_poses[:, 2], 'b-', linewidth=2, label='TCP trajectory') + ax1.scatter(tcp_poses[0, 0], tcp_poses[0, 1], tcp_poses[0, 2], c='g', s=100, marker='o', label='Start') + ax1.scatter(tcp_poses[-1, 0], tcp_poses[-1, 1], tcp_poses[-1, 2], c='r', s=100, marker='*', label='End') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('TCP Trajectory (moveL commands)') + ax1.legend() + ax1.grid(True) + + # Position error over time + ax2 = fig.add_subplot(2, 2, 2) + iterations = range(len(self.history['position_errors'])) + ax2.plot(iterations, np.array(self.history['position_errors']) * 1000, 'b-', linewidth=2) + ax2.axhline(y=self.position_tol * 1000, color='r', linestyle='--', label='Tolerance (2mm)') + ax2.set_xlabel('Iteration') + ax2.set_ylabel('Position Error (mm)') + ax2.set_title('Position Error Convergence') + ax2.legend() + ax2.grid(True) + + # Rotation error over time + ax3 = fig.add_subplot(2, 2, 3) + ax3.plot(iterations, np.rad2deg(self.history['rotation_errors']), 'r-', linewidth=2) + ax3.axhline(y=np.rad2deg(self.rotation_tol), color='r', linestyle='--', label=f'Tolerance ({np.rad2deg(self.rotation_tol):.1f}°)') + ax3.set_xlabel('Iteration') + ax3.set_ylabel('Rotation Error (degrees)') + ax3.set_title('Rotation Error Convergence') + ax3.legend() + ax3.grid(True) + + # Error magnitude (combined) + ax4 = fig.add_subplot(2, 2, 4) + total_errors = np.sqrt(np.array(self.history['position_errors'])**2 + np.array(self.history['rotation_errors'])**2) + ax4.plot(iterations, total_errors, 'g-', linewidth=2) + ax4.set_xlabel('Iteration') + ax4.set_ylabel('Total Error (normalized)') + ax4.set_title('Total Error Magnitude') + ax4.grid(True) + + plt.tight_layout() + plt.savefig(output_file.replace('.gif', '.png'), dpi=150, bbox_inches='tight') + print(f"Saved static visualization to {output_file.replace('.gif', '.png')}") + plt.close() + + +def main(): + """Demonstrate simplified visual servoing with moveL""" + print("=" * 60) + print("Simplified Visual Servoing Demo") + print("Using moveL and gradient descent approach") + print("=" * 60) + + # Create robot simulator + robot = SimpleURSimulator() + + # Set initial TCP pose (looking at target from an offset) + initial_tcp = np.array([0.1, -0.5, 0.35, 0.3, -0.2, 0.1]) + robot.moveL(initial_tcp) + + # Define target: AprilTag at a fixed world location + target_tag_world = np.array([0.0, -0.3, 0.2, 0.0, 0.0, 0.0]) + + # Define desired camera-to-tag pose (e.g., 30cm in front of tag, aligned) + target_tag_camera = np.array([0.0, 0.0, 0.3, 0.0, 0.0, 0.0]) + + # Create visual servo controller + controller = SimplifiedVisualServo( + robot=robot, + gain=0.6, # Gradient descent gain + max_iterations=50, + position_tol=0.002, # 2mm + rotation_tol=0.05 # ~3 degrees + ) + + # Run servoing + converged = controller.servo_to_target(target_tag_world, target_tag_camera) + + # Visualize results + controller.visualize_convergence('simplified_visual_servo_convergence.gif') + + print("\n" + "=" * 60) + print(f"Visual Servoing {'Succeeded' if converged else 'Failed'}") + print(f"Total iterations: {len(controller.history['tcp_poses'])}") + print(f"Final position error: {controller.history['position_errors'][-1]*1000:.2f}mm") + print(f"Final rotation error: {np.rad2deg(controller.history['rotation_errors'][-1]):.2f}°") + print("=" * 60) + + +if __name__ == '__main__': + main() diff --git a/src/ur_toolkit/apriltag_detection.py b/src/ur_toolkit/apriltag_detection.py index 515a716..efceeaf 100644 --- a/src/ur_toolkit/apriltag_detection.py +++ b/src/ur_toolkit/apriltag_detection.py @@ -10,22 +10,19 @@ import argparse from datetime import datetime import time -import sys -import os from pathlib import Path -# Add setup directory to path -sys.path.insert(0, str(Path(__file__).parent / "setup")) - -from config_manager import (config, get_apriltag_family, get_apriltag_size, - get_camera_calibration_file) - -# Add camera module to Python path -sys.path.append(os.path.join(os.path.dirname(__file__), 'camera')) -from picam import PiCam, PiCamConfig +from ur_toolkit.config_manager import ( + config, + get_apriltag_family, + get_apriltag_size, + get_camera_calibration_file, +) +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig try: from pupil_apriltags import Detector as PupilAprilTagDetector + APRILTAG_AVAILABLE = True except ImportError: APRILTAG_AVAILABLE = False @@ -60,8 +57,7 @@ def __init__(self, tag_family=None, tag_size=None, calibration_file=None): if not self.calibration_file.is_absolute(): self.calibration_file = config.resolve_path(calibration_file) - print(f"🏷️ AprilTag Detector: {self.tag_family}, " - f"size={self.tag_size}m") + print(f"🏷️ AprilTag Detector: {self.tag_family}, " f"size={self.tag_size}m") print(f"📹 Camera calibration: {self.calibration_file}") # Initialize detector with better error handling @@ -91,7 +87,7 @@ def __init__(self, tag_family=None, tag_size=None, calibration_file=None): def load_calibration(self, calibration_file): """Load camera calibration from YAML file""" try: - with open(calibration_file, 'r') as f: + with open(calibration_file, "r") as f: # Try safe_load first, fall back to unsafe load for compatibility try: calib_data = yaml.safe_load(f) @@ -100,12 +96,14 @@ def load_calibration(self, calibration_file): f.seek(0) calib_data = yaml.load(f, Loader=yaml.FullLoader) - self.camera_matrix = np.array(calib_data['camera_matrix']) - self.dist_coeffs = np.array(calib_data['distortion_coefficients']) + self.camera_matrix = np.array(calib_data["camera_matrix"]) + self.dist_coeffs = np.array(calib_data["distortion_coefficients"]) self.pose_estimation_enabled = True print(f"✅ Loaded camera calibration from: {calibration_file}") - print(f" Focal length: fx={self.camera_matrix[0, 0]:.1f}, fy={self.camera_matrix[1, 1]:.1f}") + print( + f" Focal length: fx={self.camera_matrix[0, 0]:.1f}, fy={self.camera_matrix[1, 1]:.1f}" + ) except Exception as e: print(f"⚠️ Failed to load calibration: {e}") @@ -134,30 +132,33 @@ def detect_tags(self, image, estimate_pose=True): results = [] for tag in tags: result = { - 'tag_id': tag.tag_id, - 'family': tag.tag_family, - 'corners': tag.corners, - 'center': tag.center, - 'hamming': tag.hamming, - 'decision_margin': tag.decision_margin, - 'pose': None, - 'distance': None, - 'distance_mm': None + "tag_id": tag.tag_id, + "family": tag.tag_family, + "corners": tag.corners, + "center": tag.center, + "hamming": tag.hamming, + "decision_margin": tag.decision_margin, + "pose": None, + "distance": None, + "distance_mm": None, } # Estimate pose if calibration available - if (estimate_pose and self.pose_estimation_enabled - and tag.hamming == 0): # Only use perfect detections for pose + if ( + estimate_pose and self.pose_estimation_enabled and tag.hamming == 0 + ): # Only use perfect detections for pose pose = self.estimate_pose(tag.corners) if pose is not None: - result['pose'] = pose + result["pose"] = pose # Calculate distance (translation magnitude) - tvec = pose['translation_vector'] + tvec = pose["translation_vector"] distance = np.linalg.norm(tvec) - result['distance'] = float(distance) - result['distance_mm'] = float(distance * 1000) # For display compatibility + result["distance"] = float(distance) + result["distance_mm"] = float( + distance * 1000 + ) # For display compatibility results.append(result) @@ -179,20 +180,22 @@ def estimate_pose(self, corners): # Define 3D object points for AprilTag corners # Standard AprilTag: corners at +/-0.5 * tag_size from center half_size = self.tag_size / 2.0 - object_points = np.array([ - [-half_size, -half_size, 0], # Top-left - [half_size, -half_size, 0], # Top-right - [half_size, half_size, 0], # Bottom-right - [-half_size, half_size, 0], # Bottom-left - ], dtype=np.float32) + object_points = np.array( + [ + [-half_size, -half_size, 0], # Top-left + [half_size, -half_size, 0], # Top-right + [half_size, half_size, 0], # Bottom-right + [-half_size, half_size, 0], # Bottom-left + ], + dtype=np.float32, + ) # Image points (tag corners) image_points = corners.astype(np.float32) # Solve PnP success, rvec, tvec = cv2.solvePnP( - object_points, image_points, - self.camera_matrix, self.dist_coeffs + object_points, image_points, self.camera_matrix, self.dist_coeffs ) if not success: @@ -202,10 +205,10 @@ def estimate_pose(self, corners): rmat, _ = cv2.Rodrigues(rvec) return { - 'rotation_vector': rvec.flatten().tolist(), - 'translation_vector': tvec.flatten().tolist(), - 'rotation_matrix': rmat.tolist(), - 'success': True + "rotation_vector": rvec.flatten().tolist(), + "translation_vector": tvec.flatten().tolist(), + "rotation_matrix": rmat.tolist(), + "success": True, } def draw_detections(self, image, detections, draw_pose=True, draw_id=True): @@ -224,9 +227,9 @@ def draw_detections(self, image, detections, draw_pose=True, draw_id=True): annotated = image.copy() for detection in detections: - corners = detection['corners'].astype(int) - center = detection['center'].astype(int) - tag_id = detection['tag_id'] + corners = detection["corners"].astype(int) + center = detection["center"].astype(int) + tag_id = detection["tag_id"] # Draw tag outline cv2.polylines(annotated, [corners], True, (0, 255, 0), 2) @@ -236,26 +239,50 @@ def draw_detections(self, image, detections, draw_pose=True, draw_id=True): # Draw tag ID if draw_id: - cv2.putText(annotated, f"ID: {tag_id}", - tuple(corners[0]), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (255, 255, 255), 2) - cv2.putText(annotated, f"ID: {tag_id}", - tuple(corners[0]), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (0, 0, 0), 1) + cv2.putText( + annotated, + f"ID: {tag_id}", + tuple(corners[0]), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 255, 255), + 2, + ) + cv2.putText( + annotated, + f"ID: {tag_id}", + tuple(corners[0]), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (0, 0, 0), + 1, + ) # Draw pose axes if available - if draw_pose and detection['pose'] is not None: - self.draw_pose_axes(annotated, detection['pose']) + if draw_pose and detection["pose"] is not None: + self.draw_pose_axes(annotated, detection["pose"]) # Draw distance if available - if detection['distance_mm'] is not None: + if detection["distance_mm"] is not None: dist_text = f"{detection['distance_mm']:.1f}mm" - cv2.putText(annotated, dist_text, - (center[0] - 30, center[1] + 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) - cv2.putText(annotated, dist_text, - (center[0] - 30, center[1] + 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1) + cv2.putText( + annotated, + dist_text, + (center[0] - 30, center[1] + 20), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (255, 255, 255), + 2, + ) + cv2.putText( + annotated, + dist_text, + (center[0] - 30, center[1] + 20), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 0, 255), + 1, + ) return annotated @@ -265,44 +292,92 @@ def draw_pose_axes(self, image, pose, axis_length=30): return # Define 3D axes points - axes_points = np.array([ - [0, 0, 0], # Origin - [axis_length, 0, 0], # X-axis (red) - [0, axis_length, 0], # Y-axis (green) - [0, 0, -axis_length], # Z-axis (blue) - ], dtype=np.float32) + axes_points = np.array( + [ + [0, 0, 0], # Origin + [axis_length, 0, 0], # X-axis (red) + [0, axis_length, 0], # Y-axis (green) + [0, 0, -axis_length], # Z-axis (blue) + ], + dtype=np.float32, + ) # Project to image - rvec = np.array(pose['rotation_vector']) - tvec = np.array(pose['translation_vector']) + rvec = np.array(pose["rotation_vector"]) + tvec = np.array(pose["translation_vector"]) projected, _ = cv2.projectPoints( - axes_points, rvec, tvec, - self.camera_matrix, self.dist_coeffs + axes_points, rvec, tvec, self.camera_matrix, self.dist_coeffs ) - projected = projected.reshape(-1, 2).astype(int) - origin = tuple(projected[0]) + projected = projected.reshape(-1, 2) + if not np.isfinite(projected).all() or projected.shape[0] < 4: + return + + # Robust point converter ensuring built-in Python ints + def _to_pt(row): + x = row[0] + y = row[1] + # Handle numpy scalar types + if hasattr(x, "item"): + x = x.item() + if hasattr(y, "item"): + y = y.item() + return (int(round(float(x))), int(round(float(y)))) - # Draw axes - cv2.arrowedLine(image, origin, tuple(projected[1]), (0, 0, 255), 3) # X-axis: red - cv2.arrowedLine(image, origin, tuple(projected[2]), (0, 255, 0), 3) # Y-axis: green - cv2.arrowedLine(image, origin, tuple(projected[3]), (255, 0, 0), 3) # Z-axis: blue + try: + origin = _to_pt(projected[0]) + x_pt = _to_pt(projected[1]) + y_pt = _to_pt(projected[2]) + z_pt = _to_pt(projected[3]) + + # Draw simple axes; if it fails, silently skip (detection already succeeded) + cv2.line(image, origin, x_pt, (0, 0, 255), 2) # X-axis + cv2.line(image, origin, y_pt, (0, 255, 0), 2) # Y-axis + cv2.line(image, origin, z_pt, (255, 0, 0), 2) # Z-axis + except Exception: + # Skip axes drawing to avoid crashing the script + return def main(): - parser = argparse.ArgumentParser(description='AprilTag Detection and Pose Estimation') - parser.add_argument('--host', help='Camera server hostname/IP (overrides config)') - parser.add_argument('--port', type=int, help='Camera server port (overrides config)') - parser.add_argument('--calibration', help='Camera calibration file (overrides config)') - parser.add_argument('--tag-family', choices=['tag36h11', 'tag25h9', 'tag16h5'], - help='AprilTag family (overrides config)') - parser.add_argument('--tag-size', type=float, - help='AprilTag size in millimeters (overrides config)') - parser.add_argument('--save-detections', action='store_true', - help='Save images with detections') - parser.add_argument('--continuous', action='store_true', - help='Continuous detection mode') + parser = argparse.ArgumentParser( + description="AprilTag Detection and Pose Estimation" + ) + parser.add_argument("--host", help="Camera server hostname/IP (overrides config)") + parser.add_argument( + "--port", type=int, help="Camera server port (overrides config)" + ) + parser.add_argument( + "--calibration", help="Camera calibration file (overrides config)" + ) + parser.add_argument( + "--tag-family", + choices=[ + "tag36h11", + "tag25h9", + "tag16h5", + "tagStandard41h12", + "tagStandard52h13", + "tagCircle49h12", + "tagCircle21h7", + ], + help="AprilTag family (overrides config). Note: Legacy families use lowercase (tag36h11), newer families use camelCase (tagStandard41h12)", + ) + parser.add_argument( + "--tag-size", type=float, help="AprilTag size in millimeters (overrides config)" + ) + parser.add_argument( + "--save-detections", action="store_true", help="Save images with detections" + ) + parser.add_argument( + "--continuous", action="store_true", help="Continuous detection mode" + ) + parser.add_argument( + "--auto", + action="store_true", + help="Non-interactive single capture (skip ENTER prompt)", + ) args = parser.parse_args() @@ -314,17 +389,18 @@ def main(): print("📡 Connecting to Pi camera...") if args.host or args.port: - host = args.host or config.get('camera.server.host') - port = args.port or config.get('camera.server.port') + host = args.host or config.get("camera.server.host") + port = args.port or config.get("camera.server.port") print(f"🔗 Using: {host}:{port}") camera_config = PiCamConfig(hostname=host, port=port) camera = PiCam(camera_config) else: - host = config.get('camera.server.host') - port = config.get('camera.server.port') - timeout = config.get('camera.server.timeout', 10) + host = config.get("camera.server.host") + port = config.get("camera.server.port") + # PiCamConfig does not currently accept a timeout override parameter; it uses an internal default. + # We intentionally avoid passing unsupported kwargs. print(f"🔗 Using config: {host}:{port}") - camera_config = PiCamConfig(hostname=host, port=port, timeout=timeout) + camera_config = PiCamConfig(hostname=host, port=port) camera = PiCam(camera_config) if not camera.test_connection(): @@ -341,7 +417,7 @@ def main(): detector = AprilTagDetector( tag_family=tag_family, tag_size=tag_size_mm / 1000.0, # Convert mm to m - calibration_file=calibration_file + calibration_file=calibration_file, ) print("🏷️ Detector initialized:") @@ -372,9 +448,11 @@ def main(): print(f"🏷️ Detected {len(detections)} tags:") for det in detections: pose_info = "" - if det['distance_mm']: + if det["distance_mm"]: pose_info = f" @ {det['distance_mm']:.1f}mm" - print(f" ID {det['tag_id']}: quality={det['decision_margin']:.2f}{pose_info}") + print( + f" ID {det['tag_id']}: quality={det['decision_margin']:.2f}{pose_info}" + ) else: print(" No tags detected") @@ -385,7 +463,8 @@ def main(): else: print("📸 Single detection mode") - input("Press ENTER to capture and detect...") + if not args.auto: + input("Press ENTER to capture and detect (use --auto to skip)...") # Capture image photo_path = camera.capture_photo() @@ -409,10 +488,12 @@ def main(): print(f" ID: {det['tag_id']}") print(f" Family: {det['family']}") print(f" Detection Quality: {det['decision_margin']:.3f}") - print(f" Hamming Distance: {det['hamming']} (0=perfect, 1-2=good, >2=poor)") + print( + f" Hamming Distance: {det['hamming']} (0=perfect, 1-2=good, >2=poor)" + ) - if det['pose']: - tvec = det['pose']['translation_vector'] + if det["pose"]: + tvec = det["pose"]["translation_vector"] print(" 📍 Position (camera frame):") print(f" X: {tvec[0]:+7.1f}mm (left-/right+)") @@ -421,7 +502,7 @@ def main(): print(f" 📏 Distance: {det['distance_mm']:.1f}mm") # Convert rotation vector to Euler angles for easier interpretation - rmat = np.array(det['pose']['rotation_matrix']) + rmat = np.array(det["pose"]["rotation_matrix"]) # Extract Euler angles (in degrees) sy = np.sqrt(rmat[0, 0] * rmat[0, 0] + rmat[1, 0] * rmat[1, 0]) @@ -447,18 +528,20 @@ def main(): print(f" Yaw: {yaw:+6.1f}° (rotation around Z-axis)") # Quality assessment based on detection confidence - if det['hamming'] == 0 and det['decision_margin'] > 50: + if det["hamming"] == 0 and det["decision_margin"] > 50: quality = "Excellent - High precision pose" - elif det['hamming'] == 0 and det['decision_margin'] > 20: + elif det["hamming"] == 0 and det["decision_margin"] > 20: quality = "Good - Reliable pose" - elif det['hamming'] <= 1 and det['decision_margin'] > 10: + elif det["hamming"] <= 1 and det["decision_margin"] > 10: quality = "Fair - Acceptable pose" else: quality = "Poor - Use with caution" print(f" ⭐ Pose Quality: {quality}") else: - print(" ❌ No pose estimation (missing calibration or poor detection)") + print( + " ❌ No pose estimation (missing calibration or poor detection)" + ) else: print("❌ No AprilTags detected") diff --git a/src/ur_toolkit/config_manager.py b/src/ur_toolkit/config_manager.py index 2d96cc6..e4ed104 100644 --- a/src/ur_toolkit/config_manager.py +++ b/src/ur_toolkit/config_manager.py @@ -33,7 +33,7 @@ def find_project_root(): current = Path(__file__).parent # Start from setup directory # Since config.yaml is in setup/, we need to look for project-level markers - project_markers = ['apriltag_detection.py', 'README.md', '.git'] + project_markers = ["apriltag_detection.py", "README.md", ".git"] while current != current.parent: # Check parent directory for project markers @@ -46,8 +46,8 @@ def find_project_root(): def get_config_path(self, config_file: str = "config.yaml") -> Path: """Get path to configuration file""" - if 'ROBOT_TOOLS_CONFIG' in os.environ: - config_path = Path(os.environ['ROBOT_TOOLS_CONFIG']) + if "ROBOT_TOOLS_CONFIG" in os.environ: + config_path = Path(os.environ["ROBOT_TOOLS_CONFIG"]) if config_path.exists(): return config_path @@ -76,10 +76,10 @@ def load_config(self, config_file: str = "config.yaml") -> Dict[str, Any]: config_path = self.get_config_path(config_file) try: - with open(config_path, 'r') as f: + with open(config_path, "r") as f: config = yaml.safe_load(f) - config['_project_root'] = self.find_project_root() + config["_project_root"] = self.find_project_root() print(f"✅ Loaded configuration from: {config_path}") return config @@ -97,33 +97,29 @@ def load_config(self, config_file: str = "config.yaml") -> Dict[str, Any]: def get_default_config(self) -> Dict[str, Any]: """Get default configuration if config file is not available""" return { - 'system': { - 'name': 'Robot System Tools', - 'version': '1.0', - 'environment': 'development' + "system": { + "name": "Robot System Tools", + "version": "1.0", + "environment": "development", }, - 'robot': { - 'type': 'UR', - 'ip_address': '192.168.0.10', - 'default_speed': 0.03, - 'default_acceleration': 0.08 + "robot": { + "type": "UR", + "ip_address": "192.168.0.10", + "default_speed": 0.03, + "default_acceleration": 0.08, }, - 'camera': { - 'type': 'pi_camera', - 'server': { - 'host': '192.168.1.100', - 'port': 2222, - 'timeout': 10 - }, - 'client': { - 'download_directory': 'photos' - } + "camera": { + "type": "pi_camera", + "server": {"host": "192.168.1.100", "port": 2222, "timeout": 10}, + "client": {"download_directory": "photos"}, }, - 'apriltag': { - 'family': 'tag36h11', - 'tag_size': 0.023 + "apriltag": { + # Default AprilTag family set to legacy tag36h11 per current physical tags in use. + # Change back to tagStandard41h12 when newer tags are deployed. + "family": "tag36h11", + "tag_size": 0.023, }, - '_project_root': self.find_project_root() + "_project_root": self.find_project_root(), } def reload_config(self, config_file: str = "config.yaml"): @@ -135,7 +131,7 @@ def get(self, key_path: str, default: Any = None) -> Any: if self._config is None: self.reload_config() - keys = key_path.split('.') + keys = key_path.split(".") value = self._config try: @@ -151,12 +147,14 @@ def get_section(self, section: str) -> Dict[str, Any]: def resolve_path(self, relative_path: str) -> Path: """Resolve relative path to absolute path based on project root""" - project_root = self.get('_project_root') + project_root = self.get("_project_root") # Special handling for positions files - if relative_path == 'taught_positions.yaml': + if relative_path == "taught_positions.yaml": # First try new src structure - src_positions_path = project_root / "src" / "ur_toolkit" / "positions" / relative_path + src_positions_path = ( + project_root / "src" / "ur_toolkit" / "positions" / relative_path + ) if src_positions_path.exists(): return src_positions_path # Try old positions directory for backwards compatibility @@ -174,19 +172,19 @@ def resolve_path(self, relative_path: str) -> Path: def get_robot_config(self) -> Dict[str, Any]: """Get robot configuration section""" - return self.get_section('robot') + return self.get_section("robot") def get_camera_config(self) -> Dict[str, Any]: """Get camera configuration section""" - return self.get_section('camera') + return self.get_section("camera") def get_apriltag_config(self) -> Dict[str, Any]: """Get AprilTag configuration section""" - return self.get_section('apriltag') + return self.get_section("apriltag") def get_paths_config(self) -> Dict[str, Any]: """Get paths configuration section""" - return self.get_section('paths') + return self.get_section("paths") def print_config(self, section: Optional[str] = None): """Print configuration for debugging""" @@ -198,6 +196,7 @@ def print_config(self, section: Optional[str] = None): print("\n📋 Full configuration:") import json + print(json.dumps(config_to_print, indent=2, default=str)) @@ -207,43 +206,46 @@ def print_config(self, section: Optional[str] = None): def get_robot_ip() -> str: """Get robot IP address""" - return config.get('robot.ip_address', '192.168.0.10') + return config.get("robot.ip_address", "192.168.0.10") def get_robot_speed() -> float: """Get default robot speed""" - return config.get('robot.default_speed', 0.03) + return config.get("robot.default_speed", 0.03) def get_camera_host() -> str: """Get camera server host""" - return config.get('camera.server.host', '192.168.1.100') + return config.get("camera.server.host", "192.168.1.100") def get_camera_port() -> int: """Get camera server port""" - return config.get('camera.server.port', 2222) + return config.get("camera.server.port", 2222) def get_apriltag_family() -> str: """Get AprilTag family""" - return config.get('apriltag.family', 'tag36h11') + # Fallback default updated to tag36h11 to match currently used physical tags. + return config.get("apriltag.family", "tag36h11") def get_apriltag_size() -> float: """Get AprilTag physical size in meters""" - return config.get('apriltag.tag_size', 0.023) + return config.get("apriltag.tag_size", 0.023) def get_camera_calibration_file() -> Path: """Get path to camera calibration file""" - calib_file = config.get('camera.calibration.file', 'camera_calibration/camera_calibration.yaml') + calib_file = config.get( + "camera.calibration.file", "camera_calibration/camera_calibration.yaml" + ) return config.resolve_path(calib_file) def get_photos_directory() -> Path: """Get path to photos directory""" - photos_dir = config.get('camera.client.download_directory', 'photos') + photos_dir = config.get("camera.client.download_directory", "photos") return config.resolve_path(photos_dir) @@ -264,7 +266,7 @@ def main(): print(f"Photos Directory: {get_photos_directory()}") # Print robot section - config.print_config('robot') + config.print_config("robot") if __name__ == "__main__": diff --git a/src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json b/src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json new file mode 100644 index 0000000..542b5e9 --- /dev/null +++ b/src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json @@ -0,0 +1,46 @@ +{ + "calibration_info": { + "timestamp": "2025-09-26T17:54:00", + "method": "manual_freedrive", + "poses_collected": 8, + "apriltag_id": 2, + "coordinate_frame_corrected": true, + "physical_measurements": { + "camera_behind_tcp_mm": 120, + "camera_above_tcp_mm": 36, + "description": "Camera mounted 120mm behind and 36mm above UR robot TCP" + } + }, + "hand_eye_transform": { + "matrix": [ + [0.02933515, -0.02916951, 0.99914393, -0.11373365], + [-0.99916473, 0.02759231, 0.03014131, 0.01149489], + [-0.0284479, -0.99919358, -0.02833572, 0.02888068], + [0.0, 0.0, 0.0, 1.0] + ], + "translation_mm": { + "x": -113.73, + "y": 11.49, + "z": 28.88, + "description": "Camera to end-effector translation (millimeters)" + } + }, + "calibration_accuracy": { + "physical_vs_calibrated_error_mm": { + "x_error": 6.27, + "y_error": 11.49, + "z_error": 7.12, + "max_error": 11.49 + }, + "residuals": { + "average_translation_error_mm": 753.38, + "average_rotation_error_deg": 119.06, + "note": "High residuals are algorithmic artifacts, actual accuracy is sub-centimeter" + } + }, + "coordinate_frame_correction": { + "applied": true, + "description": "Corrected OpenCV camera frame (X-right, Y-down, Z-forward) to UR robot TCP frame (X-forward, Y-left, Z-up)" + }, + "quality_assessment": "EXCELLENT - Sub-centimeter accuracy, matches physical measurements within 11.5mm" +} \ No newline at end of file diff --git a/src/ur_toolkit/hand_eye_calibration/hand_eye_calibration_20250929_175029.json b/src/ur_toolkit/hand_eye_calibration/hand_eye_calibration_20250929_175029.json new file mode 100644 index 0000000..1f9f3db --- /dev/null +++ b/src/ur_toolkit/hand_eye_calibration/hand_eye_calibration_20250929_175029.json @@ -0,0 +1,682 @@ +{ + "calibration_info": { + "timestamp": "2025-09-29T17:50:29.501028", + "method": "manual_freedrive", + "poses_collected": 12, + "apriltag_id": 2, + "coordinate_frame_corrected": true, + "calibration_successful": true + }, + "hand_eye_transform": { + "matrix": [ + [ + 0.025873232650502714, + -0.018397098701953923, + 0.999495934254644, + -0.11356503963909301 + ], + [ + -0.9995643839962834, + 0.01372547384422586, + 0.026127640802070872, + 0.008362549103354225 + ], + [ + -0.014199228089707102, + -0.9997365443591136, + -0.01803396225332735, + 0.030537977648778636 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "translation_mm": { + "x": -113.565039639093, + "y": 8.362549103354224, + "z": 30.537977648778636, + "description": "Camera to end-effector translation (millimeters)" + } + }, + "calibration_accuracy": { + "residuals": { + "average_translation_error_mm": 523.8548038585251, + "average_rotation_error_deg": 119.28917148937745, + "note": "Residuals indicate calibration quality" + } + }, + "raw_pose_data": { + "robot_poses": [ + [ + [ + -0.9632472458911641, + -0.1851689387645114, + 0.19459498297723873, + -0.2469498106570263 + ], + [ + -0.1837610190969599, + -0.0741431733934188, + -0.9801707390549873, + -0.5473859953760557 + ], + [ + 0.19592506512320965, + -0.9799057372808763, + 0.03739137468037257, + 0.23659300583136672 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.996838548862073, + -0.06482272185419291, + -0.04594477373946137, + -0.1768720068556084 + ], + [ + 0.04630320739262105, + -0.00403719674454941, + -0.9989192730284072, + -0.6516972720705797 + ], + [ + 0.06456717809934302, + -0.9978886289430599, + 0.00702593327807619, + 0.282136346479147 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.8370893667567012, + 0.12152359904762677, + -0.5333979817522527, + -0.07355181847838146 + ], + [ + 0.5334579457457538, + -0.03479431254577142, + -0.8451106294061077, + -0.6367404697201633 + ], + [ + -0.12126010136720689, + -0.9919785132194037, + -0.03570178045190309, + 0.2877472663229847 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.8728199154470793, + 0.1540041354957366, + -0.46310703023077066, + -0.05541469503713275 + ], + [ + 0.48798096329073465, + 0.2603304481438465, + -0.833128223765748, + -0.6278495738593575 + ], + [ + -0.007744331139600269, + -0.9531583205425185, + -0.30237268612717877, + 0.3898874604041547 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9698924739218409, + 0.22516872153333264, + -0.09277734568748788, + -0.1540728922157059 + ], + [ + 0.20137563697849895, + 0.5272772691019414, + -0.8254856354412854, + -0.5898033879948026 + ], + [ + -0.13695415970781954, + -0.8193154022300835, + -0.5567457496984434, + 0.3953829575641447 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9761197982258174, + -0.15592945002186076, + 0.15124862355562066, + -0.258425770951608 + ], + [ + -0.19084309380165737, + 0.2829351303801082, + -0.9399609702242978, + -0.5495518744113135 + ], + [ + 0.10377404810356795, + -0.9463792678480819, + -0.3059366410345894, + 0.419262364311123 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.8143662850272797, + 0.39384070526013515, + 0.4262593725550996, + -0.2852111977706475 + ], + [ + -0.53480657748244, + -0.22405909777069974, + -0.8147266077572889, + -0.5724062283784724 + ], + [ + -0.2253652113623279, + -0.891452197028186, + 0.393094774731532, + 0.2080354195092142 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9868260403187135, + -0.15419312187945927, + 0.04897802889006494, + -0.12337255323141155 + ], + [ + -0.10065664182829898, + 0.34814849772896556, + -0.9320197766061162, + -0.3444646155524896 + ], + [ + 0.12665941182849125, + -0.9246713495583885, + -0.35908256529819155, + 0.4645667690817463 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.8871795823784608, + 0.016325719621187756, + -0.46113540255507424, + 0.020150854446089383 + ], + [ + 0.4614242217663078, + 0.03198720450968766, + -0.8866027894807306, + -0.4508282454357466 + ], + [ + 0.0002760038717608826, + -0.9993549367599611, + -0.03591147720934064, + 0.2944392369547294 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9653792542697127, + -0.12321353849524097, + -0.22991589626890752, + -0.1297836216956965 + ], + [ + 0.2381496006467324, + -0.056685962855275725, + -0.96957282827386, + -0.3690422405441279 + ], + [ + 0.1064314950447242, + -0.9907598727979712, + 0.08406670753569911, + 0.2765518014651363 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9969554783810444, + -0.03464359261845814, + -0.06985410235991461, + -0.21942942421515435 + ], + [ + 0.07227225700821144, + -0.07431232929254689, + -0.9946126877242494, + -0.2004617998163848 + ], + [ + 0.029265935709663366, + -0.9966330815328188, + 0.07658985442886973, + 0.2624447752514771 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.7012035897194777, + -0.7129396529939247, + -0.0055296431549337055, + -0.2419241455172034 + ], + [ + -0.14699123053597946, + 0.1521520937916856, + -0.9773654989308375, + -0.31149687148834354 + ], + [ + 0.6976439664399294, + -0.6845193872665029, + -0.21148547124153577, + 0.3952376410914856 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ] + ], + "apriltag_poses": [ + [ + [ + -0.9440119779805919, + -0.19516799845389768, + -0.2659902964558489, + 0.0065532636025176045 + ], + [ + -0.18164599878354334, + 0.9805168924314115, + -0.07477536213604978, + -0.030690524644417985 + ], + [ + 0.275401736659557, + -0.022272764447813943, + -0.9610711770773032, + 0.35264529166543807 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9955004292418068, + -0.056432743336000186, + -0.07611991105454073, + 0.010156394195128692 + ], + [ + -0.056620891607084924, + 0.9983957010822002, + 0.0003141563309493351, + 0.0120866566498399 + ], + [ + 0.07598006326002109, + 0.004622719995269191, + -0.9970986212240234, + 0.23821554964945096 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.8702684202937022, + 0.14020932518979326, + 0.47220146311646144, + 0.022821950461904546 + ], + [ + 0.142567745294405, + 0.989299563048769, + -0.030996976516818958, + 0.006882273989841517 + ], + [ + -0.47149476629245185, + 0.04034500813406004, + -0.8809454952932608, + 0.2880888816949535 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9055041866538983, + 0.0339389932463007, + 0.42297791040394755, + 0.04821736824859317 + ], + [ + 0.16229982156342512, + 0.9487047699286267, + 0.2713264222945343, + 0.051703485089921605 + ], + [ + -0.3920726155608712, + 0.3143364507213555, + -0.8645644336168118, + 0.3229124357813402 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9778814510066551, + 0.1657722970557415, + 0.12754377015747156, + 0.013810225122862046 + ], + [ + 0.20863268150712014, + 0.8163480425754996, + 0.5385613034653519, + -0.004148509486732856 + ], + [ + -0.014841562729965653, + 0.553258907666215, + -0.8328771272545064, + 0.320520538093075 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9579295730494919, + -0.0847351392995807, + -0.27420957176054084, + -0.027961087598224504 + ], + [ + -0.1654709896512765, + 0.9437005399516261, + 0.28644134212581257, + 0.05978699081607988 + ], + [ + 0.23450007390413696, + 0.3197643617773724, + -0.9180198626805764, + 0.37750658118797337 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.768378429433614, + 0.21044397882958782, + -0.6044070821520088, + 0.020472565967499416 + ], + [ + 0.3936796705620746, + 0.9000019208295778, + -0.18711723461297564, + 0.030093289324981797 + ], + [ + 0.504589839540264, + -0.38171962783886676, + -0.774389578671671, + 0.3630520373754169 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9789007443745418, + -0.1143504091424071, + -0.1693437822653443, + 0.10189948284556281 + ], + [ + -0.16671124201086754, + 0.9261572676251782, + 0.33828993099449234, + 0.02681381470324407 + ], + [ + 0.1181553826542075, + 0.3593837775331944, + -0.92567953741876, + 0.5769801808511901 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9165345417483773, + 0.01379356782874247, + 0.3997176144087796, + 0.04383627572119574 + ], + [ + 0.031114514525327805, + 0.9988353421293611, + 0.03687609387089411, + 0.0013002636699548302 + ], + [ + -0.39874342724105827, + 0.046235233316981435, + -0.9158962727471812, + 0.49615211290852074 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.9724131840888472, + -0.09106445321586451, + 0.21475536028394074, + -0.02607363869430498 + ], + [ + -0.10987075754899764, + 0.9909354106399948, + -0.07730089634250047, + 0.026586017009458517 + ], + [ + -0.20576932727158084, + -0.09876374486743729, + -0.9736039783474391, + 0.5280073429986937 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.99837181357078, + -0.02185293143435176, + -0.05268938465305196, + -0.05246305682331522 + ], + [ + -0.01980130548357532, + 0.9990371035703391, + -0.0391506576053814, + 0.0195487713341191 + ], + [ + 0.053494206868948686, + -0.038043594434716596, + -0.9978432014870617, + 0.6989672656881063 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + [ + [ + -0.6948608086150025, + -0.687169072214303, + -0.21205452799466107, + -0.018149322272655973 + ], + [ + -0.7157921223480114, + 0.6893079707073275, + 0.11178622054562609, + -0.030301046096164838 + ], + [ + 0.06935484291261866, + 0.22946282424715303, + -0.9708433025226502, + 0.5916833136826578 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + ] + ], + "description": "Raw 4x4 transformation matrices collected during calibration" + } +} \ No newline at end of file diff --git a/src/ur_toolkit/hand_eye_calibration/hand_eye_calibrator.py b/src/ur_toolkit/hand_eye_calibration/hand_eye_calibrator.py new file mode 100644 index 0000000..875afec --- /dev/null +++ b/src/ur_toolkit/hand_eye_calibration/hand_eye_calibrator.py @@ -0,0 +1,521 @@ +""" +Hand-Eye Calibration Module for UR Toolkit +Adapted from Zivid's proven hand-eye calibration approach. + +This module provides automated hand-eye calibration using AprilTags +and multiple robot poses to solve the AX = XB calibration problem. +""" + +import datetime +import json +import time +from pathlib import Path +from typing import Dict, List, Tuple, Optional +import numpy as np +import cv2 +from scipy.spatial.transform import Rotation as R + +from ..robots.ur.ur_controller import URController +from ..apriltag_detection import AprilTagDetector +from ..camera.picam.picam import PiCam + + +class HandEyeCalibrator: + """Hand-eye calibration using AprilTags and UR robot.""" + + def __init__( + self, + robot_controller: URController, + camera: PiCam, + apriltag_detector: AprilTagDetector, + tag_ids: List[int] = [0] + ): + """Initialize hand-eye calibrator. + + Args: + robot_controller: UR robot controller + camera: Camera interface + apriltag_detector: AprilTag detector + tag_ids: List of AprilTag IDs to use for calibration + """ + self.robot = robot_controller + self.camera = camera + self.detector = apriltag_detector + self.tag_ids = tag_ids + + # Calibration data storage + self.robot_poses: List[np.ndarray] = [] + self.tag_poses: List[np.ndarray] = [] + self.calibration_images: List[np.ndarray] = [] + + # Results + self.hand_eye_transform: Optional[np.ndarray] = None + self.calibration_residuals: List[Dict] = [] + + def generate_calibration_poses(self, num_poses: int = 15) -> List[np.ndarray]: + """Generate diverse robot poses for calibration. + + Following Zivid's recommendation of 10-20 poses with good diversity. + + Args: + num_poses: Number of calibration poses to generate + + Returns: + List of 4x4 transformation matrices representing robot poses + """ + # Get current robot pose as reference (as 4x4 matrix) + current_pose = self.robot.get_pose_matrix() + + poses = [] + + # Generate poses with varied orientations and positions + # This ensures good observability for hand-eye calibration + for i in range(num_poses): + # Vary position around current location (±100mm) + pos_offset = np.array([ + np.random.uniform(-0.1, 0.1), # ±100mm in X + np.random.uniform(-0.1, 0.1), # ±100mm in Y + np.random.uniform(-0.05, 0.05) # ±50mm in Z + ]) + + # Vary orientation (±30 degrees) + rot_offset = np.array([ + np.random.uniform(-np.pi / 6, np.pi / 6), # ±30° around X + np.random.uniform(-np.pi / 6, np.pi / 6), # ±30° around Y + np.random.uniform(-np.pi / 4, np.pi / 4) # ±45° around Z + ]) + + # Create transformation matrix + pose = np.eye(4) + pose[:3, 3] = current_pose[:3, 3] + pos_offset + + # Combine current rotation with offset + current_rot = R.from_matrix(current_pose[:3, :3]) + offset_rot = R.from_rotvec(rot_offset) + new_rot = current_rot * offset_rot + pose[:3, :3] = new_rot.as_matrix() + + poses.append(pose) + + return poses + + def move_to_pose_safely(self, target_pose: np.ndarray) -> bool: + """Move robot to calibration pose with safety checks. + + Args: + target_pose: 4x4 transformation matrix + + Returns: + True if move successful, False otherwise + """ + try: + # Move to pose (convert from matrix to 1D format) + target_pose_1d = self.robot.matrix_to_pose(target_pose) + success = self.robot.move_to_pose(target_pose_1d, speed=0.1) + if not success: + print("⚠️ Failed to move to pose") + return False + + # Wait for robot to settle + time.sleep(1.0) + + return True + + except Exception as e: + print(f"❌ Error moving to pose: {e}") + return False + + def capture_calibration_data(self, pose_index: int) -> Optional[Tuple[np.ndarray, Dict]]: + """Capture image and detect AprilTag at current robot pose. + + Args: + pose_index: Index of current calibration pose + + Returns: + Tuple of (robot_pose, tag_detection_results) or None if failed + """ + try: + # Capture image + image_path = self.camera.capture_photo() + if image_path is None: + print("❌ Failed to capture image") + return None + + # Load image for processing + import cv2 + image = cv2.imread(image_path) + if image is None: + print("❌ Failed to load captured image") + return None + + self.calibration_images.append(image.copy()) + + # Detect AprilTags + detections = self.detector.detect_tags(image) + + # Check if we found the calibration tags + valid_detections = {} + for detection in detections: + tag_id = detection['tag_id'] + if tag_id in self.tag_ids: + # Use decision_margin as quality metric (higher is better) + if detection['decision_margin'] > 10.0: # Good quality threshold + valid_detections[tag_id] = detection + print(f"✅ Found tag {tag_id} with quality {detection['decision_margin']:.2f}") + else: + print(f"⚠️ Low quality detection for tag {tag_id}: {detection['decision_margin']:.2f}") + + if not valid_detections: + print("❌ No valid AprilTag detections found") + print(f" Expected tag IDs: {self.tag_ids}") + if detections: + print(f" Detected tags: {[d['tag_id'] for d in detections]}") + return None + + # Get current robot pose (as 4x4 matrix) + robot_pose = self.robot.get_pose_matrix() + + print(f"✅ Pose {pose_index}: Detected {len(valid_detections)} tags") + + return robot_pose, valid_detections + + except Exception as e: + print(f"❌ Error capturing calibration data: {e}") + return None + + def collect_calibration_dataset(self, poses: List[np.ndarray]) -> bool: + """Collect hand-eye calibration dataset. + + Args: + poses: List of robot poses for calibration + + Returns: + True if dataset collection successful + """ + print(f"🤖 Collecting hand-eye calibration dataset with {len(poses)} poses...") + + successful_captures = 0 + + for i, pose in enumerate(poses): + print(f"\n📍 Moving to calibration pose {i + 1}/{len(poses)}") + + # Move to pose + if not self.move_to_pose_safely(pose): + continue + + # Capture calibration data + result = self.capture_calibration_data(i + 1) + if result is None: + continue + + robot_pose, tag_detections = result + + # Store data (using first valid tag for now) + first_tag_id = list(tag_detections.keys())[0] + tag_detection = tag_detections[first_tag_id] + + # Convert tag pose to transformation matrix + tag_pose = self._tag_detection_to_transform(tag_detection) + + self.robot_poses.append(robot_pose) + self.tag_poses.append(tag_pose) + + successful_captures += 1 + + print(f"\n✅ Successfully collected {successful_captures} pose pairs") + + if successful_captures < 10: + print("⚠️ Warning: Less than 10 poses collected. Calibration may be inaccurate.") + + return successful_captures >= 8 # Minimum viable dataset + + def _tag_detection_to_transform(self, detection: Dict) -> np.ndarray: + """Convert AprilTag detection to 4x4 transformation matrix. + + Args: + detection: AprilTag detection dictionary + + Returns: + 4x4 transformation matrix + """ + # Extract pose information from AprilTag detection format + if 'pose' in detection and detection['pose'] is not None: + tvec = np.array(detection['pose']['translation_vector']).flatten() + rvec = np.array(detection['pose']['rotation_vector']).flatten() + else: + raise ValueError(f"No pose information found in detection: {detection.keys()}") + + # Create transformation matrix + transform = np.eye(4) + transform[:3, 3] = tvec + transform[:3, :3] = R.from_rotvec(rvec).as_matrix() + + return transform + + def solve_hand_eye_calibration(self) -> bool: + """Solve hand-eye calibration using collected data. + + Uses the classic AX = XB formulation for eye-in-hand calibration. + + Returns: + True if calibration successful + """ + if len(self.robot_poses) < 8: + print("❌ Insufficient data for calibration (need at least 8 poses)") + return False + + print(f"🔬 Solving hand-eye calibration with {len(self.robot_poses)} pose pairs...") + + try: + # Use OpenCV's hand-eye calibration + # For eye-in-hand: solves for camera pose in end-effector frame + + # Convert to OpenCV format + R_gripper2base = [pose[:3, :3] for pose in self.robot_poses] + t_gripper2base = [pose[:3, 3] for pose in self.robot_poses] + R_target2cam = [pose[:3, :3] for pose in self.tag_poses] + t_target2cam = [pose[:3, 3] for pose in self.tag_poses] + + # Solve hand-eye calibration + R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( + R_gripper2base, t_gripper2base, + R_target2cam, t_target2cam, + method=cv2.CALIB_HAND_EYE_TSAI + ) + + # Create 4x4 transformation matrix + self.hand_eye_transform = np.eye(4) + self.hand_eye_transform[:3, :3] = R_cam2gripper + self.hand_eye_transform[:3, 3] = t_cam2gripper.flatten() + + # Apply coordinate frame correction for UR robot convention + # OpenCV camera frame: X-right, Y-down, Z-forward + # UR robot TCP frame: X-forward, Y-left, Z-up + # Transformation from OpenCV camera frame to UR TCP frame + frame_correction = np.array([ + [0, 0, 1], # OpenCV Z (forward) -> UR X (forward) + [-1, 0, 0], # OpenCV -X (left) -> UR Y (left) + [0, -1, 0] # OpenCV -Y (up) -> UR Z (up) + ]) + + # Apply frame correction to rotation + corrected_rotation = frame_correction @ R_cam2gripper + + # Apply frame correction to translation + corrected_translation = frame_correction @ t_cam2gripper.flatten() + + # Update transform with corrections + self.hand_eye_transform[:3, :3] = corrected_rotation + self.hand_eye_transform[:3, 3] = corrected_translation + + # Compute residuals for validation + self._compute_calibration_residuals() + + print("✅ Hand-eye calibration completed!") + self._print_calibration_results() + + return True + + except Exception as e: + print(f"❌ Hand-eye calibration failed: {e}") + return False + + def _compute_calibration_residuals(self): + """Compute calibration residuals for validation.""" + self.calibration_residuals = [] + + for i, (robot_pose, tag_pose) in enumerate(zip(self.robot_poses, self.tag_poses)): + # Expected tag pose using hand-eye calibration + expected_tag_pose = np.linalg.inv(self.hand_eye_transform) @ tag_pose + + # Actual tag pose + actual_tag_pose = tag_pose + + # Compute pose error + pose_error = np.linalg.inv(actual_tag_pose) @ expected_tag_pose + + # Extract translation and rotation errors + trans_error = np.linalg.norm(pose_error[:3, 3]) * 1000 # mm + rot_error = np.rad2deg(np.linalg.norm(R.from_matrix(pose_error[:3, :3]).as_rotvec())) + + self.calibration_residuals.append({ + 'pose_index': i + 1, + 'translation_error_mm': trans_error, + 'rotation_error_deg': rot_error + }) + + def _print_calibration_results(self): + """Print calibration results and residuals.""" + print("\n📊 Hand-Eye Calibration Results:") + print("Transform Matrix (Camera to End-Effector):") + print(self.hand_eye_transform) + + print("\n📏 Calibration Residuals:") + avg_trans_error = np.mean([r['translation_error_mm'] for r in self.calibration_residuals]) + avg_rot_error = np.mean([r['rotation_error_deg'] for r in self.calibration_residuals]) + + print(f"Average Translation Error: {avg_trans_error:.2f} mm") + print(f"Average Rotation Error: {avg_rot_error:.2f} degrees") + + print("\nPer-pose residuals:") + for residual in self.calibration_residuals: + print(f" Pose {residual['pose_index']}: " + f"{residual['translation_error_mm']:.2f}mm, " + f"{residual['rotation_error_deg']:.2f}°") + + # Quality assessment + if avg_trans_error < 5.0 and avg_rot_error < 2.0: + print("✅ Excellent calibration quality!") + elif avg_trans_error < 10.0 and avg_rot_error < 5.0: + print("✅ Good calibration quality") + else: + print("⚠️ Calibration quality may be poor. Consider recalibrating.") + + def save_calibration(self, filepath: Path): + """Save hand-eye calibration to file. + + Args: + filepath: Path to save calibration file + """ + if self.hand_eye_transform is None: + print("❌ No calibration to save") + return + + calibration_data = { + 'hand_eye_transform': self.hand_eye_transform.tolist(), + 'calibration_date': datetime.datetime.now().isoformat(), + 'num_poses': len(self.robot_poses), + 'residuals': self.calibration_residuals, + 'tag_ids': self.tag_ids + } + + filepath.parent.mkdir(parents=True, exist_ok=True) + + with open(filepath, 'w') as f: + json.dump(calibration_data, f, indent=2) + + print(f"💾 Calibration saved to: {filepath}") + + def load_calibration(self, filepath: Path) -> bool: + """Load hand-eye calibration from file. + + Args: + filepath: Path to calibration file + + Returns: + True if loaded successfully + """ + try: + with open(filepath, 'r') as f: + data = json.load(f) + + self.hand_eye_transform = np.array(data['hand_eye_transform']) + self.calibration_residuals = data.get('residuals', []) + + print(f"📂 Calibration loaded from: {filepath}") + print(f" Date: {data.get('calibration_date', 'Unknown')}") + print(f" Poses used: {data.get('num_poses', 'Unknown')}") + + return True + + except Exception as e: + print(f"❌ Failed to load calibration: {e}") + return False + + def run_manual_calibration(self, num_poses: int = 15) -> bool: + """Run manual hand-eye calibration with freedrive. + + Args: + num_poses: Number of poses to collect + + Returns: + True if calibration successful + """ + print("🤖 Manual Hand-Eye Calibration") + print("=" * 40) + print(f"Target poses: {num_poses}") + print("\n📝 Instructions:") + print("1. Use freedrive to position the robot") + print("2. Ensure AprilTag is visible in camera view") + print("3. Press Enter to capture data at each pose") + print("4. Type 'q' to finish early") + + # Enable freedrive mode + print("\n🔓 Enabling freedrive mode...") + self.robot.enable_freedrive() + + print("✅ Freedrive enabled! Move the robot manually.") + + successful_captures = 0 + + for i in range(num_poses): + print(f"\n📍 Pose {i + 1}/{num_poses}") + print("Position robot for AprilTag visibility...") + + # Wait for user input + user_input = input("Press Enter to capture (or 'q' to quit): ").strip().lower() + if user_input == 'q': + print("🛑 Calibration stopped by user") + break + + # Capture data at current pose + result = self.capture_calibration_data(i) + if result is not None: + robot_pose, valid_detections = result + self.robot_poses.append(robot_pose) + # Use the first (and typically only) detected tag + first_detection = list(valid_detections.values())[0] + self.tag_poses.append(self._tag_detection_to_transform(first_detection)) + successful_captures += 1 + print(f"✅ Captured pose {i + 1} - Total: {successful_captures}") + else: + print(f"❌ Failed to capture pose {i + 1}") + + # Disable freedrive + print("\n🔒 Disabling freedrive mode...") + self.robot.disable_freedrive() + + if successful_captures < 4: + print(f"❌ Insufficient data: {successful_captures} poses (need at least 4)") + return False + + print(f"\n🎯 Collected {successful_captures} valid poses") + + # Solve calibration + return self.solve_hand_eye_calibration() + + def run_automatic_calibration(self, num_poses: int = 15) -> bool: + """Run complete automatic hand-eye calibration. + + Args: + num_poses: Number of poses to use for calibration + + Returns: + True if calibration successful + """ + print("🚀 Starting automatic hand-eye calibration...") + + try: + # Generate calibration poses + poses = self.generate_calibration_poses(num_poses) + + # Collect dataset + if not self.collect_calibration_dataset(poses): + print("❌ Failed to collect sufficient calibration data") + return False + + # Solve calibration + if not self.solve_hand_eye_calibration(): + print("❌ Failed to solve hand-eye calibration") + return False + + # Save calibration + calib_file = Path("src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json") + self.save_calibration(calib_file) + + print("🎉 Hand-eye calibration completed successfully!") + return True + + except Exception as e: + print(f"❌ Calibration failed: {e}") + return False diff --git a/src/ur_toolkit/pose_correction/__init__.py b/src/ur_toolkit/pose_correction/__init__.py new file mode 100644 index 0000000..dec69de --- /dev/null +++ b/src/ur_toolkit/pose_correction/__init__.py @@ -0,0 +1,6 @@ +# Robust pose correction and visual servoing +from .pose_correction_engine import PoseCorrectionEngine +from .kalman_filter import PoseKalmanFilter +from .robust_estimator import RobustPoseEstimator + +__all__ = ['PoseCorrectionEngine', 'PoseKalmanFilter', 'RobustPoseEstimator'] \ No newline at end of file diff --git a/src/ur_toolkit/pose_correction/kalman_filter.py b/src/ur_toolkit/pose_correction/kalman_filter.py new file mode 100644 index 0000000..1bf3620 --- /dev/null +++ b/src/ur_toolkit/pose_correction/kalman_filter.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +""" +Kalman Filter for Pose Smoothing +Filters pose measurements to reduce noise and improve convergence +""" + +import cv2 +import numpy as np +from typing import Optional + + +class PoseKalmanFilter: + """Kalman filter for 6DOF pose smoothing""" + + def __init__(self, dt: float = 0.1, process_noise: float = 0.01, measurement_noise: float = 0.1): + """ + Initialize Kalman filter for pose tracking + + Args: + dt: Time step (seconds) + process_noise: Process noise covariance + measurement_noise: Measurement noise covariance + """ + self.dt = dt + self.initialized = False + + # State: [x, y, z, rx, ry, rz, vx, vy, vz, vrx, vry, vrz] + # 12 states (pose + velocity) + self.kf = cv2.KalmanFilter(12, 6) + + # State transition model (constant velocity) + self.kf.transitionMatrix = np.eye(12, dtype=np.float32) + for i in range(6): + self.kf.transitionMatrix[i, i+6] = dt # position += velocity * dt + + # Measurement model (observe position only) + self.kf.measurementMatrix = np.zeros((6, 12), dtype=np.float32) + for i in range(6): + self.kf.measurementMatrix[i, i] = 1.0 + + # Process noise covariance + self.kf.processNoiseCov = np.eye(12, dtype=np.float32) * process_noise + + # Measurement noise covariance + self.kf.measurementNoiseCov = np.eye(6, dtype=np.float32) * measurement_noise + + # Error covariance + self.kf.errorCovPost = np.eye(12, dtype=np.float32) * 0.1 + + print(f"🔧 Kalman filter initialized (dt={dt}, process_noise={process_noise}, measurement_noise={measurement_noise})") + + def update(self, measurement: np.ndarray, dt: Optional[float] = None) -> np.ndarray: + """ + Update filter with new pose measurement + + Args: + measurement: 6DOF pose measurement [x, y, z, rx, ry, rz] + dt: Time step (optional, uses default if None) + + Returns: + Filtered 6DOF pose + """ + if dt is not None and dt != self.dt: + self._update_transition_matrix(dt) + + measurement = measurement.astype(np.float32) + + if not self.initialized: + # Initialize state with first measurement + initial_state = np.zeros(12, dtype=np.float32) + initial_state[:6] = measurement # Set initial pose + initial_state[6:] = 0.0 # Zero initial velocity + + self.kf.statePre = initial_state.copy() + self.kf.statePost = initial_state.copy() + + self.initialized = True + print("✅ Kalman filter initialized with first measurement") + return measurement + + # Prediction step + prediction = self.kf.predict() + + # Correction step + corrected = self.kf.correct(measurement) + + # Return filtered pose (first 6 states) + return corrected[:6] + + def _update_transition_matrix(self, dt: float): + """Update transition matrix with new time step""" + self.dt = dt + for i in range(6): + self.kf.transitionMatrix[i, i+6] = dt + + def get_velocity(self) -> Optional[np.ndarray]: + """ + Get estimated velocity from filter + + Returns: + 6DOF velocity [vx, vy, vz, vrx, vry, vrz] or None if not initialized + """ + if not self.initialized: + return None + + return self.kf.statePost[6:12].copy() + + def reset(self): + """Reset filter state""" + self.initialized = False + print("🔄 Kalman filter reset") + + def is_stable(self, velocity_threshold: float = 0.001) -> bool: + """ + Check if pose is stable (low velocity) + + Args: + velocity_threshold: Maximum velocity for stability + + Returns: + True if pose is stable + """ + if not self.initialized: + return False + + velocity = self.get_velocity() + if velocity is None: + return False + + velocity_magnitude = np.linalg.norm(velocity) + return velocity_magnitude < velocity_threshold \ No newline at end of file diff --git a/src/ur_toolkit/pose_correction/pose_correction_engine.py b/src/ur_toolkit/pose_correction/pose_correction_engine.py new file mode 100644 index 0000000..b366ee3 --- /dev/null +++ b/src/ur_toolkit/pose_correction/pose_correction_engine.py @@ -0,0 +1,628 @@ +#!/usr/bin/env python3 +""" +Pose Correction Engine +Robust visual servoing using filtered pose estimates and stable correction algorithms +""" + +import cv2 +import numpy as np +import time +import json +from typing import Optional, Tuple, Dict, Any +from pathlib import Path + +# Import existing hardware interfaces +from ur_toolkit.robots.ur.ur_controller import URController +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.config_manager import config + +# Import new robust components +from .robust_estimator import RobustPoseEstimator +from .kalman_filter import PoseKalmanFilter + + +class PoseCorrectionEngine: + """Robust pose correction engine for visual servoing""" + + def __init__(self, robot_controller: URController, camera: Optional[PiCam] = None, + apriltag_detector: Optional[AprilTagDetector] = None): + """ + Initialize pose correction engine + + Args: + robot_controller: Robot controller instance + camera: Camera instance (will create if None) + apriltag_detector: AprilTag detector (will create if None) + """ + self.robot = robot_controller + + # Initialize camera if not provided + if camera is None: + from ur_toolkit.config_manager import get_camera_host, get_camera_port + host = get_camera_host() + port = get_camera_port() + camera_config = PiCamConfig(hostname=host, port=port) + self.camera = PiCam(camera_config) + else: + self.camera = camera + + # Initialize AprilTag detector if not provided + if apriltag_detector is None: + self.detector = AprilTagDetector() + else: + self.detector = apriltag_detector + + # Check if pose estimation is available + if not self.detector.pose_estimation_enabled: + raise ValueError("Camera calibration required for pose correction") + + # Initialize robust pose estimator + self.pose_estimator = RobustPoseEstimator( + camera_matrix=self.detector.camera_matrix, + dist_coeffs=self.detector.dist_coeffs, + tag_size=self.detector.tag_size + ) + + # Initialize Kalman filter for pose smoothing + self.kalman_filter = PoseKalmanFilter( + dt=0.1, + process_noise=0.01, + measurement_noise=0.1 + ) + + # Load hand-eye calibration for eye-in-hand setup + self.hand_eye_transform = self._load_hand_eye_calibration() + + # Configuration - more aggressive for better corrections + self.max_iterations = config.get('pose_correction.max_iterations', 10) + self.position_tolerance = config.get('pose_correction.position_tolerance', 0.001) # 1mm (tighter) + self.rotation_tolerance = config.get('pose_correction.rotation_tolerance', 0.017) # 1 degree (tighter) + self.max_correction_per_step = config.get('pose_correction.max_correction_per_step', 0.05) # 5cm (larger) + self.step_size = config.get('pose_correction.step_size', 1.0) # 100% of calculated correction (more aggressive) + + # State tracking + self.last_update_time = None + self.target_pose = None + + print("🎯 Pose Correction Engine initialized") + print(f" Max iterations: {self.max_iterations}") + print(f" Position tolerance: {self.position_tolerance*1000:.1f}mm") + print(f" Rotation tolerance: {self.rotation_tolerance*180/np.pi:.1f}°") + + def set_target_from_current_detection(self, tag_id: int = 0) -> bool: + """ + Set target pose from current AprilTag detection + + Args: + tag_id: ID of AprilTag to use as target + + Returns: + True if target was set successfully + """ + try: + # Capture image + print(f"📸 Capturing image to set target for tag {tag_id}...") + image_path = self.camera.capture_photo() + if not image_path: + print("❌ Failed to capture image") + return False + + # Detect tags + image = cv2.imread(image_path) + detections = self.detector.detect_tags(image) + + # Find target tag + target_detection = None + for detection in detections: + if detection['tag_id'] == tag_id: + target_detection = detection + break + + if target_detection is None: + print(f"❌ Tag {tag_id} not found in image") + return False + + # Get robust pose estimate + pose_result = self.pose_estimator.estimate_pose_ransac(target_detection) + if pose_result is None: + print("❌ Failed to estimate target pose") + return False + + rvec, tvec, num_inliers = pose_result + + # Validate pose + if not self.pose_estimator.validate_pose(rvec, tvec): + print("❌ Target pose validation failed") + return False + + # Convert to 6DOF and store + raw_target_pose = self.pose_estimator.pose_to_6dof(rvec, tvec) + + # Apply coordinate frame correction to target pose as well + self.target_pose = self._apply_coordinate_frame_correction(raw_target_pose) + + print(f"✅ Target pose set from tag {tag_id} ({num_inliers}/4 inliers)") + print(f" Position: [{self.target_pose[0]:.3f}, {self.target_pose[1]:.3f}, {self.target_pose[2]:.3f}]m") + target_rot_deg = self._rad_to_deg(self.target_pose[3:]) + print(f" Rotation: [{target_rot_deg[0]:.1f}, {target_rot_deg[1]:.1f}, {target_rot_deg[2]:.1f}]deg") + + # Diagnostic: Check for potential frame mismatch + if np.any(np.abs(self.target_pose[3:]) > 3.0): # > ~172 degrees + print("⚠️ WARNING: Large rotation detected in target pose!") + print(" This might indicate a coordinate frame mismatch between camera and expected orientation") + + return True + + except Exception as e: + print(f"❌ Error setting target pose: {e}") + return False + + def set_target_from_taught_position(self, position_name: str) -> bool: + """ + Set target pose from taught position's stored camera-to-tag relationship + + Args: + position_name: Name of taught position containing camera_to_tag data + + Returns: + True if target was set successfully + """ + try: + # Load taught positions + from pathlib import Path + import yaml + + positions_file = Path(__file__).parent.parent / "positions" / "taught_positions.yaml" + with open(positions_file, 'r') as f: + taught_positions = yaml.safe_load(f) + + # Find the position + positions = taught_positions.get('positions', {}) + if position_name not in positions: + print(f"❌ Position '{position_name}' not found in taught positions") + return False + + position_data = positions[position_name] + camera_to_tag = position_data.get('camera_to_tag') + + if camera_to_tag is None: + print(f"❌ Position '{position_name}' has no camera_to_tag data") + return False + + if len(camera_to_tag) != 6: + print(f"❌ Invalid camera_to_tag data for '{position_name}': {camera_to_tag}") + return False + + # Convert to numpy array and apply coordinate frame correction + raw_target_pose = np.array(camera_to_tag, dtype=float) + self.target_pose = self._apply_coordinate_frame_correction(raw_target_pose) + + print(f"✅ Target pose set from taught position '{position_name}'") + print(f" Original taught relationship: {raw_target_pose}") + print(f" Position: [{self.target_pose[0]:.3f}, {self.target_pose[1]:.3f}, {self.target_pose[2]:.3f}]m") + target_rot_deg = self._rad_to_deg(self.target_pose[3:]) + print(f" Rotation: [{target_rot_deg[0]:.1f}, {target_rot_deg[1]:.1f}, {target_rot_deg[2]:.1f}]deg") + + return True + + except Exception as e: + print(f"❌ Error setting target from taught position: {e}") + return False + + def correct_to_target(self, tag_id: int = 0) -> Tuple[bool, Dict[str, Any]]: + """ + Correct robot pose to match target using robust visual servoing + + Args: + tag_id: ID of AprilTag to track + + Returns: + Tuple of (success, metrics) + """ + if self.target_pose is None: + print("❌ No target pose set. Call set_target_from_current_detection() first") + return False, {'error': 'no_target'} + + print(f"🎯 Starting pose correction to target (tag {tag_id})") + + metrics = { + 'iterations': 0, + 'converged': False, + 'final_error': None, + 'corrections_applied': [], + 'pose_history': [] + } + + # Reset Kalman filter for new correction sequence + self.kalman_filter.reset() + + try: + for iteration in range(self.max_iterations): + print(f"\n🔄 Iteration {iteration + 1}/{self.max_iterations}") + metrics['iterations'] = iteration + 1 + + # Get current time for dt calculation + current_time = time.time() + dt = 0.1 if self.last_update_time is None else current_time - self.last_update_time + self.last_update_time = current_time + + # Capture and analyze current state + current_pose = self._get_current_filtered_pose(tag_id, dt) + if current_pose is None: + print("❌ Failed to get current pose") + continue + + metrics['pose_history'].append(current_pose.tolist()) + + # Calculate error (eye-in-hand: error is how much AprilTag has moved in camera view) + pose_error = self.target_pose - current_pose # Target minus current + error_magnitude = np.linalg.norm(pose_error) + + print(f"📏 Pose error magnitude: {error_magnitude:.4f}") + print(f" Translation error: [{pose_error[0]:.4f}, {pose_error[1]:.4f}, {pose_error[2]:.4f}]m") + rotation_error_deg = self._rad_to_deg(pose_error[3:]) + print(f" Rotation error: [{rotation_error_deg[0]:.1f}, {rotation_error_deg[1]:.1f}, {rotation_error_deg[2]:.1f}]deg") + print(f" Rotation error (rad): [{pose_error[3]:.4f}, {pose_error[4]:.4f}, {pose_error[5]:.4f}]rad") + + # Check convergence + if (np.linalg.norm(pose_error[:3]) < self.position_tolerance and + np.linalg.norm(pose_error[3:]) < self.rotation_tolerance): + print("✅ Converged to target pose") + metrics['converged'] = True + metrics['final_error'] = error_magnitude + break + + # Calculate and apply correction + correction = self._calculate_stable_correction(pose_error) + if correction is None: + print("❌ Failed to calculate correction") + continue + + # Apply correction to robot + if not self._apply_correction(correction, error_magnitude): + print("❌ Failed to apply correction") + continue + + metrics['corrections_applied'].append(correction.tolist()) + + # Brief pause for system to settle + time.sleep(0.2) + + if not metrics['converged']: + final_pose = self._get_current_filtered_pose(tag_id, 0.1) + if final_pose is not None: + final_error = np.linalg.norm(self.target_pose - final_pose) + metrics['final_error'] = final_error + print(f"⚠️ Did not converge within {self.max_iterations} iterations") + print(f" Final error: {final_error:.4f}") + + return metrics['converged'], metrics + + except Exception as e: + print(f"❌ Pose correction error: {e}") + metrics['error'] = str(e) + return False, metrics + + def _get_current_filtered_pose(self, tag_id: int, dt: float) -> Optional[np.ndarray]: + """ + Get current filtered pose estimate + + Args: + tag_id: AprilTag ID to detect + dt: Time step for filtering + + Returns: + Filtered 6DOF pose or None if detection failed + """ + try: + # Capture image + image_path = self.camera.capture_photo() + if not image_path: + return None + + # Detect tags + image = cv2.imread(image_path) + detections = self.detector.detect_tags(image) + + # Find target tag + target_detection = None + for detection in detections: + if detection['tag_id'] == tag_id: + target_detection = detection + break + + if target_detection is None: + print(f"⚠️ Tag {tag_id} not detected") + return None + + # Get robust pose estimate + pose_result = self.pose_estimator.estimate_pose_ransac(target_detection) + if pose_result is None: + return None + + rvec, tvec, num_inliers = pose_result + + # Validate pose + if not self.pose_estimator.validate_pose(rvec, tvec): + return None + + # Convert to 6DOF + raw_pose = self.pose_estimator.pose_to_6dof(rvec, tvec) + + # Apply coordinate frame correction to handle 180° orientation mismatch + corrected_pose = self._apply_coordinate_frame_correction(raw_pose) + + # Apply Kalman filtering + filtered_pose = self.kalman_filter.update(corrected_pose, dt) + + return filtered_pose + + except Exception as e: + print(f"❌ Error getting current pose: {e}") + return None + + def _calculate_stable_correction(self, pose_error: np.ndarray) -> Optional[np.ndarray]: + """ + Calculate stable correction using pose transformation approach + + Args: + pose_error: 6DOF pose error [x, y, z, rx, ry, rz] + + Returns: + 6DOF correction vector or None if calculation failed + """ + try: + # Apply step size to prevent overshooting + correction = pose_error * self.step_size + + # Apply safety limits + translation_correction = correction[:3] + rotation_correction = correction[3:] + + # Limit translation magnitude + translation_magnitude = np.linalg.norm(translation_correction) + if translation_magnitude > self.max_correction_per_step: + translation_correction = (translation_correction / translation_magnitude) * self.max_correction_per_step + + # Limit rotation magnitude (max 10 degrees per step) + max_rotation_per_step = 0.175 # ~10 degrees + rotation_magnitude = np.linalg.norm(rotation_correction) + if rotation_magnitude > max_rotation_per_step: + rotation_correction = (rotation_correction / rotation_magnitude) * max_rotation_per_step + + final_correction = np.concatenate([translation_correction, rotation_correction]) + + print(f"🔧 Calculated correction (step_size={self.step_size})") + print(f" Translation: [{final_correction[0]:.4f}, {final_correction[1]:.4f}, {final_correction[2]:.4f}]m") + rotation_correction_deg = self._rad_to_deg(final_correction[3:]) + print(f" Rotation: [{rotation_correction_deg[0]:.1f}, {rotation_correction_deg[1]:.1f}, {rotation_correction_deg[2]:.1f}]deg") + + return final_correction + + except Exception as e: + print(f"❌ Error calculating correction: {e}") + return None + + def _apply_correction(self, correction: np.ndarray, error_magnitude: float) -> bool: + """ + Apply correction to robot pose with proper coordinate transformation + + Args: + correction: 6DOF correction vector in camera frame + + Returns: + True if correction applied successfully + """ + try: + # Get current robot pose + current_robot_pose = self.robot.get_tcp_pose() + + # Transform camera-frame correction to robot frame using hand-eye calibration + camera_rot_deg = self._rad_to_deg(correction[3:]) + print(f"🔧 Camera correction: [{correction[0]:.4f}, {correction[1]:.4f}, {correction[2]:.4f}]m, [{camera_rot_deg[0]:.1f}, {camera_rot_deg[1]:.1f}, {camera_rot_deg[2]:.1f}]deg") + + if self.hand_eye_transform is not None: + # Use much more aggressive scaling for meaningful corrections + if error_magnitude < 0.01: # Very close to target, be conservative + robot_correction = -correction * 0.5 # 50% + elif error_magnitude < 0.05: # Close to target, moderate scaling + robot_correction = -correction * 0.8 # 80% + else: # Far from target, full corrections + robot_correction = -correction * 1.0 # 100% + print(f"✅ Using aggressive scaling (hand-eye calibration available but bypassed)") + else: + # Fallback: full scaling (eye-in-hand requires inverse) + robot_correction = -correction * 1.0 # Negative for eye-in-hand, full correction + print(f"⚠️ No hand-eye calibration, using full inverse scaling") + + robot_rot_deg = self._rad_to_deg(robot_correction[3:]) + print(f"🔧 Robot correction: [{robot_correction[0]:.4f}, {robot_correction[1]:.4f}, {robot_correction[2]:.4f}]m, [{robot_rot_deg[0]:.1f}, {robot_rot_deg[1]:.1f}, {robot_rot_deg[2]:.1f}]deg") + + # Apply correction with proper pose composition + target_robot_pose = self._compose_pose_correction(current_robot_pose, robot_correction) + + # Move robot to corrected pose + success = self.robot.move_to_pose(target_robot_pose, speed=0.02, wait=True) + + if success: + print(f"✅ Applied correction to robot") + else: + print(f"❌ Failed to move robot to corrected pose") + + return success + + except Exception as e: + print(f"❌ Error applying correction: {e}") + return False + + def _transform_camera_to_robot_correction(self, camera_correction: np.ndarray) -> np.ndarray: + """ + Transform camera frame correction to robot frame + + Args: + camera_correction: 6DOF correction in camera frame + + Returns: + 6DOF correction in robot frame + """ + # For this simplified version, we'll assume the camera is mounted looking down + # with Z-axis pointing toward the robot's work surface + # This transform should be replaced with proper hand-eye calibration matrix + + # Simple approximate transformation (camera looking down at workspace) + robot_correction = camera_correction.copy() + + # Flip Y and Z to account for camera orientation + # Camera: +X=right, +Y=down, +Z=forward (into scene) + # Robot: +X=forward, +Y=left, +Z=up + robot_correction[0] = -camera_correction[2] # Camera Z → Robot -X (back/forward) + robot_correction[1] = -camera_correction[0] # Camera X → Robot -Y (left/right) + robot_correction[2] = camera_correction[1] # Camera Y → Robot Z (up/down) + + # Scale down the correction to be more conservative + robot_correction *= 0.3 # Reduce correction magnitude + + # Limit rotation corrections which are most problematic + robot_correction[3:] *= 0.1 # Very conservative rotation corrections + + return robot_correction + + def _compose_pose_correction(self, current_pose: np.ndarray, correction: np.ndarray) -> np.ndarray: + """ + Properly compose pose with correction (not simple addition) + + Args: + current_pose: Current robot pose [x,y,z,rx,ry,rz] + correction: Pose correction [dx,dy,dz,drx,dry,drz] + + Returns: + Target pose with correction applied + """ + # For small corrections, simple addition is approximately correct + # For larger corrections, proper pose composition should be used + target_pose = current_pose + correction + + # Ensure rotation angles stay within reasonable bounds + target_pose[3:] = self._normalize_angles(target_pose[3:]) + + return target_pose + + def _normalize_angles(self, angles: np.ndarray) -> np.ndarray: + """Normalize angles to [-π, π] range""" + normalized = angles.copy() + for i in range(len(normalized)): + while normalized[i] > np.pi: + normalized[i] -= 2 * np.pi + while normalized[i] < -np.pi: + normalized[i] += 2 * np.pi + return normalized + + def _rad_to_deg(self, rad_array: np.ndarray) -> np.ndarray: + """Convert radians to degrees for readable logging""" + return np.rad2deg(rad_array) + + def _apply_coordinate_frame_correction(self, pose_6dof: np.ndarray) -> np.ndarray: + """ + Apply coordinate frame correction to handle 180° orientation mismatch + + The AprilTag detection shows ~180° rotation in X and Z axes compared to expected + coordinate frame. This method corrects for this systematic offset. + + Args: + pose_6dof: Raw 6DOF pose [x, y, z, rx, ry, rz] + + Returns: + Corrected 6DOF pose with coordinate frame alignment + """ + corrected_pose = pose_6dof.copy() + + # Apply 180° rotation correction around Y-axis to align coordinate frames + # This compensates for the systematic orientation offset discovered in diagnostics + corrected_pose[3] += np.pi # Add 180° to X-rotation (around Y-axis) + corrected_pose[5] += np.pi # Add 180° to Z-rotation (around Y-axis) + + # Normalize angles to [-π, π] range + corrected_pose[3] = np.arctan2(np.sin(corrected_pose[3]), np.cos(corrected_pose[3])) + corrected_pose[5] = np.arctan2(np.sin(corrected_pose[5]), np.cos(corrected_pose[5])) + + return corrected_pose + + def _load_hand_eye_calibration(self) -> Optional[np.ndarray]: + """ + Load hand-eye calibration transformation matrix + + Returns: + 4x4 transformation matrix from camera to end-effector frame + """ + try: + calib_file = Path("src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json") + if not calib_file.exists(): + print("⚠️ Hand-eye calibration file not found, using simple transformation") + return None + + with open(calib_file, 'r') as f: + data = json.load(f) + + # Extract transformation matrix + transform = np.array(data['hand_eye_transform']['matrix']) + print(f"✅ Loaded hand-eye calibration from: {calib_file}") + return transform + + except Exception as e: + print(f"❌ Error loading hand-eye calibration: {e}") + return None + + def _transform_with_hand_eye_calibration(self, camera_correction: np.ndarray, error_magnitude: float) -> np.ndarray: + """ + Transform camera frame correction to robot frame using hand-eye calibration + + Args: + camera_correction: 6DOF correction in camera frame [x,y,z,rx,ry,rz] + + Returns: + 6DOF correction in robot frame + """ + try: + # Convert 6DOF correction to homogeneous transformation matrix + translation = camera_correction[:3] + rotation = camera_correction[3:] + + # Create rotation matrix from rotation vector + rotation_matrix, _ = cv2.Rodrigues(rotation) + + # Create 4x4 transformation matrix + camera_transform = np.eye(4) + camera_transform[:3, :3] = rotation_matrix + camera_transform[:3, 3] = translation + + # For eye-in-hand: robot_correction = inv(hand_eye) @ camera_correction @ hand_eye + # But for small corrections, we can use the simpler linear transformation + hand_eye_inv = np.linalg.inv(self.hand_eye_transform) + + # Transform the correction + robot_transform = hand_eye_inv @ camera_transform @ self.hand_eye_transform + + # Extract 6DOF from transformation matrix + robot_translation = robot_transform[:3, 3] + robot_rotation_matrix = robot_transform[:3, :3] + robot_rotation, _ = cv2.Rodrigues(robot_rotation_matrix) + + # For eye-in-hand, we typically need to invert the correction direction + # because moving the robot moves the camera in the opposite direction + # Use adaptive scaling based on error magnitude + if error_magnitude < 0.2: # Close to target, be more conservative + translation_scale = 0.01 + rotation_scale = 0.005 + else: # Far from target, normal corrections + translation_scale = 0.03 + rotation_scale = 0.01 + + robot_correction = np.concatenate([-robot_translation * translation_scale, -robot_rotation.flatten() * rotation_scale]) + + return robot_correction + + except Exception as e: + print(f"❌ Error in hand-eye transformation: {e}") + # Fallback to simple inverse scaling + return -camera_correction * 0.05 \ No newline at end of file diff --git a/src/ur_toolkit/pose_correction/robust_estimator.py b/src/ur_toolkit/pose_correction/robust_estimator.py new file mode 100644 index 0000000..63cf138 --- /dev/null +++ b/src/ur_toolkit/pose_correction/robust_estimator.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +""" +Robust Pose Estimator using OpenCV solvePnP +Provides stable pose estimation to replace direct AprilTag pose calculation +""" + +import cv2 +import numpy as np +from typing import Optional, Tuple +from scipy.spatial.transform import Rotation as R + + +class RobustPoseEstimator: + """Robust pose estimation using OpenCV solvePnP with RANSAC""" + + def __init__(self, camera_matrix: np.ndarray, dist_coeffs: np.ndarray, tag_size: float = 0.05): + """ + Initialize robust pose estimator + + Args: + camera_matrix: Camera intrinsic matrix (3x3) + dist_coeffs: Distortion coefficients + tag_size: Physical size of AprilTag in meters + """ + self.camera_matrix = camera_matrix + self.dist_coeffs = dist_coeffs + self.tag_size = tag_size + + # Define 3D tag corners in tag coordinate frame (Z=0 plane) + self.tag_corners_3d = np.array([ + [-tag_size/2, -tag_size/2, 0], + [ tag_size/2, -tag_size/2, 0], + [ tag_size/2, tag_size/2, 0], + [-tag_size/2, tag_size/2, 0] + ], dtype=np.float32) + + print(f"🎯 Robust Pose Estimator initialized (tag_size={tag_size}m)") + + def estimate_pose_ransac(self, detection) -> Optional[Tuple[np.ndarray, np.ndarray, int]]: + """ + Estimate pose using robust RANSAC approach + + Args: + detection: AprilTag detection (dict with 'corners' key or object with .corners attribute) + + Returns: + Tuple of (rvec, tvec, num_inliers) or None if estimation failed + """ + try: + # Extract 2D corners from detection (handle both dict and object formats) + if isinstance(detection, dict): + corners_2d = detection['corners'].reshape(-1, 2).astype(np.float32) + else: + corners_2d = detection.corners.reshape(-1, 2).astype(np.float32) + + # Use solvePnPRansac for robust estimation + success, rvec, tvec, inliers = cv2.solvePnPRansac( + self.tag_corners_3d, corners_2d, + self.camera_matrix, self.dist_coeffs, + iterationsCount=500, + reprojectionError=2.0, + confidence=0.95, + flags=cv2.SOLVEPNP_IPPE_SQUARE # Optimal for planar squares + ) + + if not success or inliers is None or len(inliers) < 3: + print("⚠️ RANSAC failed, trying standard pose estimation...") + # Fallback to standard pose estimation + fallback_result = self.estimate_pose_standard(detection) + if fallback_result is not None: + rvec_fb, tvec_fb = fallback_result + print("✅ Standard pose estimation succeeded as fallback") + return rvec_fb, tvec_fb, 4 # Assume all corners used + else: + print("❌ Both RANSAC and standard pose estimation failed") + return None + + num_inliers = len(inliers) + print(f"✅ Robust pose estimated with {num_inliers}/4 inliers") + + return rvec, tvec, num_inliers + + except Exception as e: + print(f"❌ Pose estimation error: {e}") + return None + + def estimate_pose_standard(self, detection) -> Optional[Tuple[np.ndarray, np.ndarray]]: + """ + Estimate pose using standard solvePnP (faster, less robust) + + Args: + detection: AprilTag detection object with corners + + Returns: + Tuple of (rvec, tvec) or None if estimation failed + """ + try: + # Extract 2D corners from detection (handle both dict and object formats) + if isinstance(detection, dict): + corners_2d = detection['corners'].reshape(-1, 2).astype(np.float32) + else: + corners_2d = detection.corners.reshape(-1, 2).astype(np.float32) + + success, rvec, tvec = cv2.solvePnP( + self.tag_corners_3d, corners_2d, + self.camera_matrix, self.dist_coeffs, + flags=cv2.SOLVEPNP_IPPE_SQUARE + ) + + if not success: + return None + + return rvec, tvec + + except Exception as e: + print(f"❌ Standard pose estimation error: {e}") + return None + + def pose_to_6dof(self, rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: + """ + Convert rotation vector and translation to 6DOF pose + + Args: + rvec: Rotation vector (3x1) + tvec: Translation vector (3x1) + + Returns: + 6DOF pose [x, y, z, rx, ry, rz] + """ + # Extract translation + translation = tvec.flatten() + + # Convert rotation vector to euler angles + rotation_matrix, _ = cv2.Rodrigues(rvec) + rotation = R.from_matrix(rotation_matrix) + euler_angles = rotation.as_euler('xyz', degrees=False) + + return np.concatenate([translation, euler_angles]) + + def pose_to_matrix(self, rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: + """ + Convert rotation vector and translation to 4x4 transformation matrix + + Args: + rvec: Rotation vector (3x1) + tvec: Translation vector (3x1) + + Returns: + 4x4 transformation matrix + """ + rotation_matrix, _ = cv2.Rodrigues(rvec) + + transform = np.eye(4) + transform[:3, :3] = rotation_matrix + transform[:3, 3] = tvec.flatten() + + return transform + + def validate_pose(self, rvec: np.ndarray, tvec: np.ndarray, + max_distance: float = 1.0, max_rotation: float = np.pi) -> bool: + """ + Validate that pose estimate is reasonable + + Args: + rvec: Rotation vector + tvec: Translation vector + max_distance: Maximum allowed distance from camera (meters) + max_rotation: Maximum allowed rotation (radians) + + Returns: + True if pose is valid + """ + try: + # Check distance + distance = np.linalg.norm(tvec) + if distance > max_distance: + print(f"⚠️ Pose validation failed: distance {distance:.3f}m > {max_distance}m") + return False + + # Check rotation magnitude + rotation_magnitude = np.linalg.norm(rvec) + if rotation_magnitude > max_rotation: + print(f"⚠️ Pose validation failed: rotation {rotation_magnitude:.3f}rad > {max_rotation:.3f}rad") + return False + + return True + + except Exception as e: + print(f"❌ Pose validation error: {e}") + return False \ No newline at end of file diff --git a/src/ur_toolkit/positions/pose_correction_history.json b/src/ur_toolkit/positions/pose_correction_history.json new file mode 100644 index 0000000..d4898d4 --- /dev/null +++ b/src/ur_toolkit/positions/pose_correction_history.json @@ -0,0 +1,763 @@ +{ + "grasp-B": [ + { + "timestamp": "2025-09-24T17:17:24.256125", + "position_name": "grasp-B", + "original_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "corrected_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "original_tag_pose": [ + 0.004, + 0.012, + 0.304, + -0.027, + -3.06, + -0.116 + ], + "current_tag_pose": [ + 0.004587821745362918, + 0.011197626629486462, + 0.3036680629593786, + -0.025534345055306697, + -3.0491967315368664, + -0.11643315025270483 + ], + "correction_delta": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "tag_delta": [ + 0.0005878217453629177, + -0.0008023733705135386, + -0.0003319370406213995, + 0.001465654944693303, + 0.01080326846313362, + -0.00043315025270482255 + ], + "metrics": { + "position_name": "grasp-B", + "observation_pose": "right-column-observe", + "observation_offset": [ + 0.004108925139692743, + -0.10301444690712325, + -0.11483987405945315, + 0.0036171775020144422, + 0.030824312289923217, + 0.03060287121572758 + ], + "tag_id": 2, + "method": "observation", + "iterations": 1, + "total_correction": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "final_error": 0.010961108186393695, + "converged": true, + "corrections_applied": [], + "final_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "final_obs_pose": [ + -0.145, + -0.552, + 0.291, + 0.0, + -2.257, + 2.183 + ] + } + }, + { + "timestamp": "2025-09-24T17:18:48.605026", + "position_name": "grasp-B", + "original_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "corrected_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "original_tag_pose": [ + 0.004587821745362918, + 0.011197626629486462, + 0.3036680629593786, + -0.025534345055306697, + -3.0491967315368664, + -0.11643315025270483 + ], + "current_tag_pose": [ + 0.006390836202496207, + 0.00955381478284972, + 0.3295758616873924, + -0.024198148162549088, + -3.029795233033335, + -0.10866677264351536 + ], + "correction_delta": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "tag_delta": [ + 0.001803014457133289, + -0.0016438118466367418, + 0.025907798728013798, + 0.0013361968927576087, + 0.019401498503531567, + 0.007766377609189468 + ], + "metrics": { + "position_name": "grasp-B", + "observation_pose": "right-column-observe", + "observation_offset": [ + 0.004108925139692743, + -0.10301444690712325, + -0.11483987405945315, + 0.0036171775020144422, + 0.030824312289923217, + 0.03060287121572758 + ], + "tag_id": 2, + "method": "observation", + "iterations": 1, + "total_correction": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "final_error": 0.03340190415138515, + "converged": true, + "corrections_applied": [], + "final_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "final_obs_pose": [ + -0.145, + -0.552, + 0.291, + 0.0, + -2.257, + 2.183 + ] + } + }, + { + "timestamp": "2025-09-24T17:20:36.229193", + "position_name": "grasp-B", + "original_robot_pose": [ + -0.141, + -0.655, + 0.177, + 0.003, + -2.227, + 2.214 + ], + "corrected_robot_pose": [ + -0.1379730445833015, + -0.6532712339148175, + 0.1763012992680356, + 0.0036936957905076033, + -2.221048231737977, + 2.214544585215203 + ], + "original_tag_pose": [ + 0.006390836202496207, + 0.00955381478284972, + 0.3295758616873924, + -0.024198148162549088, + -3.029795233033335, + -0.10866677264351536 + ], + "current_tag_pose": [ + 0.005042829885036635, + 0.009550357313701281, + 0.3289624418814822, + -0.02036029306868384, + -3.0165404960560247, + -0.11959608775808148 + ], + "correction_delta": [ + 0.003026955416698479, + 0.001728766085182576, + -0.0006987007319644023, + 0.0006936957905076032, + 0.005951768262022927, + 0.0005445852152030639 + ], + "tag_delta": [ + -0.0013480063174595722, + -3.4574691484389602e-06, + -0.0006134198059101759, + 0.0038378550938652466, + 0.013254736977310166, + -0.010929315114566115 + ], + "metrics": { + "position_name": "grasp-B", + "observation_pose": "right-column-observe", + "observation_offset": [ + 0.004108925139692743, + -0.10301444690712325, + -0.11483987405945315, + 0.0036171775020144422, + 0.030824312289923217, + 0.03060287121572758 + ], + "tag_id": 2, + "method": "observation", + "iterations": 5, + "total_correction": [ + 0.003026955416698482, + 0.0017287660851826037, + -0.0006987007319643873, + 0.000693695790507603, + 0.005951768262023024, + 0.0005445852152033302 + ], + "final_error": 0.01766523506146892, + "converged": true, + "corrections_applied": [ + { + "iteration": 1, + "tag_error": [ + -0.003916582596279831, + 0.00014787006016246489, + -0.001949359538907347, + -0.0036481142115271127, + -0.018812046258703585, + -0.02947696592239056 + ], + "robot_correction": [ + 0.0011749747788839494, + 0.0005848078616722041, + -4.436101804873946e-05, + -0.008843089776717168, + 0.0010944342634581338, + 0.005643613877611075 + ], + "correction_magnitude": 0.010628872688186856 + }, + { + "iteration": 2, + "tag_error": [ + -0.0031828074232473536, + 0.0007781066060977247, + -0.0015780647005919635, + -0.010979269639311208, + 0.007242403477599968, + 0.019286005260403152 + ], + "robot_correction": [ + 0.000954842226974206, + 0.00047341941017758904, + -0.0002334319818293174, + 0.005785801578120945, + 0.0032937808917933623, + -0.00217272104327999 + ], + "correction_magnitude": 0.007087704092506142 + }, + { + "iteration": 3, + "tag_error": [ + -0.0021602995424829206, + 0.0006000395619886658, + -0.0005769305854185225, + -0.004720273301323224, + -0.009771285774674787, + 0.01773615961143002 + ], + "robot_correction": [ + 0.0006480898627448762, + 0.00017307917562555674, + -0.00018001186859659972, + 0.005320847883429005, + 0.0014160819903969672, + 0.002931385732402436 + ], + "correction_magnitude": 0.0062763137157342335 + }, + { + "iteration": 4, + "tag_error": [ + -0.000830161826984835, + 0.0008029862116324359, + -0.0016581987923575126, + -0.0004915703879152002, + 0.019525644505100637, + -0.005232879647750599 + ], + "robot_correction": [ + 0.0002490485480954505, + 0.0004974596377072538, + -0.00024089586348973076, + -0.0015698638943251796, + 0.00014747111637456004, + -0.005857693351530191 + ], + "correction_magnitude": 0.006096418117627294 + } + ], + "final_robot_pose": [ + -0.1379730445833015, + -0.6532712339148175, + 0.1763012992680356, + 0.0036936957905076033, + -2.221048231737977, + 2.214544585215203 + ], + "final_obs_pose": [ + -0.1419730445833015, + -0.5502712339148175, + 0.2903012992680356, + 0.000693695790507603, + -2.251048231737977, + 2.1835445852152033 + ] + } + }, + { + "timestamp": "2025-09-24T17:24:22.879622", + "position_name": "grasp-B", + "original_robot_pose": [ + -0.1379730445833015, + -0.6532712339148175, + 0.1763012992680356, + 0.0036936957905076033, + -2.221048231737977, + 2.214544585215203 + ], + "corrected_robot_pose": [ + -0.1499163787166164, + -0.6241909827903812, + 0.17390599216293898, + 0.10640781321276485, + -2.2490203888100453, + 2.2354570862156797 + ], + "original_tag_pose": [ + 0.005042829885036635, + 0.009550357313701281, + 0.3289624418814822, + -0.02036029306868384, + -3.0165404960560247, + -0.11959608775808148 + ], + "current_tag_pose": [ + 0.008413825254752916, + 0.00643372222503381, + 0.3295708045242047, + 0.00793431770974715, + -3.0094032455229733, + -0.09515364452905845 + ], + "correction_delta": [ + -0.011943334133314887, + 0.02908025112443624, + -0.0023953071050966113, + 0.10271411742225725, + -0.02797215707206835, + 0.020912501000476702 + ], + "tag_delta": [ + 0.003370995369716282, + -0.0031166350886674705, + 0.0006083626427224598, + 0.02829461077843099, + 0.007137250533051365, + 0.02444244322902303 + ], + "metrics": { + "position_name": "grasp-B", + "observation_pose": "right-column-observe", + "observation_offset": [ + 0.004108925139692743, + -0.10301444690712325, + -0.11483987405945315, + 0.0036171775020144422, + 0.030824312289923217, + 0.03060287121572758 + ], + "tag_id": 2, + "method": "observation", + "iterations": 15, + "total_correction": [ + -0.011943334133314904, + 0.0290802511244362, + -0.002395307105096593, + 0.10271411742225725, + -0.0279721570720681, + 0.020912501000476674 + ], + "final_error": 0.038345866849532576, + "converged": true, + "corrections_applied": [ + { + "iteration": 1, + "tag_error": [ + -0.0012548954438526476, + 0.00178536992987759, + -0.029751429038440913, + 0.0012412578808331208, + -0.036350191415433386, + 0.023886704696419875 + ], + "robot_correction": [ + 0.00037646863315579426, + 0.008925428711532273, + -0.000535610978963277, + 0.0071660114089259624, + -0.0003723773642499362, + 0.010905057424630016 + ], + "correction_magnitude": 0.01582727226296186 + }, + { + "iteration": 2, + "tag_error": [ + 0.001100408652788377, + 0.00169558538604268, + -0.02096736759338108, + -0.004454634124519189, + -0.014560780441585042, + 0.012118146126097795 + ], + "robot_correction": [ + -0.00033012259583651307, + 0.006290210278014324, + -0.000508675615812804, + 0.0036354438378293384, + 0.0013363902373557566, + 0.004368234132475513 + ], + "correction_magnitude": 0.00860339103799639 + }, + { + "iteration": 3, + "tag_error": [ + 0.001669556115730107, + 0.0013905529077014034, + -0.014128546050659108, + -0.0023050848786771057, + -0.03019080212590275, + 0.007935922789863992 + ], + "robot_correction": [ + -0.000500866834719032, + 0.004238563815197732, + -0.000417165872310421, + 0.0023807768369591975, + 0.0006915254636031317, + 0.009057240637770825 + ], + "correction_magnitude": 0.010323285908631309 + }, + { + "iteration": 4, + "tag_error": [ + 0.0024300193182077235, + 0.0016618399155752632, + -0.010119424318053516, + -0.00749542059347881, + -0.022868032194809818, + 0.002432389458620149 + ], + "robot_correction": [ + -0.000729005795462317, + 0.0030358272954160545, + -0.0004985519746725789, + 0.0007297168375860446, + 0.002248626178043643, + 0.006860409658442945 + ], + "correction_magnitude": 0.00791519285438818 + }, + { + "iteration": 5, + "tag_error": [ + 0.00240155102238274, + 0.0018969199340451437, + -0.006862483013010234, + -0.014118478753131245, + -0.010374091550278575, + 0.03412925477744065 + ], + "robot_correction": [ + -0.0007204653067148219, + 0.0020587449039030704, + -0.0005690759802135431, + 0.010238776433232195, + 0.004235543625939374, + 0.0031122274650835723 + ], + "correction_magnitude": 0.011727731270802112 + }, + { + "iteration": 6, + "tag_error": [ + 0.0027333354649049296, + 0.0019306548313823453, + -0.004557808275524322, + -0.0032143853254910162, + -0.03499966345109806, + 0.04942052535435701 + ], + "robot_correction": [ + -0.0008200006394714789, + 0.0013673424826572966, + -0.0005791964494147036, + 0.014826157606307101, + 0.0009643155976473048, + 0.010499899035329418 + ], + "correction_magnitude": 0.01827211615362772 + }, + { + "iteration": 7, + "tag_error": [ + 0.004481815812153954, + 0.002444237262079603, + -0.003105266795547512, + -0.0030270318308956816, + 0.007880249545666906, + 0.029284522721571157 + ], + "robot_correction": [ + -0.0006722723718230931, + 0.0004657900193321268, + -0.0003666355893119404, + 0.004392678408235673, + 0.00045405477463435223, + -0.001182037431850036 + ], + "correction_magnitude": 0.0046585763883636114 + }, + { + "iteration": 8, + "tag_error": [ + 0.004129294444336294, + 0.0019291123579537241, + -0.0032020038295761943, + 0.0012761370487891083, + -0.0055221607265822925, + 0.026133624821006007 + ], + "robot_correction": [ + -0.0012387883333008882, + 0.0009606011488728582, + -0.0005787337073861172, + 0.007840087446301802, + -0.0003828411146367325, + 0.0016566482179746877 + ], + "correction_magnitude": 0.008194529004698027 + }, + { + "iteration": 9, + "tag_error": [ + 0.00397872624033054, + 0.0014130878276245387, + -0.0022576244214253527, + 0.006539306746001805, + 0.005294098936205138, + 0.005292936278551402 + ], + "robot_correction": [ + -0.001193617872099162, + 0.0006772873264276058, + -0.0004239263482873616, + 0.0015878808835654205, + -0.0019617920238005414, + -0.0015882296808615414 + ], + "correction_magnitude": 0.0033099278632797604 + }, + { + "iteration": 10, + "tag_error": [ + 0.003034709815349198, + 0.0006163589813312706, + -0.001713356919599951, + 0.005181885895386366, + 0.007684474842843958, + 0.05529683841985897 + ], + "robot_correction": [ + -0.0009104129446047593, + 0.0005140070758799853, + -0.00018490769439938117, + 0.01658905152595769, + -0.0015545657686159098, + -0.0023053424528531874 + ], + "correction_magnitude": 0.01685393588880548 + }, + { + "iteration": 11, + "tag_error": [ + 0.003865572057641242, + -3.549673996782257e-05, + -0.000386465873235986, + 0.018764252203046124, + -0.004492988487560634, + 0.06412102393942135 + ], + "robot_correction": [ + -0.0005798358086461863, + 5.79698809853979e-05, + 5.324510995173386e-06, + 0.009618153590913201, + -0.0028146378304569186, + 0.000673948273134095 + ], + "correction_magnitude": 0.010061057067346519 + }, + { + "iteration": 12, + "tag_error": [ + 0.004693950983589813, + -0.00028587549213783883, + -0.001288910985769054, + 0.02314725005284985, + 0.027114789775853243, + 0.022982773933719336 + ], + "robot_correction": [ + -0.0014081852950769437, + 0.00038667329573071615, + 8.576264764135165e-05, + 0.006894832180115801, + -0.006944175015854955, + -0.008134436932755973 + ], + "correction_magnitude": 0.012808949955027602 + }, + { + "iteration": 13, + "tag_error": [ + 0.003960703806183628, + -0.0017541458270937584, + -0.000815820852442195, + 0.027297624415184255, + 0.011619692989758068, + 0.0031471465806507115 + ], + "robot_correction": [ + -0.0011882111418550884, + 0.0002447462557326585, + 0.0005262437481281275, + 0.0009441439741952134, + -0.008189287324555277, + -0.0034859078969274203 + ], + "correction_magnitude": 0.009047434306131185 + }, + { + "iteration": 14, + "tag_error": [ + 0.0033890640531517666, + -0.002382838907702139, + -0.00013189142523611919, + 0.023981844245306996, + 0.024613447630669416, + 0.02845891161141896 + ], + "robot_correction": [ + -0.00101671921594553, + 3.9567427570835755e-05, + 0.0007148516723106417, + 0.008537673483425687, + -0.007194553273592098, + -0.007384034289200824 + ], + "correction_magnitude": 0.013443352415191725 + }, + { + "iteration": 15, + "tag_error": [ + 0.003370995369716282, + -0.0031166350886674705, + 0.0006083626427224598, + 0.02829461077843099, + 0.007137250533051365, + 0.02444244322902303 + ], + "robot_correction": [ + -0.0010112986109148845, + -0.00018250879281673791, + 0.0009349905266002411, + 0.007332732968706909, + -0.008488383233529297, + -0.0021411751599154094 + ], + "correction_magnitude": 0.011503760054859771 + } + ], + "final_robot_pose": [ + -0.1499163787166164, + -0.6241909827903812, + 0.17390599216293898, + 0.10640781321276485, + -2.2490203888100453, + 2.2354570862156797 + ], + "final_obs_pose": [ + -0.15391637871661643, + -0.5211909827903812, + 0.287905992162939, + 0.10340781321276485, + -2.279020388810045, + 2.20445708621568 + ], + "user_override": true + } + } + ] +} \ No newline at end of file diff --git a/src/ur_toolkit/positions/taught_positions.yaml b/src/ur_toolkit/positions/taught_positions.yaml index e3ceb02..fc19e1d 100644 --- a/src/ur_toolkit/positions/taught_positions.yaml +++ b/src/ur_toolkit/positions/taught_positions.yaml @@ -36,32 +36,6 @@ positions: equipment_name: left-column observation_pose: grasp-A observation_offset: [0.0, 0.0, 0.02, 0.0, 0.0, 0.0] - grasp-B: - coordinates: [-0.14, -0.691, 0.176, -0.045, 2.235, -2.177] - joints: [4.672, -3.015, -0.538, 0.386, -4.634, 6.274] - description: '' - pose_type: work - equipment_name: Right workstation - observation_pose: observe-B - observation_offset: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - grasp-B-safe: - coordinates: [-0.14, -0.691, 0.196, -0.045, 2.235, -2.177] - joints: [4.672, -3.015, -0.538, 0.386, -4.634, 6.274] - description: Safe up position (20.0mm from grasp-B) - pose_type: work - equipment_name: Right workstation - observation_pose: null - observation_offset: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - observe-B: - coordinates: [-0.15, -0.554, 0.256, -0.063, 2.268, -1.992] - joints: [4.614, -2.244, -1.515, 0.544, -4.599, 6.253] - description: Observation pose for right column - pose_type: observation - equipment_name: right-column - observation_pose: observe-B - observation_offset: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - tag_reference: tag_2 - camera_to_tag: [0.001, -0.001, 0.299, -3.066, -0.021, 0.127] test-c: coordinates: [-0.189, -0.518, 0.354, 0.118, 2.579, -1.789] joints: [4.551, -1.819, -1.433, -0.249, -4.605, 6.286] @@ -108,3 +82,83 @@ positions: observation_offset: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] tag_reference: tag_2 camera_to_tag: [-0.024, 0.024, 0.407, -2.873, 0.181, 0.137] + grasp-B: + coordinates: [-0.131, -0.615, 0.108, 0.069, -2.221, 2.035] + joints: [4.687, -2.841, -0.962, 0.657, -4.685, 6.278] + description: '' + pose_type: work + equipment_name: right-column + observation_pose: right-column-observe + observation_offset: [0.004108925139692743, -0.10301444690712325, -0.11483987405945315, 0.0036171775020144422, 0.030824312289923217, + 0.03060287121572758] + last_visual_servo_update: '2025-09-24T17:20:36.243195' + right-column-observe: + coordinates: [-0.135, -0.512, 0.223, 0.065, -2.252, 2.004] + joints: [0.818, -1.054, 0.885, 0.315, 0.864, 3.011] + description: Observation pose for right-column + pose_type: observation + tag_reference: tag_2 + camera_to_tag: [-0.01, -0.071, 0.326, -2.956, -0.208, -0.173] + equipment_name: right-column + last_visual_servo_update: '2025-10-21T14:10:00.617301' + grasp-B-safe: + coordinates: [-0.131, -0.615, 0.128, 0.069, -2.221, 2.035] + joints: [4.687, -2.841, -0.962, 0.657, -4.685, 6.278] + description: Safe up position (20.0mm from grasp-B) + pose_type: work + equipment_name: right-column + observation_pose: right-column-observe + observation_offset: [0.004108925139692743, -0.10301444690712325, -0.09483987405945316, 0.0036171775020144422, 0.030824312289923217, + 0.03060287121572758] + grasp-right: + coordinates: [-0.142, -0.587, 0.112, 0.071, -2.169, 2.155] + joints: [0.916, -0.681, 0.883, -0.197, 0.922, 3.132] + description: '' + pose_type: work + equipment_name: right-column + observation_pose: right-column-observe + observation_offset: [-0.007120092742927325, -0.07527190326890298, -0.11067671989843408, 0.0058240797773486145, 0.08306896764563598, + 0.15114823170252212] + grasp-right-safe: + coordinates: [-0.142, -0.587, 0.132, 0.071, -2.169, 2.155] + joints: [0.916, -0.681, 0.883, -0.197, 0.922, 3.132] + description: Safe up position (20.0mm from grasp-right) + pose_type: work + equipment_name: right-column + observation_pose: right-column-observe + observation_offset: [-0.007120092742927325, -0.07527190326890298, -0.0906767198984341, 0.0058240797773486145, 0.08306896764563598, + 0.15114823170252212] + test: + coordinates: [-0.06, -0.457, 0.261, 0.235, -2.055, 2.205] + joints: [0.571, -1.776, 2.071, -0.424, 0.544, 3.059] + description: '' + pose_type: work + test-observe: + coordinates: [-0.151, -0.397, 0.282, -0.193, 2.221, -2.2] + joints: [0.049, -1.8, 1.901, -0.394, -0.044, 3.355] + description: '' + pose_type: observation + tag_reference: tag_2 + camera_to_tag: [0.04, 0.001, 0.464, -3.155, -0.119, -0.01] + equipment_name: test-eq + pose A: + coordinates: [-0.135, -0.588, 0.269, 0.006, -2.253, 2.14] + joints: [0.818, -0.846, 1.547, -3.776, -0.845, 6.212] + description: '' + pose_type: work + pose-A-observe: + coordinates: [-0.135, -0.588, 0.269, 0.006, -2.253, 2.139] + joints: [0.818, -0.846, 1.547, -3.776, -0.845, 6.212] + description: '' + pose_type: observation + tag_reference: tag_2 + camera_to_tag: [0.011, -0.012, 0.269, -3.118, -0.027, 0.048] + equipment_name: poseA + pose-B-observe: + coordinates: [0.025, -0.422, 0.353, -0.529, -2.253, 1.63] + joints: [1.62, -1.679, 1.879, -2.984, -2.088, 6.435] + description: '' + pose_type: observation + tag_reference: tag_2 + camera_to_tag: [0.017, -0.041, 0.478, -2.758, 0.088, -0.6] + equipment_name: pose-B diff --git a/src/ur_toolkit/positions/taught_positions_corrected.yaml b/src/ur_toolkit/positions/taught_positions_corrected.yaml new file mode 100644 index 0000000..9094ae7 --- /dev/null +++ b/src/ur_toolkit/positions/taught_positions_corrected.yaml @@ -0,0 +1,324 @@ +apriltags: + tag_1: + dictionary: DICT_4X4_1000 + marker_length: 0.023 + description: Left workstation + observation_pose: grasp-A + tag_2: + dictionary: DICT_4X4_1000 + marker_length: 0.023 + description: Right workstation + observation_pose: observe-B +positions: + safe-home: + coordinates: + - -0.056 + - -0.451 + - 0.307 + - -0.037 + - -2.166 + - 2.265 + joints: + - 5.169 + - -1.752 + - -2.405 + - 1.07 + - -5.193 + - 6.269 + description: Safe home position + pose_type: observation + tag_reference: tag_1 + camera_to_tag: + - -0.144 + - 0.045 + - 0.42 + - 3.111 + - -0.005 + - -0.056 + grasp-A: + coordinates: + - 0.097 + - -0.679 + - 0.222 + - 0.016 + - 2.222 + - -2.192 + joints: + - 5.24 + - -2.747 + - -0.909 + - 0.503 + - -5.234 + - 6.309 + description: Left column grasp + pose_type: + - work + equipment_name: left-column + observation_pose: grasp-A + observation_offset: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + tag_reference: tag_1 + camera_to_tag: + - 0.005 + - -0.05 + - 0.183 + - -3.093 + - 0.003 + - 0.079 + grasp-A-safe: + coordinates: + - 0.097 + - -0.679 + - 0.242 + - 0.016 + - 2.222 + - -2.192 + joints: + - 5.24 + - -2.747 + - -0.909 + - 0.503 + - -5.234 + - 6.309 + description: Safe approach above grasp-A (2cm offset) + pose_type: work + equipment_name: left-column + observation_pose: grasp-A + observation_offset: + - 0.0 + - 0.0 + - 0.02 + - 0.0 + - 0.0 + - 0.0 + test-c: + coordinates: + - -0.189 + - -0.518 + - 0.354 + - 0.118 + - 2.579 + - -1.789 + joints: + - 4.551 + - -1.819 + - -1.433 + - -0.249 + - -4.605 + - 6.286 + description: '' + pose_type: observation + tag_reference: tag_2 + camera_to_tag: + - -0.047 + - -0.009 + - 0.36 + - -2.784 + - 0.097 + - -0.042 + equipment_name: test-c + observation_pose: test-c + observation_offset: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + test-D: + coordinates: + - -0.403 + - -0.392 + - 0.413 + - -0.772 + - -1.809 + - 1.669 + joints: + - 4.375 + - -1.82 + - -1.513 + - 0.112 + - -5.198 + - 6.301 + description: '' + pose_type: work + observation_pose: null + dummy 4: + coordinates: + - -0.403 + - -0.392 + - 0.413 + - -0.772 + - -1.809 + - 1.669 + joints: + - 4.375 + - -1.82 + - -1.513 + - 0.112 + - -5.198 + - 6.301 + description: '' + pose_type: work + equipment_name: 4th station + observation_pose: 4th station-observe + observation_offset: + - -0.201 + - 0.089 + - 0.045 + - -0.834 + - 0.527 + - -0.186 + 4th station-observe: + coordinates: + - -0.202 + - -0.481 + - 0.368 + - 0.062 + - 2.382 + - -1.855 + joints: + - 4.375 + - -1.82 + - -1.513 + - -0.073 + - -4.316 + - 6.301 + description: Observation pose for 4th station + pose_type: observation + tag_reference: tag_2 + camera_to_tag: + - -0.024 + - 0.024 + - 0.408 + - -2.88 + - 0.181 + - 0.135 + equipment_name: 4th station + observation_pose: 4th station-observe + observation_offset: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + test-5: + coordinates: + - -0.202 + - -0.481 + - 0.368 + - 0.062 + - 2.382 + - -1.855 + joints: + - 4.375 + - -1.819 + - -1.513 + - -0.073 + - -4.316 + - 6.301 + description: '' + pose_type: + - work + - observation + equipment_name: test5 + observation_pose: test-5 + observation_offset: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + tag_reference: tag_2 + camera_to_tag: + - -0.024 + - 0.024 + - 0.407 + - -2.873 + - 0.181 + - 0.137 + grasp-B: + coordinates: + - -0.168 + - -0.674 + - 0.18 + - -0.015 + - -2.271 + - 2.16 + joints: + - -1.65 + - -2.902 + - -0.738 + - 0.45 + - 1.64 + - 6.279 + description: '' + pose_type: work + equipment_name: eq-B + observation_pose: right-column-observe + observation_offset: + - 0.0030929582069785244 + - -0.0024870088447962457 + - -0.10750223534087341 + - 0.06920929833044102 + - -1.3823217162014316 + - -4.34809171226542 + right-column-observe: + coordinates: + - -0.171 + - -0.671 + - 0.287 + - 0.054 + - 2.253 + - -2.188 + joints: + - -1.65 + - -2.666 + - -0.728 + - 0.224 + - 1.628 + - 6.303 + description: Observation pose for eq-B + pose_type: observation + tag_reference: tag_2 + camera_to_tag: + - 0.001 + - 0.013 + - 0.215 + - 0.056 + - 0.060592653589793155 + - 0.038 + equipment_name: eq-B + grasp-B-safe: + coordinates: + - -0.168 + - -0.674 + - 0.2 + - -0.015 + - -2.271 + - 2.16 + joints: + - -1.65 + - -2.902 + - -0.738 + - 0.45 + - 1.64 + - 6.279 + description: Safe up position (20.0mm from grasp-B) + pose_type: work + equipment_name: eq-B + observation_pose: right-column-observe + observation_offset: + - 0.0030929582069785244 + - -0.0024870088447962457 + - -0.08750223534087342 + - 0.06920929833044102 + - -1.3823217162014316 + - -4.34809171226542 diff --git a/src/ur_toolkit/robots/ur/ur_controller.py b/src/ur_toolkit/robots/ur/ur_controller.py index d0a36aa..e154c32 100644 --- a/src/ur_toolkit/robots/ur/ur_controller.py +++ b/src/ur_toolkit/robots/ur/ur_controller.py @@ -16,6 +16,7 @@ try: import dashboard_client import script_client + DASHBOARD_AVAILABLE = True except ImportError: DASHBOARD_AVAILABLE = False @@ -23,6 +24,7 @@ # Import centralized configuration import sys + sys.path.append(str(Path(__file__).parent.parent.parent)) import sys from pathlib import Path @@ -37,7 +39,15 @@ class URController: """Universal Robots interface using RTDE""" - def __init__(self, robot_ip=None, speed=None, acceleration=None, read_only=False, gripper_ip=None, gripper_port=None): + def __init__( + self, + robot_ip=None, + speed=None, + acceleration=None, + read_only=False, + gripper_ip=None, + gripper_port=None, + ): """ Initialize UR robot interface @@ -49,15 +59,17 @@ def __init__(self, robot_ip=None, speed=None, acceleration=None, read_only=False gripper_ip: IP address of gripper for socket-based control (optional) gripper_port: Port for gripper socket communication (default 502) """ - self.robot_ip = robot_ip or config.get('robot.ip_address', '192.168.0.10') - self.speed = speed or config.get('robot.default_speed', 0.05) - self.acceleration = acceleration or config.get('robot.default_acceleration', 0.2) + self.robot_ip = robot_ip or config.get("robot.ip_address", "192.168.0.10") + self.speed = speed or config.get("robot.default_speed", 0.05) + self.acceleration = acceleration or config.get( + "robot.default_acceleration", 0.2 + ) self.read_only = read_only # Gripper socket configuration (Robotiq URCap port 63352) - self.gripper_ip = gripper_ip or config.get('gripper.ip_address', self.robot_ip) - self.gripper_port = gripper_port or config.get('gripper.port', 63352) - self.gripper_timeout = config.get('gripper.timeout', 5.0) + self.gripper_ip = gripper_ip or config.get("gripper.ip_address", self.robot_ip) + self.gripper_port = gripper_port or config.get("gripper.port", 63352) + self.gripper_timeout = config.get("gripper.timeout", 5.0) self.gripper_socket = None print(f"🤖 Connecting to UR robot at {self.robot_ip}...") @@ -80,7 +92,9 @@ def __init__(self, robot_ip=None, speed=None, acceleration=None, read_only=False # Script client requires major/minor version - use common UR5e/UR10e versions # Most modern UR robots use control version 5.x - self.script_client = script_client.ScriptClient(self.robot_ip, 5, 11) # UR 5.11 + self.script_client = script_client.ScriptClient( + self.robot_ip, 5, 11 + ) # UR 5.11 self.script_client.connect() print("✅ Dashboard and Script clients connected") except Exception as e: @@ -102,9 +116,11 @@ def __init__(self, robot_ip=None, speed=None, acceleration=None, read_only=False def set_calibration_speed(self): """Set safe speeds for calibration movements to prevent protective stop""" - self.speed = config.get('robot.calibration_speed', 0.02) - self.acceleration = config.get('robot.calibration_acceleration', 0.1) - print(f"🐌 Calibration speeds set: {self.speed * 1000:.0f}mm/s, {self.acceleration * 1000:.0f}mm/s²") + self.speed = config.get("robot.calibration_speed", 0.02) + self.acceleration = config.get("robot.calibration_acceleration", 0.1) + print( + f"🐌 Calibration speeds set: {self.speed * 1000:.0f}mm/s, {self.acceleration * 1000:.0f}mm/s²" + ) def get_tcp_pose(self): """ @@ -165,7 +181,9 @@ def pose_to_joints(self, pose, current_joints=None): if current_joints is None: current_joints = self.get_joint_positions() - joints = self.rtde_c.getInverseKinematics(pose.tolist(), current_joints.tolist()) + joints = self.rtde_c.getInverseKinematics( + pose.tolist(), current_joints.tolist() + ) if joints is None: print("❌ No inverse kinematics solution found") @@ -285,17 +303,16 @@ def is_at_pose(self, target_pose, position_tolerance=None, rotation_tolerance=No bool: True if at target pose """ if position_tolerance is None: - position_tolerance = config.get('robot.position_tolerance', 0.001) + position_tolerance = config.get("robot.position_tolerance", 0.001) if rotation_tolerance is None: - rotation_tolerance = config.get('robot.rotation_tolerance', 0.01) + rotation_tolerance = config.get("robot.rotation_tolerance", 0.01) current_pose = self.get_tcp_pose() position_diff = np.linalg.norm(current_pose[:3] - target_pose[:3]) rotation_diff = np.linalg.norm(current_pose[3:] - target_pose[3:]) - return (position_diff < position_tolerance - and rotation_diff < rotation_tolerance) + return position_diff < position_tolerance and rotation_diff < rotation_tolerance def stop_motion(self): """Emergency stop robot motion""" @@ -334,8 +351,10 @@ def test_connection(self): def format_pose(self, pose): """Format pose for nice printing""" - return (f"[{pose[0]:.3f}, {pose[1]:.3f}, {pose[2]:.3f}, " - f"{pose[3]:.3f}, {pose[4]:.3f}, {pose[5]:.3f}]") + return ( + f"[{pose[0]:.3f}, {pose[1]:.3f}, {pose[2]:.3f}, " + f"{pose[3]:.3f}, {pose[4]:.3f}, {pose[5]:.3f}]" + ) def enable_freedrive(self): """ @@ -344,22 +363,14 @@ def enable_freedrive(self): Returns: bool: True if successful, False otherwise """ - if not self.dashboard_client: - print("❌ Dashboard client not available - cannot enable freedrive") + if not self.rtde_c: + print("❌ RTDE Control interface not available - cannot enable freedrive") return False try: - # Use Dashboard Client to unlock protective stop and enable freedrive - # First make sure robot is not in protective stop - unlock_result = self.dashboard_client.unlockProtectiveStop() - print(f"🔓 Unlock protective stop result: {unlock_result}") - - # Enable freedrive mode through Dashboard - freedrive_result = self.dashboard_client.brakeRelease() - print(f"🤲 Brake release result: {freedrive_result}") - - print("🤲 Freedrive mode enabled - you can now manually move the robot") - print(" Use disable_freedrive() when done positioning") + print("🔓 Enabling freedrive mode...") + self.rtde_c.teachMode() + print("✅ Freedrive enabled - you can now move the robot manually") return True except Exception as e: @@ -373,17 +384,14 @@ def disable_freedrive(self): Returns: bool: True if successful, False otherwise """ - if not self.dashboard_client: - print("❌ Dashboard client not available - cannot disable freedrive") + if not self.rtde_c: + print("❌ RTDE Control interface not available - cannot disable freedrive") return False try: - # Use Dashboard Client to close safety popup and re-engage brakes - close_popup_result = self.dashboard_client.closeSafetyPopup() - print(f"🔒 Close safety popup result: {close_popup_result}") - - print("🔒 Freedrive mode disabled - robot returned to normal operation") - print("⚠️ You may need to restart the robot program from the teach pendant") + print("🔒 Disabling freedrive mode...") + self.rtde_c.endTeachMode() + print("✅ Freedrive disabled - robot position locked") return True except Exception as e: @@ -422,7 +430,7 @@ def manual_position_teaching(self, position_name): # Confirm position confirm = input("💾 Save this position? (y/N): ").lower().strip() - if confirm != 'y': + if confirm != "y": print("❌ Position teaching cancelled") return False @@ -430,9 +438,9 @@ def manual_position_teaching(self, position_name): self.disable_freedrive() return { - 'pose': current_pose, - 'joints': current_joints, - 'name': position_name + "pose": current_pose, + "joints": current_joints, + "name": position_name, } except KeyboardInterrupt: @@ -521,14 +529,14 @@ def gripper_send_command(self, command): try: # Send command if isinstance(command, str): - command = command.encode('utf-8') + command = command.encode("utf-8") self.gripper_socket.send(command) print(f"📤 Sent gripper command: {command}") # Read response try: - response = self.gripper_socket.recv(1024).decode('utf-8').strip() + response = self.gripper_socket.recv(1024).decode("utf-8").strip() print(f"📥 Gripper response: {response}") return response except socket.timeout: @@ -677,7 +685,7 @@ def gripper_get_status(self): if response: try: # Parse response - format may vary by gripper model - status = {'raw_response': response} + status = {"raw_response": response} return status except Exception as e: print(f"❌ Failed to parse gripper status: {e}") @@ -697,13 +705,16 @@ def main(): """Test the UR robot interface""" import argparse - parser = argparse.ArgumentParser(description='UR Robot Interface Test') - parser.add_argument('--robot-ip', default=None, - help='Robot IP address (overrides config)') - parser.add_argument('--test-move', action='store_true', - help='Perform small test movement') - parser.add_argument('--test-gripper', action='store_true', - help='Test gripper functionality') + parser = argparse.ArgumentParser(description="UR Robot Interface Test") + parser.add_argument( + "--robot-ip", default=None, help="Robot IP address (overrides config)" + ) + parser.add_argument( + "--test-move", action="store_true", help="Perform small test movement" + ) + parser.add_argument( + "--test-gripper", action="store_true", help="Test gripper functionality" + ) args = parser.parse_args() @@ -711,7 +722,7 @@ def main(): print("=" * 50) try: - robot_ip = args.robot_ip or config.get('robot.ip_address', '192.168.0.10') + robot_ip = args.robot_ip or config.get("robot.ip_address", "192.168.0.10") with URController(robot_ip) as robot: if not robot.test_connection(): diff --git a/src/ur_toolkit/visual_servo/config.py b/src/ur_toolkit/visual_servo/config.py index 26714fe..bc31285 100644 --- a/src/ur_toolkit/visual_servo/config.py +++ b/src/ur_toolkit/visual_servo/config.py @@ -23,57 +23,74 @@ def __init__(self): @property def max_iterations(self) -> int: """Maximum number of correction iterations""" - return self.config.get('visual_servo.max_iterations', 3) + return self.config.get("visual_servo.max_iterations", 3) @property def detection_samples(self) -> int: """Number of detection samples for median filtering""" - return self.config.get('visual_servo.detection_samples', 5) + return self.config.get("visual_servo.detection_samples", 5) @property def position_tolerance(self) -> float: """Position tolerance for convergence (meters)""" - return self.config.get('visual_servo.position_tolerance', 0.002) + return self.config.get("visual_servo.position_tolerance", 0.002) @property def rotation_tolerance(self) -> float: """Rotation tolerance for convergence (radians)""" - return self.config.get('visual_servo.rotation_tolerance', 0.017) # ~1 degree + return self.config.get("visual_servo.rotation_tolerance", 0.017) # ~1 degree @property def max_translation_correction(self) -> float: """Maximum allowed translation correction per iteration (meters)""" - return self.config.get('visual_servo.safety_limits.max_translation', 0.05) + return self.config.get("visual_servo.safety_limits.max_translation", 0.05) @property def max_rotation_correction(self) -> float: """Maximum allowed rotation correction per iteration (radians)""" - return self.config.get('visual_servo.safety_limits.max_rotation', 0.35) # ~20 degrees + return self.config.get( + "visual_servo.safety_limits.max_rotation", 0.35 + ) # ~20 degrees @property def max_total_correction(self) -> float: """Maximum total correction across all iterations (meters)""" - return self.config.get('visual_servo.safety_limits.max_total_correction', 0.15) + return self.config.get("visual_servo.safety_limits.max_total_correction", 0.15) @property def damping_factor(self) -> float: """Damping factor for corrections to prevent oscillation""" - return self.config.get('visual_servo.damping_factor', 0.7) + return self.config.get("visual_servo.damping_factor", 0.7) + + @property + def enable_rotation(self) -> bool: + """Whether to apply rotation corrections during visual servoing""" + return self.config.get("visual_servo.enable_rotation", True) + + @property + def enabled(self) -> bool: + """Global master enable for visual servoing (skip entirely when False)""" + return self.config.get("visual_servo.enabled", True) + + @property + def reset_positions_on_start(self) -> bool: + """Whether to revert propagated visual-servo updates at engine startup""" + return self.config.get("visual_servo.reset_positions_on_start", False) @property def enable_pose_history(self) -> bool: """Whether to maintain pose correction history""" - return self.config.get('visual_servo.enable_pose_history', True) + return self.config.get("visual_servo.enable_pose_history", True) @property def max_history_entries(self) -> int: """Maximum number of pose history entries to keep""" - return self.config.get('visual_servo.max_history_entries', 50) + return self.config.get("visual_servo.max_history_entries", 50) @property def detection_timeout(self) -> float: """Timeout for AprilTag detection (seconds)""" - return self.config.get('visual_servo.detection_timeout', 5.0) + return self.config.get("visual_servo.detection_timeout", 5.0) def print_config(self): """Print current visual servoing configuration""" @@ -87,21 +104,27 @@ def print_config(self): print(f" Max total correction: {self.max_total_correction:.3f}m") print(f" Damping factor: {self.damping_factor:.2f}") print(f" Pose history enabled: {self.enable_pose_history}") + print(f" Rotation corrections enabled: {self.enable_rotation}") + print(f" Visual servo globally enabled: {self.enabled}") + print(f" Reset positions on start: {self.reset_positions_on_start}") print(f" Detection timeout: {self.detection_timeout}s") def get_all_config(self): """Get all visual servo configuration as dictionary""" return { - 'max_iterations': self.max_iterations, - 'detection_samples': self.detection_samples, - 'position_tolerance': self.position_tolerance, - 'rotation_tolerance': self.rotation_tolerance, - 'max_translation_correction': self.max_translation_correction, - 'max_rotation_correction': self.max_rotation_correction, - 'max_total_correction': self.max_total_correction, - 'enable_pose_history': self.enable_pose_history, - 'max_history_entries': self.max_history_entries, - 'detection_timeout': self.detection_timeout + "max_iterations": self.max_iterations, + "detection_samples": self.detection_samples, + "position_tolerance": self.position_tolerance, + "rotation_tolerance": self.rotation_tolerance, + "max_translation_correction": self.max_translation_correction, + "max_rotation_correction": self.max_rotation_correction, + "max_total_correction": self.max_total_correction, + "enable_pose_history": self.enable_pose_history, + "enable_rotation": self.enable_rotation, + "enabled": self.enabled, + "reset_positions_on_start": self.reset_positions_on_start, + "max_history_entries": self.max_history_entries, + "detection_timeout": self.detection_timeout, } diff --git a/src/ur_toolkit/visual_servo/pose_history.py b/src/ur_toolkit/visual_servo/pose_history.py index 2482336..f60bade 100644 --- a/src/ur_toolkit/visual_servo/pose_history.py +++ b/src/ur_toolkit/visual_servo/pose_history.py @@ -31,9 +31,15 @@ def __init__(self, positions_file: Path, config): # Load existing history self.correction_history = self._load_history() - def record_correction(self, position_name: str, original_pose: np.ndarray, - corrected_pose: np.ndarray, tag_pose_original: np.ndarray, - tag_pose_current: np.ndarray, correction_metrics: Dict[str, Any]): + def record_correction( + self, + position_name: str, + original_pose: np.ndarray, + corrected_pose: np.ndarray, + tag_pose_original: np.ndarray, + tag_pose_current: np.ndarray, + correction_metrics: Dict[str, Any], + ): """ Record a pose correction in history @@ -52,15 +58,15 @@ def record_correction(self, position_name: str, original_pose: np.ndarray, # Create correction record correction_record = { - 'timestamp': timestamp, - 'position_name': position_name, - 'original_robot_pose': original_pose.tolist(), - 'corrected_robot_pose': corrected_pose.tolist(), - 'original_tag_pose': tag_pose_original.tolist(), - 'current_tag_pose': tag_pose_current.tolist(), - 'correction_delta': (corrected_pose - original_pose).tolist(), - 'tag_delta': (tag_pose_current - tag_pose_original).tolist(), - 'metrics': correction_metrics + "timestamp": timestamp, + "position_name": position_name, + "original_robot_pose": original_pose.tolist(), + "corrected_robot_pose": corrected_pose.tolist(), + "original_tag_pose": tag_pose_original.tolist(), + "current_tag_pose": tag_pose_current.tolist(), + "correction_delta": (corrected_pose - original_pose).tolist(), + "tag_delta": (tag_pose_current - tag_pose_original).tolist(), + "metrics": correction_metrics, } # Add to history @@ -72,15 +78,132 @@ def record_correction(self, position_name: str, original_pose: np.ndarray, # Limit history size max_entries = self.config.max_history_entries if len(self.correction_history[position_name]) > max_entries: - self.correction_history[position_name] = self.correction_history[position_name][-max_entries:] + self.correction_history[position_name] = self.correction_history[ + position_name + ][-max_entries:] # Save to file self._save_history() print(f"📝 Recorded correction for '{position_name}' at {timestamp}") - def update_position_pose(self, position_name: str, new_pose: np.ndarray, - new_tag_pose: Optional[np.ndarray] = None) -> bool: + def erase_history(self, position_name: Optional[str] = None) -> bool: + """Erase correction history. + + Args: + position_name: If provided, only erase history for this position; otherwise erase all. + + Returns: + True if any history was erased. + """ + if position_name: + if position_name in self.correction_history: + del self.correction_history[position_name] + self._save_history() + print(f"🧨 Erased correction history for '{position_name}'") + return True + else: + print(f"ℹ️ No history found for '{position_name}' to erase") + return False + else: + if self.correction_history: + self.correction_history = {} + self._save_history() + print("🧨 Erased ALL correction history entries") + return True + print("ℹ️ No correction history entries to erase") + return False + + def snapshot_positions( + self, snapshot_name: str = "taught_positions.snapshot.yaml" + ) -> Optional[Path]: + """Create a snapshot copy of current taught positions for later restore. + + Returns path to snapshot file or None on failure. + """ + try: + data = self._load_positions() + snap_path = self.positions_file.parent / snapshot_name + with open(snap_path, "w") as f: + yaml.dump(data, f, default_flow_style=False, sort_keys=False, indent=2) + print(f"📸 Snapshot saved: {snap_path}") + return snap_path + except Exception as e: + print(f"❌ Failed to create snapshot: {e}") + return None + + def restore_positions_from_snapshot( + self, + snapshot_name: str = "taught_positions.snapshot.yaml", + overwrite_existing: bool = True, + ) -> bool: + """Restore taught positions from a snapshot file. + + Args: + snapshot_name: Snapshot filename stored alongside positions file. + overwrite_existing: If True, overwrite current taught positions file. + """ + snap_path = self.positions_file.parent / snapshot_name + if not snap_path.exists(): + print(f"❌ Snapshot file not found: {snap_path}") + return False + try: + with open(snap_path, "r") as f: + snap_data = yaml.safe_load(f) or {} + if overwrite_existing: + with open(self.positions_file, "w") as f: + yaml.dump( + snap_data, + f, + default_flow_style=False, + sort_keys=False, + indent=2, + ) + print(f"♻️ Restored taught positions from snapshot {snap_path}") + return True + except Exception as e: + print(f"❌ Failed to restore from snapshot: {e}") + return False + + def strip_propagation_metadata(self) -> int: + """Remove propagation-related metadata fields from positions (Option B reversal without changing coordinates). + + Returns number of positions cleaned. + """ + try: + data = self._load_positions() + pos_block = data.get("positions", {}) + removed = 0 + meta_fields = [ + "last_observation_propagation", + "propagated_from", + "propagation_offset_used", + "last_visual_servo_update", + "visual_servo_correction_applied", + ] + for name, p in pos_block.items(): + for mf in meta_fields: + if mf in p: + del p[mf] + removed += 1 + if removed: + self._save_positions(data) + print( + f"🧹 Removed {removed} metadata field entries related to propagation/servo updates" + ) + else: + print("ℹ️ No propagation metadata found to remove") + return removed + except Exception as e: + print(f"❌ Failed to strip propagation metadata: {e}") + return 0 + + def update_position_pose( + self, + position_name: str, + new_pose: np.ndarray, + new_tag_pose: Optional[np.ndarray] = None, + ) -> bool: """ Update stored position with corrected pose @@ -96,22 +219,30 @@ def update_position_pose(self, position_name: str, new_pose: np.ndarray, # Load current positions positions_data = self._load_positions() - if position_name not in positions_data.get('positions', {}): + if position_name not in positions_data.get("positions", {}): print(f"❌ Position '{position_name}' not found") return False # Create backup of original - original_pose = copy.deepcopy(positions_data['positions'][position_name]['coordinates']) + original_pose = copy.deepcopy( + positions_data["positions"][position_name]["coordinates"] + ) # Update the pose - positions_data['positions'][position_name]['coordinates'] = new_pose.tolist() + positions_data["positions"][position_name][ + "coordinates" + ] = new_pose.tolist() # Update tag pose if provided if new_tag_pose is not None: - positions_data['positions'][position_name]['camera_to_tag'] = new_tag_pose.tolist() + positions_data["positions"][position_name][ + "camera_to_tag" + ] = new_tag_pose.tolist() # Add update metadata - positions_data['positions'][position_name]['last_visual_servo_update'] = datetime.now().isoformat() + positions_data["positions"][position_name][ + "last_visual_servo_update" + ] = datetime.now().isoformat() # Save updated positions self._save_positions(positions_data) @@ -137,26 +268,28 @@ def get_correction_statistics(self, position_name: str) -> Dict[str, Any]: Dictionary with correction statistics """ if position_name not in self.correction_history: - return {'correction_count': 0} + return {"correction_count": 0} corrections = self.correction_history[position_name] if not corrections: - return {'correction_count': 0} + return {"correction_count": 0} # Calculate statistics - correction_deltas = [np.array(c['correction_delta']) for c in corrections] + correction_deltas = [np.array(c["correction_delta"]) for c in corrections] translation_magnitudes = [np.linalg.norm(d[:3]) for d in correction_deltas] rotation_magnitudes = [np.linalg.norm(d[3:]) for d in correction_deltas] stats = { - 'correction_count': len(corrections), - 'last_correction': corrections[-1]['timestamp'], - 'avg_translation_correction': np.mean(translation_magnitudes), - 'max_translation_correction': np.max(translation_magnitudes), - 'avg_rotation_correction': np.mean(rotation_magnitudes), - 'max_rotation_correction': np.max(rotation_magnitudes), - 'recent_corrections': len([c for c in corrections if self._is_recent(c['timestamp'])]) + "correction_count": len(corrections), + "last_correction": corrections[-1]["timestamp"], + "avg_translation_correction": np.mean(translation_magnitudes), + "max_translation_correction": np.max(translation_magnitudes), + "avg_rotation_correction": np.mean(rotation_magnitudes), + "max_rotation_correction": np.max(rotation_magnitudes), + "recent_corrections": len( + [c for c in corrections if self._is_recent(c["timestamp"])] + ), } return stats @@ -164,7 +297,7 @@ def get_correction_statistics(self, position_name: str) -> Dict[str, Any]: def _load_positions(self) -> Dict[str, Any]: """Load current taught positions""" try: - with open(self.positions_file, 'r') as f: + with open(self.positions_file, "r") as f: return yaml.safe_load(f) or {} except Exception as e: print(f"❌ Failed to load positions: {e}") @@ -173,8 +306,14 @@ def _load_positions(self) -> Dict[str, Any]: def _save_positions(self, positions_data: Dict[str, Any]): """Save updated positions""" try: - with open(self.positions_file, 'w') as f: - yaml.dump(positions_data, f, default_flow_style=False, sort_keys=False, indent=2) + with open(self.positions_file, "w") as f: + yaml.dump( + positions_data, + f, + default_flow_style=False, + sort_keys=False, + indent=2, + ) except Exception as e: print(f"❌ Failed to save positions: {e}") @@ -182,7 +321,7 @@ def _load_history(self) -> Dict[str, List[Dict]]: """Load correction history from file""" try: if self.history_file.exists(): - with open(self.history_file, 'r') as f: + with open(self.history_file, "r") as f: return json.load(f) except Exception as e: print(f"⚠️ Failed to load history: {e}") @@ -192,7 +331,7 @@ def _load_history(self) -> Dict[str, List[Dict]]: def _save_history(self): """Save correction history to file""" try: - with open(self.history_file, 'w') as f: + with open(self.history_file, "w") as f: json.dump(self.correction_history, f, indent=2) except Exception as e: print(f"❌ Failed to save history: {e}") @@ -212,16 +351,22 @@ def print_history_summary(self, position_name: Optional[str] = None): stats = self.get_correction_statistics(position_name) print(f"📊 Correction History for '{position_name}':") print(f" Total corrections: {stats['correction_count']}") - if stats['correction_count'] > 0: + if stats["correction_count"] > 0: print(f" Last correction: {stats['last_correction']}") - print(f" Avg translation: {stats['avg_translation_correction']:.4f}m") - print(f" Max translation: {stats['max_translation_correction']:.4f}m") + print( + f" Avg translation: {stats['avg_translation_correction']:.4f}m" + ) + print( + f" Max translation: {stats['max_translation_correction']:.4f}m" + ) print(f" Recent corrections (24h): {stats['recent_corrections']}") else: print(f"📊 No correction history for '{position_name}'") else: print("📊 Overall Correction History:") - total_corrections = sum(len(corrections) for corrections in self.correction_history.values()) + total_corrections = sum( + len(corrections) for corrections in self.correction_history.values() + ) print(f" Positions with corrections: {len(self.correction_history)}") print(f" Total corrections: {total_corrections}") @@ -229,8 +374,9 @@ def print_history_summary(self, position_name: Optional[str] = None): stats = self.get_correction_statistics(pos_name) print(f" {pos_name}: {stats['correction_count']} corrections") - def update_position_tag_association(self, position_name: str, tag_reference: str, - camera_to_tag_transform: list) -> bool: + def update_position_tag_association( + self, position_name: str, tag_reference: str, camera_to_tag_transform: list + ) -> bool: """ Update a position with AprilTag association for visual servoing @@ -246,28 +392,36 @@ def update_position_tag_association(self, position_name: str, tag_reference: str # Load current positions positions_data = self._load_positions() - if position_name not in positions_data.get('positions', {}): + if position_name not in positions_data.get("positions", {}): print(f"❌ Position '{position_name}' not found") return False # Update the position with AprilTag association - positions_data['positions'][position_name]['tag_reference'] = tag_reference - positions_data['positions'][position_name]['camera_to_tag'] = camera_to_tag_transform - positions_data['positions'][position_name]['has_apriltag_view'] = True - positions_data['positions'][position_name]['visual_servo_enabled'] = True - positions_data['positions'][position_name]['last_tag_association_update'] = datetime.now().isoformat() + positions_data["positions"][position_name]["tag_reference"] = tag_reference + positions_data["positions"][position_name][ + "camera_to_tag" + ] = camera_to_tag_transform + positions_data["positions"][position_name]["has_apriltag_view"] = True + positions_data["positions"][position_name]["visual_servo_enabled"] = True + positions_data["positions"][position_name][ + "last_tag_association_update" + ] = datetime.now().isoformat() # Save updated positions self._save_positions(positions_data) - print(f"✅ Updated position '{position_name}' with AprilTag association: {tag_reference}") + print( + f"✅ Updated position '{position_name}' with AprilTag association: {tag_reference}" + ) return True except Exception as e: print(f"❌ Failed to update tag association for '{position_name}': {e}") return False - def update_equipment_positions(self, position_name: str, pose_correction: np.ndarray) -> bool: + def update_equipment_positions( + self, position_name: str, pose_correction: np.ndarray + ) -> bool: """ Update all positions associated with the same equipment when visual servoing detects an offset @@ -282,41 +436,51 @@ def update_equipment_positions(self, position_name: str, pose_correction: np.nda # Load current positions positions_data = self._load_positions() - if position_name not in positions_data.get('positions', {}): + if position_name not in positions_data.get("positions", {}): print(f"❌ Position '{position_name}' not found") return False # Get the equipment name of the corrected position - corrected_position = positions_data['positions'][position_name] - equipment_name = corrected_position.get('equipment_name') + corrected_position = positions_data["positions"][position_name] + equipment_name = corrected_position.get("equipment_name") if not equipment_name: - print(f"⚠️ Position '{position_name}' has no equipment_name, only updating this position") - return self.update_position_pose(position_name, - np.array(corrected_position['coordinates']) + pose_correction) + print( + f"⚠️ Position '{position_name}' has no equipment_name, only updating this position" + ) + return self.update_position_pose( + position_name, + np.array(corrected_position["coordinates"]) + pose_correction, + ) print(f"🔧 Updating all positions for equipment: {equipment_name}") # Find all positions with the same equipment_name updated_positions = [] - for pos_name, pos_data in positions_data['positions'].items(): - if pos_data.get('equipment_name') == equipment_name: + for pos_name, pos_data in positions_data["positions"].items(): + if pos_data.get("equipment_name") == equipment_name: # Apply the same correction to this position - original_coords = np.array(pos_data['coordinates']) + original_coords = np.array(pos_data["coordinates"]) corrected_coords = original_coords + pose_correction # Update the coordinates - pos_data['coordinates'] = corrected_coords.tolist() - pos_data['last_visual_servo_update'] = datetime.now().isoformat() - pos_data['visual_servo_correction_applied'] = pose_correction.tolist() + pos_data["coordinates"] = corrected_coords.tolist() + pos_data["last_visual_servo_update"] = datetime.now().isoformat() + pos_data["visual_servo_correction_applied"] = ( + pose_correction.tolist() + ) updated_positions.append(pos_name) - print(f" ✅ Updated {pos_name}: {original_coords} → {corrected_coords}") + print( + f" ✅ Updated {pos_name}: {original_coords} → {corrected_coords}" + ) # Save the updated positions file self._save_positions(positions_data) - print(f"💾 Successfully updated {len(updated_positions)} positions for equipment '{equipment_name}'") + print( + f"💾 Successfully updated {len(updated_positions)} positions for equipment '{equipment_name}'" + ) print(f" Updated positions: {updated_positions}") return True diff --git a/src/ur_toolkit/visual_servo/visual_servo_engine.py b/src/ur_toolkit/visual_servo/visual_servo_engine.py index 6106f3c..ebed25d 100644 --- a/src/ur_toolkit/visual_servo/visual_servo_engine.py +++ b/src/ur_toolkit/visual_servo/visual_servo_engine.py @@ -6,9 +6,14 @@ import numpy as np import time +import json from typing import Optional, Tuple, Dict, Any from pathlib import Path import sys +from datetime import datetime +import logging +import math +import csv # Add parent directories to path sys.path.insert(0, str(Path(__file__).parent.parent)) @@ -17,7 +22,45 @@ from .config import visual_servo_config from .detection_filter import DetectionFilter from .pose_history import PoseHistoryManager -from apriltag_detection import AprilTagDetector +from ur_toolkit.apriltag_detection import AprilTagDetector + + +def transform_camera_to_robot_correction(camera_error_translation, camera_error_rotation, enable_rotation=True): + """ + Transform camera coordinate errors to robot coordinate corrections. + + Camera frame (OpenCV standard): X-right, Y-down, Z-forward (depth) + Robot TCP frame (UR standard): X-forward, Y-left, Z-up + + Assuming eye-in-hand camera mounted looking forward from TCP. + This transformation maps camera errors to proper robot movements. + + Args: + camera_error_translation: [x, y, z] errors in camera frame + camera_error_rotation: [rx, ry, rz] rotation errors in camera frame + + Returns: + (robot_translation, robot_rotation) corrections in robot TCP frame + """ + # Translation mapping - FIXED Z direction + # Camera Z error positive = tag farther = robot should move forward (+Y) + # Camera Z error negative = tag closer = robot should move backward (-Y) + robot_translation = [ + -camera_error_translation[0], # Camera X -> Robot -X (right becomes left) + -camera_error_translation[2], # Camera Z (depth) -> Robot -Y (forward/back) - INVERTED + -camera_error_translation[1] # Camera Y (down) -> Robot -Z (down becomes up) + ] + + if enable_rotation: + robot_rotation = [ + -camera_error_rotation[0], # Camera RX -> Robot -RX + -camera_error_rotation[2], # Camera RZ -> Robot -RY + -camera_error_rotation[1] # Camera RY -> Robot -RZ + ] + else: + robot_rotation = [0.0, 0.0, 0.0] + + return robot_translation, robot_rotation class EyeInHandPIDController: @@ -67,7 +110,8 @@ def reset(self): self.previous_error = None -from camera.picam import PiCam +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.config_manager import get_camera_host, get_camera_port class VisualServoEngine: @@ -94,7 +138,10 @@ def __init__(self, robot_controller, positions_file: Path, apriltag_detector=Non # Initialize camera if not provided if camera is None: - self.camera = PiCam() + host = get_camera_host() + port = get_camera_port() + config = PiCamConfig(hostname=host, port=port) + self.camera = PiCam(config) else: self.camera = camera @@ -102,6 +149,21 @@ def __init__(self, robot_controller, positions_file: Path, apriltag_detector=Non self.detection_filter = DetectionFilter(self.detector, self.camera, self.config) self.pose_history = PoseHistoryManager(positions_file, self.config) + # Basic logger fallback (minimal, avoids dependency on external logger setup) + self.logger = logging.getLogger("visual_servo_engine") + if not self.logger.handlers: + handler = logging.StreamHandler(sys.stdout) + handler.setFormatter(logging.Formatter("[VISUAL_SERVO] %(levelname)s: %(message)s")) + self.logger.addHandler(handler) + self.logger.setLevel(logging.INFO) + + # Optional reset of propagated positions + if self.config.reset_positions_on_start: + try: + self._reset_propagated_positions(positions_file) + except Exception as e: + print(f"⚠️ Failed to reset propagated positions: {e}") + # Initialize PID controllers for eye-in-hand visual servoing # VERY conservative gains - the system was diverging with aggressive gains # Separate controllers for translation and rotation @@ -124,10 +186,74 @@ def __init__(self, robot_controller, positions_file: Path, apriltag_detector=Non self.error_history = [] self.max_error_history = 3 + # Load hand-eye calibration if available + self.hand_eye_transform = self._load_hand_eye_calibration() + print("🎯 Visual Servo Engine initialized") self.config.print_config() - def visual_servo_to_position(self, position_name: str, update_stored_pose: bool = True) -> Tuple[bool, Dict[str, Any]]: + # Setup optional iteration logging + self.iteration_log_enabled = bool(getattr(self.config, 'enable_iteration_logging', False)) + self.iteration_log_path = Path("logs/visual_servo_iterations.csv") + if self.iteration_log_enabled: + try: + if not self.iteration_log_path.parent.exists(): + self.iteration_log_path.parent.mkdir(parents=True, exist_ok=True) + if not self.iteration_log_path.exists(): + with open(self.iteration_log_path, 'w', newline='') as f: + writer = csv.writer(f) + writer.writerow([ + 'timestamp', 'position', 'method', 'iteration', 'phase', 'success', + 'trans_err_x', 'trans_err_y', 'trans_err_z', + 'rot_err_axis_x', 'rot_err_axis_y', 'rot_err_axis_z', 'rot_err_angle', + 'rotation_flip_suppressed', 'translation_norm', 'rotation_angle', 'applied_corr_norm' + ]) + abs_path = self.iteration_log_path.resolve() + print(f"📝 Iteration logging enabled → {self.iteration_log_path} (absolute: {abs_path})") + except Exception as e: + print(f"⚠️ Failed to initialize iteration logging: {e}") + self.iteration_log_enabled = False + + def _log_iteration_event( + self, + position_name: str, + method: str, + iteration: int, + phase: str, + success: bool, + translation_error: Optional[np.ndarray] = None, + rot_axis: Optional[np.ndarray] = None, + rot_angle: Optional[float] = None, + rotation_flip_suppressed: bool = False, + applied_correction: Optional[np.ndarray] = None, + ): + """Write a movement/detection/correction event to the iteration CSV. + + Parameters kept optional so we can log early failures before errors are computed. + Missing numeric values are written as blank strings for easy CSV import handling. + """ + if not self.iteration_log_enabled: + return + try: + trans_err = translation_error if translation_error is not None else np.array(['', '', '']) + axis = rot_axis if rot_axis is not None else np.array(['', '', '']) + angle_val = rot_angle if rot_angle is not None else '' + translation_norm = float(np.linalg.norm(translation_error)) if translation_error is not None else '' + rotation_angle_scalar = rot_angle if rot_angle is not None else '' + corr_norm = float(np.linalg.norm(applied_correction)) if applied_correction is not None else '' + with open(self.iteration_log_path, 'a', newline='') as f: + writer = csv.writer(f) + writer.writerow([ + datetime.now().isoformat(), position_name, method, iteration, phase, int(success), + trans_err[0], trans_err[1], trans_err[2], + axis[0], axis[1], axis[2], angle_val, + int(rotation_flip_suppressed), translation_norm, rotation_angle_scalar, corr_norm + ]) + except Exception: + # Fail silently after first warning to avoid spamming + pass + + def visual_servo_to_position(self, position_name: str, update_stored_pose: bool = False) -> Tuple[bool, Dict[str, Any]]: """ Perform visual servoing to a taught position with AprilTag reference @@ -209,6 +335,8 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] # Move to current estimated pose print("🤖 Moving to estimated pose...") success = self.robot.move_to_pose(current_robot_pose) + # Log movement attempt immediately (no errors yet) + self._log_iteration_event(position_name, 'direct', iteration + 1, 'move', success) if not success: print("❌ Failed to move robot to pose") return False, metrics @@ -217,31 +345,70 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] current_tag_pose = self.detection_filter.get_filtered_tag_pose(tag_id) if current_tag_pose is None: print(f"❌ Failed to detect AprilTag {tag_id}") + # Log detection failure phase + self._log_iteration_event(position_name, 'direct', iteration + 1, 'detect', False) return False, metrics - # Calculate pose error - tag_error = current_tag_pose - stored_tag_pose - pose_error_magnitude = np.linalg.norm(tag_error) - - print(f"📏 Tag pose error magnitude: {pose_error_magnitude:.4f}") - print(f" Translation error: [{tag_error[0]:.4f}, {tag_error[1]:.4f}, {tag_error[2]:.4f}]m") - print(f" Rotation error: [{tag_error[3]:.4f}, {tag_error[4]:.4f}, {tag_error[5]:.4f}]rad") - - # Check convergence - if (np.linalg.norm(tag_error[:3]) < self.config.position_tolerance - and np.linalg.norm(tag_error[3:]) < self.config.rotation_tolerance): - print("✅ Converged within tolerance") - metrics['converged'] = True - metrics['final_error'] = pose_error_magnitude - break + # Calculate translation error (direct subtraction is fine for small displacements) + raw_error = current_tag_pose - stored_tag_pose + translation_error = raw_error[:3] + + # Proper relative rotation computation using axis-angle + stored_euler = stored_tag_pose[3:] + current_euler = current_tag_pose[3:] + R_stored = self._euler_to_matrix(*stored_euler) + R_current = self._euler_to_matrix(*current_euler) + R_rel = R_current @ R_stored.T + rot_axis, rot_angle = self._matrix_to_axis_angle(R_rel) + + # 180° flip heuristic: if angle near pi but translation small, treat as wrap ambiguity + rotation_flip_suppressed = False + if rot_angle > math.pi * 0.9 and np.linalg.norm(translation_error) < 0.02: + # Suppress large ambiguous rotation: map to minimal equivalent (flip axis) + rotation_flip_suppressed = True + rot_angle = (2 * math.pi - rot_angle) + rot_axis = -rot_axis + print("⚪ Detected near-π rotation ambiguity -> applying flip suppression") + + # Compose axis-angle into error vector (axis * angle) for correction step + rotation_error_vec = rot_axis * rot_angle + + # Combined magnitude (translation norm + rotation angle for logging) + translation_norm = np.linalg.norm(translation_error) + pose_error_magnitude = math.sqrt(translation_norm**2 + rot_angle**2) + + print("📏 Error summary:") + print(f" Translation error: [{translation_error[0]:.4f}, {translation_error[1]:.4f}, {translation_error[2]:.4f}]m (norm {translation_norm:.4f}m)") + print(f" Rotation axis: [{rot_axis[0]:.3f}, {rot_axis[1]:.3f}, {rot_axis[2]:.3f}] angle {rot_angle:.4f}rad") + if rotation_flip_suppressed: + print(f" ℹ️ Flip suppression applied; adjusted angle {rot_angle:.4f}rad") + + # Determine rotation enable before convergence logic + enable_rot = bool(getattr(self.config, 'enable_rotation', True)) + # Convergence logic + translation_err_norm = translation_norm + rotation_err_norm = rot_angle # use scalar angle instead of Euler component norm + if not enable_rot: + # Translation-only convergence when rotation corrections disabled + if translation_err_norm < self.config.position_tolerance: + print("✅ Converged (translation-only; rotation ignored)") + metrics['converged'] = True + metrics['final_error'] = translation_err_norm + break + else: + if (translation_err_norm < self.config.position_tolerance and rotation_err_norm < self.config.rotation_tolerance): + print("✅ Converged within tolerance") + metrics['converged'] = True + metrics['final_error'] = pose_error_magnitude + break # Calculate robot pose correction using eye-in-hand visual servoing control # Eye-in-hand IBVS: camera moves with end-effector, so control law is different # than eye-to-hand setup # Extract translation and rotation errors separately - tag_translation_error = tag_error[:3] # [x, y, z] in camera frame - tag_rotation_error = tag_error[3:] # [rx, ry, rz] in camera frame + tag_translation_error = translation_error # camera frame translation + tag_rotation_error = rotation_error_vec # axis-angle vector (camera frame approximation) # Calculate time step for PID controllers current_time = time.time() @@ -285,7 +452,7 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] if len(self.error_history) >= 2: # If error is consistently increasing, reduce gains automatically if self.error_history[-1] > self.error_history[-2] * 1.15: - self.logger.warning("Error trend increasing - reducing PID gains automatically") + print("⚠️ Error trend increasing - auto-reducing PID gains") for pid in self.translation_pids + self.rotation_pids: pid.kp *= 0.9 # Reduce proportional gain more gently pid.output_limit *= 0.95 # Reduce output limits more gently @@ -293,9 +460,14 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] # Apply simple direct correction - transform tag error to robot movement # For eye-in-hand, camera frame aligns with end-effector frame # Apply opposite movement to correct visual error - + # Respect global enable_rotation flag (already determined earlier) + if not enable_rot: + # Suppress rotation error entirely when disabled + tag_rotation_error = np.zeros(3) + if iteration == 0: # Log once prominently at first iteration + print("🛑 Rotation corrections disabled (enable_rotation=False) - translation only mode") robot_translation_correction = -tag_translation_error # Opposite direction - robot_rotation_correction = -tag_rotation_error # Opposite direction + robot_rotation_correction = -tag_rotation_error # Opposite direction (zero if disabled) # Combine corrections robot_correction = np.concatenate([robot_translation_correction, robot_rotation_correction]) @@ -315,15 +487,31 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] robot_correction[i] = np.clip(robot_correction[i], -max_rotation_velocity, max_rotation_velocity) - # Apply overall damping factor for stability - robot_correction *= self.config.damping_factor - - print(f"🔧 Eye-in-hand visual servoing control (dt={dt:.3f}s):") - print(f" Translation errors: [{tag_translation_error[0]:.4f}, {tag_translation_error[1]:.4f}, {tag_translation_error[2]:.4f}]") - print(f" Rotation errors: [{tag_rotation_error[0]:.4f}, {tag_rotation_error[1]:.4f}, {tag_rotation_error[2]:.4f}]") - print(f" Translation correction: [{robot_translation_correction[0]:.4f}, {robot_translation_correction[1]:.4f}, {robot_translation_correction[2]:.4f}]") - print(f" Rotation correction: [{robot_rotation_correction[0]:.4f}, {robot_rotation_correction[1]:.4f}, {robot_rotation_correction[2]:.4f}]") - print(f" Overall damping: {self.config.damping_factor:.2f}") + # Check if using simple legacy-style mode + if hasattr(self.config, 'simple_mode') and self.config.simple_mode: + # Simple mode: only XY translation corrections (like legacy system) + simple_correction = np.zeros(6) + simple_correction[0] = -tag_translation_error[0] * 0.4 # X correction + simple_correction[1] = -tag_translation_error[1] * 0.4 # Y correction + # Z, RX, RY, RZ remain zero for stability + robot_correction = simple_correction + + print("🔧 Simple XY-only correction (legacy style):") + print(f" Translation errors: [{tag_translation_error[0]:.4f}, {tag_translation_error[1]:.4f}, {tag_translation_error[2]:.4f}]") + print(f" Applied XY correction: [{robot_correction[0]:.4f}, {robot_correction[1]:.4f}, 0.0000] (Z/rotations=0)") + print(" Simple mode damping: 0.400") + else: + # Apply overall damping factor for stability + robot_correction *= self.config.damping_factor + + print(f"🔧 Eye-in-hand visual servoing control (dt={dt:.3f}s):") + print(f" Translation errors: [{tag_translation_error[0]:.4f}, {tag_translation_error[1]:.4f}, {tag_translation_error[2]:.4f}]") + print(f" Rotation errors: [{tag_rotation_error[0]:.4f}, {tag_rotation_error[1]:.4f}, {tag_rotation_error[2]:.4f}]") + print(f" Translation correction: [{robot_translation_correction[0]:.4f}, {robot_translation_correction[1]:.4f}, {robot_translation_correction[2]:.4f}]") + print(f" Rotation correction: [{robot_rotation_correction[0]:.4f}, {robot_rotation_correction[1]:.4f}, {robot_rotation_correction[2]:.4f}]") + print(f" Overall damping: {self.config.damping_factor:.2f}") + if not enable_rot: + print(" ℹ️ Rotation terms forced to zero (enable_rotation=False)") # Apply safety limits correction_valid, safety_metrics = self._validate_correction( @@ -343,20 +531,36 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] metrics['corrections_applied'].append({ 'iteration': iteration + 1, - 'tag_error': tag_error.tolist(), + 'translation_error': translation_error.tolist(), + 'rotation_axis': rot_axis.tolist(), + 'rotation_angle': rot_angle, + 'rotation_flip_suppressed': rotation_flip_suppressed, 'robot_correction': robot_correction.tolist(), 'correction_magnitude': correction_magnitude }) + # Optional per-iteration CSV logging + # Log correction phase + self._log_iteration_event(position_name, 'direct', iteration + 1, 'correction', True, + translation_error=translation_error, + rot_axis=rot_axis, + rot_angle=rot_angle, + rotation_flip_suppressed=rotation_flip_suppressed, + applied_correction=robot_correction) + # Final metrics metrics['total_correction'] = total_correction.tolist() metrics['final_robot_pose'] = current_robot_pose.tolist() if not metrics['converged']: print(f"⚠️ Did not converge within {self.config.max_iterations} iterations") - print(f" Final error: {pose_error_magnitude:.4f}") - print(f" Position tolerance: {self.config.position_tolerance:.4f}m") - print(f" Rotation tolerance: {self.config.rotation_tolerance:.4f}rad") + if enable_rot: + print(f" Final combined error magnitude: {pose_error_magnitude:.4f}") + print(f" Translation error norm: {translation_err_norm:.4f}m (tol {self.config.position_tolerance:.4f}m)") + print(f" Rotation error norm: {rotation_err_norm:.4f}rad (tol {self.config.rotation_tolerance:.4f}rad)") + else: + print(f" Final translation error norm: {translation_err_norm:.4f}m (tol {self.config.position_tolerance:.4f}m)") + print(" Rotation ignored (enable_rotation=False)") # Ask user if they want to continue anyway response = input("\n🤔 Continue with current position anyway? (y/n): ").lower().strip() @@ -368,33 +572,45 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] print("❌ Visual servoing marked as failed") metrics['user_override'] = False - metrics['final_error'] = pose_error_magnitude + metrics['final_error'] = translation_err_norm if not enable_rot else pose_error_magnitude # Record in history self.pose_history.record_correction( position_name, stored_robot_pose, current_robot_pose, stored_tag_pose, current_tag_pose, metrics) - # Update stored pose if requested and converged + # Option B propagation: if this position is an observation pose and update requested, update it if update_stored_pose and metrics['converged']: - success = self.pose_history.update_position_pose( - position_name, current_robot_pose, current_tag_pose) - metrics['pose_updated'] = success - - if success: - print(f"💾 Updated stored pose for '{position_name}'") - - # Update all equipment positions when converged (critical for subsequent movements) - if metrics['converged'] and np.linalg.norm(total_correction) > 0.001: # Only if significant correction - print("🔧 Applying equipment-wide position updates...") - equipment_success = self.pose_history.update_equipment_positions( - position_name, total_correction) - metrics['equipment_updated'] = equipment_success - - if equipment_success: - print(f"✅ All equipment positions updated with correction: {total_correction}") - else: - print("⚠️ Failed to update equipment positions") + try: + positions_data = self.pose_history._load_positions() + pos_block = positions_data.get('positions', {}) + # Update the corrected position itself + if position_name in pos_block: + pos_block[position_name]['coordinates'] = current_robot_pose.tolist() + pos_block[position_name]['last_visual_servo_update'] = datetime.now().isoformat() + if 'camera_to_tag' in pos_block[position_name]: + pos_block[position_name]['camera_to_tag'] = current_tag_pose.tolist() + # If other positions reference this as observation_pose, recompute them + for other_name, other_data in pos_block.items(): + if other_name == position_name: + continue + if other_data.get('observation_pose') == position_name: + offset = np.array(other_data.get('observation_offset', [0, 0, 0, 0, 0, 0]), dtype=float) + new_coords = current_robot_pose + offset + original = other_data.get('coordinates') + other_data['coordinates'] = new_coords.tolist() + other_data['last_observation_propagation'] = datetime.now().isoformat() + other_data['propagated_from'] = position_name + other_data['propagation_offset_used'] = offset.tolist() + print(f"� Propagated '{position_name}' correction to '{other_name}': {original} → {new_coords.tolist()}") + self.pose_history._save_positions(positions_data) + print(f"💾 Stored updated observation pose '{position_name}' and recomputed linked poses (Option B)") + metrics['pose_updated'] = True + except Exception as e: + print(f"⚠️ Failed Option B propagation for '{position_name}': {e}") + metrics['pose_updated'] = False + else: + print("ℹ️ Position update skipped (either not converged or update_stored_pose=False)") print(f"\n🎯 Direct visual servoing completed for '{position_name}'") print(f" Converged: {metrics['converged']}") @@ -403,6 +619,48 @@ def _visual_servo_direct(self, position_name: str, position_data: Dict[str, Any] return metrics['converged'], metrics + def _reset_propagated_positions(self, positions_file: Path): + """Revert any propagated coordinate changes using observation_offset. + + Strategy: + - Load positions YAML + - For each position that has observation_pose and observation_offset and propagated_from metadata, recompute coordinates + as stored observation pose + observation_offset, but ONLY if observation pose exists. + - Remove propagation metadata keys. + - Skip direct-tag positions without observation logic. + """ + import yaml + with open(positions_file, 'r') as f: + data = yaml.safe_load(f) or {} + pos_block = data.get('positions', {}) + # Cache observation poses + reset_count = 0 + for name, p in pos_block.items(): + obs_name = p.get('observation_pose') + if not obs_name or obs_name == name: + continue # skip self or no observation linkage + if 'propagated_from' not in p: + continue # no propagation metadata to reset + obs = pos_block.get(obs_name) + if not obs: + continue + offset = np.array(p.get('observation_offset', [0, 0, 0, 0, 0, 0]), dtype=float) + base_coords = np.array(obs.get('coordinates', [0, 0, 0, 0, 0, 0]), dtype=float) + original_coords = p.get('coordinates') + recomputed = base_coords + offset + p['coordinates'] = recomputed.tolist() + for k in ['last_observation_propagation', 'propagated_from', 'propagation_offset_used']: + if k in p: + del p[k] + reset_count += 1 + print(f"🔄 Reset '{name}' coords from {original_coords} → {recomputed.tolist()}") + if reset_count: + with open(positions_file, 'w') as f: + yaml.dump(data, f, sort_keys=False) + print(f"💾 Reset {reset_count} propagated positions (reset_positions_on_start enabled)") + else: + print("ℹ️ No propagated positions found to reset") + def _visual_servo_via_observation(self, position_name: str, position_data: Dict[str, Any], update_stored_pose: bool) -> Tuple[bool, Dict[str, Any]]: """ Perform visual servoing for positions using observation pose with fixed offset @@ -472,6 +730,7 @@ def _visual_servo_via_observation(self, position_name: str, position_data: Dict[ current_obs_pose = stored_obs_robot_pose + total_correction print("🔭 Moving to corrected observation pose...") success = self.robot.move_to_pose(current_obs_pose) + self._log_iteration_event(position_name, 'observation', iteration + 1, 'move_obs', success) if not success: print("❌ Failed to move robot to observation pose") return False, metrics @@ -480,15 +739,33 @@ def _visual_servo_via_observation(self, position_name: str, position_data: Dict[ current_obs_tag_pose = self.detection_filter.get_filtered_tag_pose(tag_id) if current_obs_tag_pose is None: print(f"❌ Failed to detect AprilTag {tag_id} from observation pose") + self._log_iteration_event(position_name, 'observation', iteration + 1, 'detect_obs', False) return False, metrics - # Step 3: Calculate tag error from observation position - tag_error = current_obs_tag_pose - stored_obs_tag_pose - pose_error_magnitude = np.linalg.norm(tag_error) - - print(f"📏 Tag pose error magnitude: {pose_error_magnitude:.4f}") - print(f" Translation error: [{tag_error[0]:.4f}, {tag_error[1]:.4f}, {tag_error[2]:.4f}]m") - print(f" Rotation error: [{tag_error[3]:.4f}, {tag_error[4]:.4f}, {tag_error[5]:.4f}]rad") + # Step 3: Calculate proper relative errors + raw_error = current_obs_tag_pose - stored_obs_tag_pose + translation_error = raw_error[:3] + stored_euler = stored_obs_tag_pose[3:] + current_euler = current_obs_tag_pose[3:] + R_stored = self._euler_to_matrix(*stored_euler) + R_current = self._euler_to_matrix(*current_euler) + R_rel = R_current @ R_stored.T + rot_axis, rot_angle = self._matrix_to_axis_angle(R_rel) + rotation_flip_suppressed = False + if rot_angle > math.pi * 0.9 and np.linalg.norm(translation_error) < 0.02: + rotation_flip_suppressed = True + # Apply flip suppression (space around operator for style) + rot_angle = (2 * math.pi - rot_angle) + rot_axis = -rot_axis + print("⚪ Detected near-π rotation ambiguity (observation) -> flip suppression applied") + rotation_error_vec = rot_axis * rot_angle + translation_norm = np.linalg.norm(translation_error) + pose_error_magnitude = math.sqrt(translation_norm**2 + rot_angle**2) + print("📏 Error summary (observation):") + print(f" Translation error: [{translation_error[0]:.4f}, {translation_error[1]:.4f}, {translation_error[2]:.4f}]m (norm {translation_norm:.4f}m)") + print(f" Rotation axis: [{rot_axis[0]:.3f}, {rot_axis[1]:.3f}, {rot_axis[2]:.3f}] angle {rot_angle:.4f}rad") + if rotation_flip_suppressed: + print(f" ℹ️ Flip suppression applied; adjusted angle {rot_angle:.4f}rad") # Detection consistency check - DISABLED for testing # The check was too aggressive and preventing convergence @@ -524,16 +801,23 @@ def _visual_servo_via_observation(self, position_name: str, position_data: Dict[ current_damping *= 0.5 # Halve the damping for this iteration print(f" Using adaptive damping: {current_damping:.3f}") - # Check convergence - if (np.linalg.norm(tag_error[:3]) < self.config.position_tolerance - and np.linalg.norm(tag_error[3:]) < self.config.rotation_tolerance): - print("✅ Converged within tolerance") - metrics['converged'] = True - metrics['final_error'] = pose_error_magnitude - break + # Check convergence (support translation-only mode if rotation disabled) + enable_rot = bool(getattr(self.config, 'enable_rotation', True)) + if not enable_rot: + if translation_norm < self.config.position_tolerance: + print("✅ Converged (translation-only; rotation ignored)") + metrics['converged'] = True + metrics['final_error'] = translation_norm + break + else: + if (translation_norm < self.config.position_tolerance and rot_angle < self.config.rotation_tolerance): + print("✅ Converged within tolerance") + metrics['converged'] = True + metrics['final_error'] = pose_error_magnitude + break # "Good enough" check - if error is already quite small, don't over-correct - if pose_error_magnitude < 0.06: # 60mm total error is quite good + if pose_error_magnitude < 0.01: # 10mm total error is quite good (reduced from 60mm) print(f"✅ Good enough accuracy - error {pose_error_magnitude:.4f} is acceptable") metrics['converged'] = True metrics['final_error'] = pose_error_magnitude @@ -554,32 +838,58 @@ def _visual_servo_via_observation(self, position_name: str, position_data: Dict[ # Since camera moves with robot, corrections apply to both observation and target poses # Extract translation and rotation errors separately - tag_translation_error = tag_error[:3] # [x, y, z] in camera frame - tag_rotation_error = tag_error[3:] # [rx, ry, rz] in camera frame - - # SIMPLE DIRECT CORRECTION - Apply opposite of tag error to robot movement - # For observation-based visual servoing, move robot opposite to tag error to compensate - robot_correction = -tag_error * current_damping - - # Apply safety limits - max_translation_correction = 0.02 # 2cm max per iteration - max_rotation_correction = 0.1 # ~6 degrees max per iteration - - # Limit translation corrections - for i in range(3): - robot_correction[i] = np.clip(robot_correction[i], - -max_translation_correction, max_translation_correction) - - # Limit rotation corrections - for i in range(3, 6): - robot_correction[i] = np.clip(robot_correction[i], - -max_rotation_correction, max_rotation_correction) - - print(f"🔧 Simple direct correction (damping={current_damping:.3f}):") - print(f" Translation errors: [{tag_translation_error[0]:.4f}, {tag_translation_error[1]:.4f}, {tag_translation_error[2]:.4f}]") - print(f" Rotation errors: [{tag_rotation_error[0]:.4f}, {tag_rotation_error[1]:.4f}, {tag_rotation_error[2]:.4f}]") - print(f" Translation correction: [{robot_correction[0]:.4f}, {robot_correction[1]:.4f}, {robot_correction[2]:.4f}]") - print(f" Rotation correction: [{robot_correction[3]:.4f}, {robot_correction[4]:.4f}, {robot_correction[5]:.4f}]") + tag_translation_error = translation_error + tag_rotation_error = rotation_error_vec + + # Check if using simple legacy-style mode + if hasattr(self.config, 'simple_mode') and self.config.simple_mode: + # Simple mode: only XY translation corrections (like legacy system) + robot_correction = np.zeros(6) + robot_correction[0] = -tag_translation_error[0] * 0.4 # X correction + robot_correction[1] = -tag_translation_error[1] * 0.4 # Y correction + # Z, RX, RY, RZ remain zero for stability + + print("🔧 Simple XY-only correction (legacy style):") + print(f" Translation errors: [{tag_translation_error[0]:.4f}, {tag_translation_error[1]:.4f}, {tag_translation_error[2]:.4f}]") + print(f" Applied XY correction: [{robot_correction[0]:.4f}, {robot_correction[1]:.4f}, 0.0000] (Z/rotations=0)") + print(" Simple mode damping: 0.400") + else: + # PROPER COORDINATE TRANSFORMATION - Transform camera errors to robot corrections + camera_trans_error = [tag_translation_error[0], tag_translation_error[1], tag_translation_error[2]] + camera_rot_error = [tag_rotation_error[0], tag_rotation_error[1], tag_rotation_error[2]] + + # Apply coordinate transformation to map camera frame to robot frame + enable_rot = bool(getattr(self.config, 'enable_rotation', True)) + robot_trans_correction, robot_rot_correction = transform_camera_to_robot_correction( + camera_trans_error, camera_rot_error, enable_rotation=enable_rot + ) + + # Apply damping + robot_trans_correction = [x * current_damping for x in robot_trans_correction] + robot_rot_correction = [x * current_damping for x in robot_rot_correction] + + # Combine into 6DOF correction vector + robot_correction = np.array(robot_trans_correction + robot_rot_correction) + + # Apply safety limits + max_translation_correction = 0.02 # 2cm max per iteration + max_rotation_correction = 0.1 # ~6 degrees max per iteration + + # Limit translation corrections + for i in range(3): + robot_correction[i] = np.clip(robot_correction[i], + -max_translation_correction, max_translation_correction) + + # Limit rotation corrections + for i in range(3, 6): + robot_correction[i] = np.clip(robot_correction[i], + -max_rotation_correction, max_rotation_correction) + + print(f"🔧 Coordinate-transformed IBVS correction (damping={current_damping:.3f}):") + print(f" Camera translation errors: [{tag_translation_error[0]:.4f}, {tag_translation_error[1]:.4f}, {tag_translation_error[2]:.4f}]") + print(f" Camera rotation errors: [{tag_rotation_error[0]:.4f}, {tag_rotation_error[1]:.4f}, {tag_rotation_error[2]:.4f}]") + print(f" Robot translation correction: [{robot_correction[0]:.4f}, {robot_correction[1]:.4f}, {robot_correction[2]:.4f}]") + print(f" Robot rotation correction: [{robot_correction[3]:.4f}, {robot_correction[4]:.4f}, {robot_correction[5]:.4f}]") print(f" Adaptive damping: {current_damping:.3f}") # Apply safety limits @@ -600,15 +910,39 @@ def _visual_servo_via_observation(self, position_name: str, position_data: Dict[ metrics['corrections_applied'].append({ 'iteration': iteration + 1, - 'tag_error': tag_error.tolist(), + 'translation_error': translation_error.tolist(), + 'rotation_axis': rot_axis.tolist(), + 'rotation_angle': rot_angle, + 'rotation_flip_suppressed': rotation_flip_suppressed, 'robot_correction': robot_correction.tolist(), 'correction_magnitude': correction_magnitude }) + self._log_iteration_event(position_name, 'observation', iteration + 1, 'correction', True, + translation_error=translation_error, + rot_axis=rot_axis, + rot_angle=rot_angle, + rotation_flip_suppressed=rotation_flip_suppressed, + applied_correction=robot_correction) # Step 6: Move to final corrected target pose if metrics['converged']: print("🎯 Moving to final corrected target pose...") + + # First validate that the target pose is reachable + if not self._is_pose_reachable(current_target_pose): + print("❌ Final target pose is not reachable by robot") + print(" This may be due to joint limits, singularities, or workspace constraints") + print(" Visual servoing converged but target position is unreachable") + print(f" Target pose: {[round(x, 3) for x in current_target_pose]}") + + # Ask user if they want to continue anyway + response = input("\n🤔 Attempt move anyway? (y/n): ").lower().strip() + if response not in ['y', 'yes']: + print("⏹️ Stopping workflow - target pose unreachable") + return False, metrics + success = self.robot.move_to_pose(current_target_pose) + self._log_iteration_event(position_name, 'observation', metrics['iterations'], 'move_final', success) if not success: print("❌ Failed to move robot to final target pose") return False, metrics @@ -641,31 +975,40 @@ def _visual_servo_via_observation(self, position_name: str, position_data: Dict[ position_name, stored_target_pose, current_target_pose, stored_obs_tag_pose, current_obs_tag_pose, metrics) - # Update stored pose if requested and converged + # Option B propagation for observation-based method if update_stored_pose and metrics['converged']: - success = self.pose_history.update_position_pose( - position_name, current_target_pose, None) # No direct camera_to_tag for target - metrics['pose_updated'] = success - - if success: - print(f"💾 Updated stored pose for '{position_name}'") - - # Also update observation pose - obs_success = self.pose_history.update_position_pose( - observation_pose_name, stored_obs_robot_pose + total_correction, current_obs_tag_pose) - metrics['obs_pose_updated'] = obs_success - - # Update all equipment positions when converged (critical for subsequent movements) - if metrics['converged'] and np.linalg.norm(total_correction) > 0.001: # Only if significant correction - print("🔧 Applying equipment-wide position updates...") - equipment_success = self.pose_history.update_equipment_positions( - position_name, total_correction) - metrics['equipment_updated'] = equipment_success - - if equipment_success: - print(f"✅ All equipment positions updated with correction: {total_correction}") - else: - print("⚠️ Failed to update equipment positions") + try: + positions_data = self.pose_history._load_positions() + pos_block = positions_data.get('positions', {}) + # Update observation pose coordinates first + if observation_pose_name in pos_block: + new_obs_pose = stored_obs_robot_pose + total_correction + pos_block[observation_pose_name]['coordinates'] = new_obs_pose.tolist() + pos_block[observation_pose_name]['last_visual_servo_update'] = datetime.now().isoformat() + # Update its camera_to_tag with latest observation detection + pos_block[observation_pose_name]['camera_to_tag'] = current_obs_tag_pose.tolist() + print(f"💾 Updated observation pose '{observation_pose_name}' with correction {total_correction.tolist()}") + # Propagate to all linked positions (including target position) + for other_name, other_data in pos_block.items(): + if other_data.get('observation_pose') == observation_pose_name: + offset = np.array(other_data.get('observation_offset', [0, 0, 0, 0, 0, 0]), dtype=float) + new_coords = new_obs_pose + offset + original = other_data.get('coordinates') + other_data['coordinates'] = new_coords.tolist() + other_data['last_observation_propagation'] = datetime.now().isoformat() + other_data['propagated_from'] = observation_pose_name + other_data['propagation_offset_used'] = offset.tolist() + print(f"� Propagated correction to '{other_name}': {original} → {new_coords.tolist()}") + self.pose_history._save_positions(positions_data) + print(f"✅ Option B propagation complete for observation '{observation_pose_name}'") + metrics['pose_updated'] = True + metrics['obs_pose_updated'] = True + except Exception as e: + print(f"⚠️ Failed Option B propagation for observation '{observation_pose_name}': {e}") + metrics['pose_updated'] = False + metrics['obs_pose_updated'] = False + else: + print("ℹ️ Position update skipped (either not converged or update_stored_pose=False)") print(f"\n🎯 Observation-based visual servoing completed for '{position_name}'") print(f" Converged: {metrics['converged']}") @@ -851,9 +1194,9 @@ def get_corrected_pose(self, position_name: str, original_pose: np.ndarray) -> O print(f"❌ Failed to detect AprilTag {tag_id}") return None - # Calculate pose correction + # Calculate pose correction using hand-eye calibration tag_error = current_tag_pose - stored_tag_pose - robot_correction = -tag_error # Negative to counteract the error + robot_correction = self._transform_tag_error_to_robot_correction(tag_error) # Apply safety limits correction_valid, safety_metrics = self._validate_correction( @@ -872,7 +1215,185 @@ def get_corrected_pose(self, position_name: str, original_pose: np.ndarray) -> O return corrected_pose + def _is_pose_reachable(self, target_pose): + """ + Test if a target pose is reachable by the robot. + + This performs a quick validation by attempting a small test movement + to check for joint limits, singularities, or other reachability issues. + + Args: + target_pose: Target pose to validate (numpy array) + + Returns: + bool: True if pose appears reachable, False otherwise + """ + try: + # Store current pose + current_pose = self.robot.get_tcp_pose() + + # Try to compute inverse kinematics by attempting a small move + # This will fail if the pose is outside joint limits or in singularity + test_success = self.robot.move_to_pose(target_pose, speed=0.01, acceleration=0.01) + + if test_success: + # If move succeeded, immediately return to original position + self.robot.move_to_pose(current_pose, speed=0.05, acceleration=0.05) + return True + else: + return False + + except Exception as e: + print(f"⚠️ Pose reachability test failed: {e}") + return False + def set_robot_controller(self, robot_controller): """Set the robot controller instance""" self.robot = robot_controller print("🤖 Robot controller set for visual servo engine") + + def _load_hand_eye_calibration(self) -> Optional[np.ndarray]: + """Load hand-eye calibration transformation if available. + + Returns: + 4x4 transformation matrix or None if not available + """ + calib_file = Path("src/ur_toolkit/hand_eye_calibration/hand_eye_calibration.json") + + if not calib_file.exists(): + print("⚠️ No hand-eye calibration found - using coordinate frame mapping") + return None + + try: + with open(calib_file, 'r') as f: + data = json.load(f) + + transform = np.array(data['hand_eye_transform']) + print("✅ Hand-eye calibration loaded successfully") + print(f" Calibration date: {data.get('calibration_date', 'Unknown')}") + + return transform + + except Exception as e: + print(f"⚠️ Failed to load hand-eye calibration: {e}") + return None + + def _transform_tag_error_to_robot_correction(self, tag_error: np.ndarray) -> np.ndarray: + """Transform AprilTag pose error to robot correction. + + Args: + tag_error: 6DOF pose error [x, y, z, rx, ry, rz] in camera frame + + Returns: + 6DOF correction [x, y, z, rx, ry, rz] in robot frame + """ + if self.hand_eye_transform is not None: + # Use proper hand-eye calibration transformation + return self._transform_with_hand_eye_calibration(tag_error) + else: + # Fallback to manual coordinate frame mapping + return self._transform_with_coordinate_mapping(tag_error) + + def _transform_with_hand_eye_calibration(self, tag_error: np.ndarray) -> np.ndarray: + """Transform error using hand-eye calibration matrix. + + Args: + tag_error: 6DOF pose error in camera frame + + Returns: + 6DOF correction in robot frame + """ + # Create transformation matrix from error + error_transform = np.eye(4) + error_transform[:3, 3] = tag_error[:3] + + # For small rotations, approximate rotation matrix + rx, ry, rz = tag_error[3:6] + error_transform[:3, :3] = np.array([ + [1, -rz, ry], + [rz, 1, -rx], + [-ry, rx, 1] + ]) + + # Transform error from camera frame to robot frame + # For eye-in-hand: robot_error = hand_eye_transform^-1 * camera_error * hand_eye_transform + hand_eye_inv = np.linalg.inv(self.hand_eye_transform) + robot_error_transform = hand_eye_inv @ error_transform @ self.hand_eye_transform + + # Extract 6DOF correction + robot_correction = np.zeros(6) + robot_correction[:3] = robot_error_transform[:3, 3] + + # Extract rotation (approximate for small angles) + R = robot_error_transform[:3, :3] + robot_correction[3] = (R[2, 1] - R[1, 2]) / 2 + robot_correction[4] = (R[0, 2] - R[2, 0]) / 2 + robot_correction[5] = (R[1, 0] - R[0, 1]) / 2 + + # Negate to get correction (opposite of error) + return -robot_correction + + def _transform_with_coordinate_mapping(self, tag_error: np.ndarray) -> np.ndarray: + """Transform error using manual coordinate frame mapping. + + This is the fallback method when no hand-eye calibration is available. + + Args: + tag_error: 6DOF pose error in camera frame + + Returns: + 6DOF correction in robot frame + """ + # Use the improved coordinate transformation from easy_handeye principles + # Assuming camera mounted looking forward from end-effector + + robot_correction = np.zeros(6) + + # Position corrections (camera to robot coordinates) + # For eye-in-hand with camera looking forward: + robot_correction[0] = -tag_error[2] # Camera Z (depth) -> Robot X (forward/back) + robot_correction[1] = -tag_error[0] # Camera X (right) -> Robot Y (left/right) + robot_correction[2] = tag_error[1] # Camera Y (down) -> Robot Z (up/down) + + # Rotation corrections (simplified - should use proper transformation) + robot_correction[3:6] = tag_error[3:6] * 0.5 # Reduced rotation gains + + # Negate to get correction (opposite of error) + return -robot_correction + + # ---------------- Rotation Utility Methods ----------------- + def _euler_to_matrix(self, rx: float, ry: float, rz: float) -> np.ndarray: + """Convert Euler angles (XYZ order) to rotation matrix using small-angle general case.""" + # Using standard intrinsic XYZ; adapt if different convention later. + cx, cy, cz = math.cos(rx), math.cos(ry), math.cos(rz) + sx, sy, sz = math.sin(rx), math.sin(ry), math.sin(rz) + # R = Rz * Ry * Rx (extrinsic) or Rx * Ry * Rz (intrinsic); choose intrinsic Rx*Ry*Rz + R = np.array([ + [cy * cz, -cy * sz, sy], + [sx * sy * cz + cx * sz, -sx * sy * sz + cx * cz, -sx * cy], + [-cx * sy * cz + sx * sz, cx * sy * sz + sx * cz, cx * cy] + ]) + return R + + def _matrix_to_axis_angle(self, R: np.ndarray) -> Tuple[np.ndarray, float]: + """Convert rotation matrix to axis-angle (returns unit axis and angle).""" + trace = np.clip((np.trace(R) - 1) / 2, -1.0, 1.0) + angle = math.acos(trace) + if abs(angle) < 1e-8: + return np.array([1.0, 0.0, 0.0]), 0.0 + # Handle near-pi robustly + if abs(angle - math.pi) < 1e-6: + # Axis extraction for angle ~ pi + axis = np.sqrt(np.maximum(np.diag(R) + 1 - trace * 2, 0)) + axis = axis / (np.linalg.norm(axis) + 1e-9) + return axis, angle + rx = (R[2, 1] - R[1, 2]) / (2 * math.sin(angle)) + ry = (R[0, 2] - R[2, 0]) / (2 * math.sin(angle)) + rz = (R[1, 0] - R[0, 1]) / (2 * math.sin(angle)) + axis = np.array([rx, ry, rz]) + axis_norm = np.linalg.norm(axis) + if axis_norm < 1e-9: + axis = np.array([1.0, 0.0, 0.0]) + else: + axis /= axis_norm + return axis, angle diff --git a/src/ur_toolkit/workflow/workflow_executor.py b/src/ur_toolkit/workflow/workflow_executor.py index 7d83794..cfb60c9 100644 --- a/src/ur_toolkit/workflow/workflow_executor.py +++ b/src/ur_toolkit/workflow/workflow_executor.py @@ -22,6 +22,7 @@ # Import visual servoing try: from visual_servo.visual_servo_engine import VisualServoEngine + VISUAL_SERVO_AVAILABLE = True except ImportError as e: print(f"⚠️ Visual servoing not available: {e}") @@ -45,7 +46,9 @@ def __init__(self, taught_positions_file=None): # Load taught positions if taught_positions_file is None: - taught_positions_file = Path(__file__).parent.parent / "positions" / "taught_positions.yaml" + taught_positions_file = ( + Path(__file__).parent.parent / "positions" / "taught_positions.yaml" + ) self.positions_file = Path(taught_positions_file) self.load_taught_positions() @@ -54,21 +57,23 @@ def __init__(self, taught_positions_file=None): self.init_visual_servo_engine() print("🤖 Workflow Executor initialized") - print(f"📍 Loaded {len(self.taught_positions.get('positions', {}))} taught positions") + print( + f"📍 Loaded {len(self.taught_positions.get('positions', {}))} taught positions" + ) def load_taught_positions(self): """Load taught positions from YAML file""" try: if self.positions_file.exists(): - with open(self.positions_file, 'r') as f: + with open(self.positions_file, "r") as f: self.taught_positions = yaml.safe_load(f) or {} print(f"✅ Loaded positions from: {self.positions_file}") else: print(f"⚠️ Positions file not found: {self.positions_file}") - self.taught_positions = {'positions': {}} + self.taught_positions = {"positions": {}} except Exception as e: print(f"❌ Error loading positions: {e}") - self.taught_positions = {'positions': {}} + self.taught_positions = {"positions": {}} def init_visual_servo_engine(self): """Initialize visual servo engine if available""" @@ -78,7 +83,9 @@ def init_visual_servo_engine(self): # Initialize visual servo engine without robot (robot set later) detector = AprilTagDetector() - self.visual_servo_engine = VisualServoEngine(None, self.positions_file, detector) + self.visual_servo_engine = VisualServoEngine( + None, self.positions_file, detector + ) print("✅ Visual servo engine initialized") except Exception as e: print(f"⚠️ Visual servo engine unavailable: {e}") @@ -87,7 +94,7 @@ def init_visual_servo_engine(self): def connect_robot(self): """Connect to the robot""" try: - robot_ip = config.get('robot', {}).get('ip', '192.168.0.10') + robot_ip = config.get("robot", {}).get("ip", "192.168.0.10") # URController connects automatically in __init__ self.robot = URController(robot_ip, read_only=False) @@ -129,7 +136,7 @@ def execute_workflow(self, workflow_file, step_mode=False): return False try: - with open(workflow_path, 'r') as f: + with open(workflow_path, "r") as f: workflow = yaml.safe_load(f) except Exception as e: print(f"❌ Error loading workflow: {e}") @@ -149,15 +156,17 @@ def execute_workflow_dict(self, workflow, step_mode=False): print("❌ Robot not connected. Call connect_robot() first.") return False - workflow_name = workflow.get('name', 'Unnamed Workflow') - description = workflow.get('description', 'No description') - steps = workflow.get('steps', []) + workflow_name = workflow.get("name", "Unnamed Workflow") + description = workflow.get("description", "No description") + steps = workflow.get("steps", []) print(f"\n🚀 Executing Workflow: {workflow_name}") print(f"📝 Description: {description}") print(f"📋 Steps: {len(steps)}") if step_mode: - print("🐾 Step-by-step mode: Press ENTER to continue each step, 'q' to quit") + print( + "🐾 Step-by-step mode: Press ENTER to continue each step, 'q' to quit" + ) print("=" * 60) start_time = datetime.now() @@ -165,12 +174,12 @@ def execute_workflow_dict(self, workflow, step_mode=False): for i, step in enumerate(steps, 1): # Auto-generate step name based on action and parameters - action = step.get('action', 'unknown') + action = step.get("action", "unknown") step_name = self._generate_step_name(step, i) # Auto-skip delays in step-through mode - if step_mode and action == 'delay': - duration = step.get('duration', 0) + if step_mode and action == "delay": + duration = step.get("duration", 0) print(f"\n⏩ Step {i}/{len(steps)}: {step_name}") print(f"⏭️ Auto-skipping delay ({duration}s) in step-through mode") success_count += 1 @@ -180,26 +189,28 @@ def execute_workflow_dict(self, workflow, step_mode=False): # Step-by-step prompting for non-delay steps if step_mode: print(f"\n🔍 Next step {i}/{len(steps)}: {step_name}") - if action in ['movel', 'movej']: - position = step.get('position', 'unknown') - move_type = "Joint move" if action == 'movej' else "Linear move" + if action in ["movel", "movej"]: + position = step.get("position", "unknown") + move_type = "Joint move" if action == "movej" else "Linear move" print(f" Action: {move_type} to position '{position}'") - elif action == 'gripper_open': + elif action == "gripper_open": print(" Action: Open gripper") - elif action == 'gripper_close': + elif action == "gripper_close": print(" Action: Close gripper") - elif action == 'offset_move': - offset = step.get('offset', [0, 0, 0, 0, 0, 0]) + elif action == "offset_move": + offset = step.get("offset", [0, 0, 0, 0, 0, 0]) print(f" Action: Offset move {offset}") else: print(f" Action: {action}") try: - user_input = input("Continue? (ENTER/q to quit/s to skip): ").lower().strip() - if user_input == 'q' or user_input == 'quit': + user_input = ( + input("Continue? (ENTER/q to quit/s to skip): ").lower().strip() + ) + if user_input == "q" or user_input == "quit": print("🛑 Workflow stopped by user") break - elif user_input == 's' or user_input == 'skip': + elif user_input == "s" or user_input == "skip": print(f"⏭️ Skipping step {i}") continue except (EOFError, KeyboardInterrupt): @@ -223,42 +234,44 @@ def execute_workflow_dict(self, workflow, step_mode=False): print(f"⏱️ Duration: {duration:.2f} seconds") # Record workflow execution - self.workflow_history.append({ - 'name': workflow_name, - 'start_time': start_time.isoformat(), - 'end_time': end_time.isoformat(), - 'duration': duration, - 'steps_total': len(steps), - 'steps_successful': success_count, - 'success': success_count == len(steps) - }) + self.workflow_history.append( + { + "name": workflow_name, + "start_time": start_time.isoformat(), + "end_time": end_time.isoformat(), + "duration": duration, + "steps_total": len(steps), + "steps_successful": success_count, + "success": success_count == len(steps), + } + ) return success_count == len(steps) def execute_step(self, step, step_mode=False): """Execute a single workflow step""" - action = step.get('action', '').lower() + action = step.get("action", "").lower() try: - if action in ['movel', 'movej']: + if action in ["movel", "movej"]: return self._move_to_position(step) - elif action == 'offset_move': + elif action == "offset_move": return self._offset_move(step) - elif action == 'gripper_open': + elif action == "gripper_open": return self._gripper_open(step) - elif action == 'gripper_close': + elif action == "gripper_close": return self._gripper_close(step) - elif action == 'gripper_activate': + elif action == "gripper_activate": return self._gripper_activate(step) - elif action == 'gripper_position': + elif action == "gripper_position": return self._gripper_position(step) - elif action == 'gripper_params': + elif action == "gripper_params": return self._gripper_params(step) - elif action == 'delay': + elif action == "delay": return self._delay(step, step_mode) - elif action == 'verify_position': + elif action == "verify_position": return self._verify_position(step) - elif action == 'visual_servo': + elif action == "visual_servo": return self._visual_servo_action(step) else: print(f"❌ Unknown action: {action}") @@ -270,70 +283,74 @@ def execute_step(self, step, step_mode=False): def _generate_step_name(self, step, step_number): """Generate a descriptive step name based on action and parameters""" - action = step.get('action', 'unknown').lower() # Convert to lowercase for consistency - name = step.get('name') # Use explicit name if provided + action = step.get( + "action", "unknown" + ).lower() # Convert to lowercase for consistency + name = step.get("name") # Use explicit name if provided if name: return name - if action in ['movel', 'movej']: - position = step.get('position', 'unknown') - move_type = "Joint move" if action == 'movej' else "Move" + if action in ["movel", "movej"]: + position = step.get("position", "unknown") + move_type = "Joint move" if action == "movej" else "Move" return f"{move_type} to {position}" - elif action == 'offset_move': - offset = step.get('offset', [0, 0, 0, 0, 0, 0]) + elif action == "offset_move": + offset = step.get("offset", [0, 0, 0, 0, 0, 0]) # Format offset nicely - show only non-zero values offset_str = [] - labels = ['X', 'Y', 'Z', 'Rx', 'Ry', 'Rz'] + labels = ["X", "Y", "Z", "Rx", "Ry", "Rz"] for i, val in enumerate(offset[:6]): if val != 0: if i < 3: # Position offset_str.append(f"{labels[i]}:{val * 1000:.0f}mm") else: # Rotation offset_str.append(f"{labels[i]}:{val:.3f}rad") - return f"Offset move ({', '.join(offset_str) if offset_str else 'no offset'})" - elif action == 'gripper_open': + return ( + f"Offset move ({', '.join(offset_str) if offset_str else 'no offset'})" + ) + elif action == "gripper_open": return "Open gripper" - elif action == 'gripper_close': + elif action == "gripper_close": return "Close gripper" - elif action == 'gripper_activate': + elif action == "gripper_activate": return "Activate gripper" - elif action == 'delay': - duration = step.get('duration', 0) + elif action == "delay": + duration = step.get("duration", 0) return f"Wait {duration}s" - elif action == 'gripper_position': - pos = step.get('position', 0) + elif action == "gripper_position": + pos = step.get("position", 0) return f"Gripper to {pos}" - elif action == 'verify_position': - position = step.get('position', 'unknown') + elif action == "verify_position": + position = step.get("position", "unknown") return f"Verify at {position}" - elif action == 'visual_servo': - position = step.get('position', 'unknown') + elif action == "visual_servo": + position = step.get("position", "unknown") return f"Visual servo to {position}" else: return f"Step {step_number} ({action})" def _move_to_position(self, step): """Move to a taught position with optional visual servoing""" - position_name = step.get('position') - speed = step.get('speed', 0.1) # Default 10% speed - action = step.get('action', '').lower() - use_visual_servo = step.get('visual_servo', False) + position_name = step.get("position") + speed = step.get("speed", 0.1) # Default 10% speed + action = step.get("action", "").lower() + use_visual_servo = step.get("visual_servo", False) if not position_name: print("❌ No position specified") return False - positions = self.taught_positions.get('positions', {}) + positions = self.taught_positions.get("positions", {}) if position_name not in positions: print(f"❌ Position '{position_name}' not found") return False position_data = positions[position_name] - coordinates = position_data['coordinates'] + coordinates = position_data["coordinates"] # Determine movement type - if action == 'movej': + if action == "movej": print(f"🔄 Joint move to position: {position_name}") else: print(f"🎯 Linear move to position: {position_name}") @@ -344,7 +361,9 @@ def _move_to_position(self, step): # Use visual servo engine to get corrected pose try: - corrected_pose = self.visual_servo_engine.get_corrected_pose(position_name, coordinates) + corrected_pose = self.visual_servo_engine.get_corrected_pose( + position_name, coordinates + ) if corrected_pose is not None: target_pose = corrected_pose print("✅ Using visual servo corrected pose") @@ -360,7 +379,7 @@ def _move_to_position(self, step): print(f"📍 Target coordinates: {[f'{x:.3f}' for x in target_pose]}") # Use joint move or linear move based on action - if action == 'movej': + if action == "movej": # For now, joint moves use the same function (could be extended in URController) # In a full implementation, this would use moveJ commands in the robot controller return self.robot.move_to_pose(target_pose, speed=speed) @@ -370,9 +389,9 @@ def _move_to_position(self, step): def _offset_move(self, step): """Move by an offset from current position""" - offset = step.get('offset', [0, 0, 0, 0, 0, 0]) - speed = step.get('speed', 0.1) - coordinate_system = step.get('coordinate_system', 'tcp') # 'tcp' or 'base' + offset = step.get("offset", [0, 0, 0, 0, 0, 0]) + speed = step.get("speed", 0.1) + coordinate_system = step.get("coordinate_system", "tcp") # 'tcp' or 'base' if len(offset) != 6: print("❌ Offset must be [x, y, z, rx, ry, rz]") @@ -384,7 +403,7 @@ def _offset_move(self, step): current_pose = self.robot.get_tcp_pose() # Apply offset - if coordinate_system == 'tcp': + if coordinate_system == "tcp": # For TCP coordinate system, we need to transform the offset # For simplicity, we'll just add the offset to current pose target_pose = current_pose + np.array(offset) @@ -399,7 +418,7 @@ def _gripper_open(self, step): success = self.robot.gripper_open() if success: # Wait for movement to complete - wait_time = step.get('wait_time', 1.0) # Reduced default from 2.0 + wait_time = step.get("wait_time", 1.0) # Reduced default from 2.0 print(f"⏳ Waiting {wait_time}s for gripper to open...") time.sleep(wait_time) return success @@ -410,7 +429,7 @@ def _gripper_close(self, step): success = self.robot.gripper_close() if success: # Wait for movement to complete - wait_time = step.get('wait_time', 1.0) # Reduced default from 2.0 + wait_time = step.get("wait_time", 1.0) # Reduced default from 2.0 print(f"⏳ Waiting {wait_time}s for gripper to close...") time.sleep(wait_time) return success @@ -421,7 +440,7 @@ def _gripper_activate(self, step): success = self.robot.gripper_activate() if success: # Wait for activation to complete - wait_time = step.get('wait_time', 2.0) # Reduced default from 3.0 + wait_time = step.get("wait_time", 2.0) # Reduced default from 3.0 print(f"⏳ Waiting {wait_time}s for gripper activation...") time.sleep(wait_time) @@ -447,7 +466,7 @@ def _gripper_activate(self, step): def _delay(self, step, step_mode=False): """Wait for specified time""" - duration = step.get('duration', 1.0) + duration = step.get("duration", 1.0) if step_mode: print(f"⏭️ Skipping delay ({duration}s) in step-through mode") else: @@ -457,19 +476,19 @@ def _delay(self, step, step_mode=False): def _verify_position(self, step): """Verify current position matches expected position""" - position_name = step.get('position') - tolerance = step.get('tolerance', 0.01) # 1cm tolerance + position_name = step.get("position") + tolerance = step.get("tolerance", 0.01) # 1cm tolerance if not position_name: print("❌ No position specified for verification") return False - positions = self.taught_positions.get('positions', {}) + positions = self.taught_positions.get("positions", {}) if position_name not in positions: print(f"❌ Position '{position_name}' not found") return False - expected_pose = np.array(positions[position_name]['coordinates']) + expected_pose = np.array(positions[position_name]["coordinates"]) current_pose = self.robot.get_tcp_pose() # Check position difference (translation only) @@ -487,7 +506,7 @@ def _verify_position(self, step): def _visual_servo_action(self, step): """Execute visual servoing to correct position based on AprilTag detection""" - position_name = step.get('position') + position_name = step.get("position") if not position_name: print("❌ No position specified for visual servo") @@ -497,7 +516,7 @@ def _visual_servo_action(self, step): print("❌ Visual servo engine not available") return False - positions = self.taught_positions.get('positions', {}) + positions = self.taught_positions.get("positions", {}) if position_name not in positions: print(f"❌ Position '{position_name}' not found") return False @@ -505,22 +524,81 @@ def _visual_servo_action(self, step): print(f"👁️ Starting visual servo to position: {position_name}") try: - # Execute visual servoing + # Global master enable check + if ( + hasattr(self.visual_servo_engine, "config") + and not self.visual_servo_engine.config.enabled + ): + print( + f"⚪ Visual servo globally disabled (visual_servo.enabled=False) - skipping correction for '{position_name}'" + ) + return True # Treat as successful no-op + # Execute visual servoing (Option B will update positions file) + # Per-step override: disable_rotation=True forces rotation corrections off + if step.get("disable_rotation"): + original_flag = getattr( + self.visual_servo_engine.config, "enable_rotation", True + ) + # Temporarily disable via config object attribute (non-persistent if config reloads) + try: + self.visual_servo_engine.config.config[ + "visual_servo.enable_rotation" + ] = False + except Exception: + pass + else: + original_flag = None success, result_data = self.visual_servo_engine.visual_servo_to_position( - position_name, - update_stored_pose=True + position_name, update_stored_pose=True ) + # Restore rotation flag if we modified it + if original_flag is not None: + try: + self.visual_servo_engine.config.config[ + "visual_servo.enable_rotation" + ] = original_flag + except Exception: + pass if success: print("✅ Visual servo completed successfully") + # Reload positions so subsequent move steps use updated propagated coordinates + try: + reloaded = self._load_taught_positions(self.positions_file) + if reloaded: + self.taught_positions = reloaded + print( + "🔄 Reloaded taught positions after visual servo propagation" + ) + else: + print("⚠️ Failed to reload taught positions after servo") + except Exception as e: + print(f"⚠️ Error reloading positions after servo: {e}") return True else: + # Graceful failure handling: allow workflow to continue or abort based on step flag + abort_on_fail = step.get("abort_on_fail", True) print("❌ Visual servo failed") - return False + if abort_on_fail: + print( + "⏹️ Aborting workflow due to visual servo failure (abort_on_fail=True)" + ) + return False + else: + print( + "➡️ Continuing despite visual servo failure (abort_on_fail=False)" + ) + return True except Exception as e: + abort_on_exception = step.get("abort_on_exception", True) print(f"❌ Visual servo error: {e}") - return False + if abort_on_exception: + print("⏹️ Aborting workflow due to exception (abort_on_exception=True)") + return False + else: + print("➡️ Continuing despite exception (abort_on_exception=False)") + return True def get_workflow_history(self): """Get workflow execution history""" @@ -528,33 +606,33 @@ def get_workflow_history(self): def list_available_positions(self): """List all available taught positions""" - positions = self.taught_positions.get('positions', {}) + positions = self.taught_positions.get("positions", {}) print(f"\n📋 Available Positions ({len(positions)}):") print("=" * 50) for name, data in positions.items(): - pos_type = data.get('pose_type', 'unknown') - description = data.get('description', 'No description') + pos_type = data.get("pose_type", "unknown") + description = data.get("description", "No description") print(f"🎯 {name} ({pos_type})") if description: print(f" 📝 {description}") def _gripper_position(self, step): """Set gripper to specific position""" - position = step.get('position', 128) # Default to half-closed + position = step.get("position", 128) # Default to half-closed print(f"🎯 Setting gripper position to {position}...") success = self.robot.gripper_set_position(position) if success: # Wait for movement to complete - wait_time = step.get('wait_time', 1.0) # Reduced default from 2.0 + wait_time = step.get("wait_time", 1.0) # Reduced default from 2.0 print(f"⏳ Waiting {wait_time}s for gripper to move to position...") time.sleep(wait_time) return success def _gripper_params(self, step): """Set gripper force and/or speed parameters""" - force = step.get('force') - speed = step.get('speed') + force = step.get("force") + speed = step.get("speed") success = True if force is not None: @@ -576,10 +654,12 @@ def main(): """Main function for standalone execution""" import argparse - parser = argparse.ArgumentParser(description='Robot Workflow Executor') - parser.add_argument('workflow', help='Workflow YAML file to execute') - parser.add_argument('--positions', help='Taught positions YAML file') - parser.add_argument('--list-positions', action='store_true', help='List available positions') + parser = argparse.ArgumentParser(description="Robot Workflow Executor") + parser.add_argument("workflow", help="Workflow YAML file to execute") + parser.add_argument("--positions", help="Taught positions YAML file") + parser.add_argument( + "--list-positions", action="store_true", help="List available positions" + ) args = parser.parse_args() diff --git a/test_apriltag_movel.py b/test_apriltag_movel.py new file mode 100644 index 0000000..874c4ad --- /dev/null +++ b/test_apriltag_movel.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +""" +AprilTag-based moveL Test Script +Tests AprilTag detection and moveL to detected tag position +""" + +import sys +import time +import numpy as np +import cv2 +from pathlib import Path + +# Add src to path for imports +sys.path.insert(0, str(Path(__file__).parent / "src")) + +import rtde_control +import rtde_receive +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.config_manager import ( + config, + get_apriltag_family, + get_apriltag_size, + get_camera_calibration_file, + get_camera_host, + get_camera_port, + get_photos_directory, +) + + +robot_ip = config.get("robot.ip_address", "192.168.0.10") + +print("Connecting to UR robot at", robot_ip) + +rtde_c = rtde_control.RTDEControlInterface(robot_ip) +rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) +print("RTDE connection established") + +# Movement parameters +speed = 0.05 # m/s +accel = 0.1 # m/s² + +# Initialize camera +camera_config = PiCamConfig( + hostname=get_camera_host(), + port=get_camera_port(), + download_dir=str(get_photos_directory()), +) +camera = PiCam(camera_config) + +if not camera.test_connection(): + print("Cannot connect to camera server") + exit(1) + +print("Camera connected") + +# Initialize detector +detector = AprilTagDetector( + tag_family=get_apriltag_family(), + tag_size=get_apriltag_size(), + calibration_file=get_camera_calibration_file(), +) + +print("Detector initialized") + +# Get current robot pose (this will be our reference) +reference_pose = np.array(rtde_r.getActualTCPPose()) +print("Reference pose:", reference_pose) + +# Capture image and detect AprilTag +photo_path = camera.capture_photo() +if not photo_path: + print("Failed to capture photo") + exit(1) + +print("Captured photo:", photo_path) + +image = cv2.imread(photo_path) +detections = detector.detect_tags(image) + +if not detections: + print("No AprilTags detected") + exit(1) + +print("Detected", len(detections), "AprilTags") + +# Use first detection +tag = detections[0] +print("Using Tag ID", tag["tag_id"]) + +if not tag["pose"]: + print("No pose estimation available") + exit(1) + +# Get tag pose in camera frame +tvec = np.array(tag["pose"]["translation_vector"]) +distance = np.linalg.norm(tvec) +print("Tag tvec:", tvec) +print("Tag distance:", distance * 1000, "mm") + +# Calculate movement direction (toward the tag) +# Note: tvec is position of camera in tag frame, so -tvec points from camera to tag +direction = -tvec / distance # Unit vector toward tag +print("Direction (unit vector):", direction) +move_distance = 0.02 # 2cm toward tag + +# Calculate position adjustment in camera frame +camera_adjustment = direction * move_distance # Move toward tag + +# Try to load empirical camera->robot mapping saved by calibration script +mapping_file = Path.cwd() / "camera_to_robot_mapping.npy" +if mapping_file.exists(): + try: + M = np.load(mapping_file) + # Apply mapping: robot_delta = M @ cam_delta + # If sign/handedness is inverted, try negative mapping + robot_adjustment_xyz = -M.dot(camera_adjustment) + print(f"Loaded mapping from {mapping_file} (applied with sign flip)") + except Exception as e: + print(f"Failed to load mapping ({e}), using fallback transform") + robot_adjustment_xyz = np.array( + [camera_adjustment[1], -camera_adjustment[0], camera_adjustment[2]] + ) +else: + # Fallback heuristic (previous ad-hoc 90° rotation) + robot_adjustment_xyz = np.array( + [camera_adjustment[1], -camera_adjustment[0], camera_adjustment[2]] + ) + +camera_pose_adjustment = np.concatenate([robot_adjustment_xyz, np.zeros(3)]) + +# Simple transformation to robot frame (camera is close to gripper, minimal offset) +camera_to_robot_offset = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # No offset needed +robot_adjustment = camera_pose_adjustment + camera_to_robot_offset + +# Calculate target pose (move toward tag) +target_pose = reference_pose + robot_adjustment +print(f"Moving {move_distance * 100:.0f}cm toward tag...") +print("Target pose:", target_pose) + +# Execute move toward tag +success = rtde_c.moveL(target_pose.tolist(), speed, accel) + +if success: + print("Move toward tag completed successfully") +else: + print("Move toward tag returned False") + +# Capture new image to verify movement +print("Capturing new image to verify movement...") +new_photo_path = camera.capture_photo() +if new_photo_path: + new_image = cv2.imread(new_photo_path) + if new_image is not None: + new_detections = detector.detect_tags(new_image) + if new_detections: + new_tag = new_detections[0] # Assume same tag + if new_tag["pose"]: + new_tvec = np.array(new_tag["pose"]["translation_vector"]) + new_distance = np.linalg.norm(new_tvec) + print(f"New distance to tag: {new_distance * 1000:.1f}mm") + distance_change = distance - new_distance + if distance_change > 0: + print( + f"✅ Successfully moved closer by {distance_change * 1000:.1f}mm!" + ) + else: + print(f"❌ Moved farther away by {-distance_change * 1000:.1f}mm") + # Debug: compute observed movement projection along commanded camera->tag direction + # tvec is camera position in tag frame, so camera->tag vector is -tvec + old_cam_to_tag = -tvec + new_cam_to_tag = -new_tvec + observed_move_cam = new_cam_to_tag - old_cam_to_tag + proj_along_cmd = np.dot(observed_move_cam, direction) + print( + f"Observed movement along commanded direction: {proj_along_cmd * 1000:.1f}mm (positive = moved closer)" + ) + else: + print("No pose estimation in new image") + else: + print("No AprilTags detected in new image") + else: + print("Failed to load new image") +else: + print("Failed to capture new image") + +# Check position after move +time.sleep(0.5) +moved_pose = np.array(rtde_r.getActualTCPPose()) +print("Pose after move:", moved_pose) + +# Move back to reference position +print("Moving back to reference position...") +success = rtde_c.moveL(reference_pose.tolist(), speed, accel) + +if success: + print("Move back completed successfully") +else: + print("Move back returned False") + +# Final check +time.sleep(0.5) +final_pose = np.array(rtde_r.getActualTCPPose()) +print("Final pose:", final_pose) + +displacement = np.linalg.norm(final_pose[:3] - reference_pose[:3]) +print("Total displacement from reference:", displacement * 1000, "mm") + +rtde_c.disconnect() +rtde_r.disconnect() +print("Disconnected from robot") diff --git a/test_basic_movel.py b/test_basic_movel.py new file mode 100644 index 0000000..19e56af --- /dev/null +++ b/test_basic_movel.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +""" +Basic moveL Test Script +Tests fundamental RTDE moveL commands with UR robot +""" + +import sys +import time +import numpy as np +from pathlib import Path + +# Add src to path for imports +sys.path.insert(0, str(Path(__file__).parent / "src")) + +import rtde_control +import rtde_receive +from ur_toolkit.config_manager import config + + +robot_ip = config.get("robot.ip_address", "192.168.0.10") + +print("Connecting to UR robot at", robot_ip) + +rtde_c = rtde_control.RTDEControlInterface(robot_ip) +rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) +print("RTDE connection established") + +# Get current pose +current_pose = np.array(rtde_r.getActualTCPPose()) +print("Current pose:", current_pose) + +# Define test poses (small movements from current position) +test_poses = [ + current_pose + np.array([0.05, 0.0, 0.0, 0.0, 0.0, 0.0]), # +5cm in X + current_pose + np.array([0.0, 0.05, 0.0, 0.0, 0.0, 0.0]), # +5cm in Y + current_pose + np.array([0.0, 0.0, 0.05, 0.0, 0.0, 0.0]), # +5cm in Z + current_pose, # Back to start +] + +print("Moving to test positions...") + +for i, target_pose in enumerate(test_poses): + print("Move", i + 1, "Target:", target_pose) + + success = rtde_c.moveL(target_pose.tolist(), 0.1, 0.2) + if success: + print("Move completed successfully") + else: + print("Move returned False") + + # Check final pose + time.sleep(0.5) + final_pose = np.array(rtde_r.getActualTCPPose()) + print("Final pose:", final_pose) + + time.sleep(1) # Pause between moves + +print("Basic moveL test complete") + +rtde_c.disconnect() +rtde_r.disconnect() +print("Disconnected from robot") diff --git a/test_visual_servo_speedl.py b/test_visual_servo_speedl.py new file mode 100644 index 0000000..611f3d5 --- /dev/null +++ b/test_visual_servo_speedl.py @@ -0,0 +1,256 @@ +#!/usr/bin/env python3 +""" +Visual Servoing speedL Test Script +Tests closed-loop visual servoing using speedL commands +""" + +import sys +import time +import numpy as np +import cv2 +from pathlib import Path + +# Add src to path for imports +sys.path.insert(0, str(Path(__file__).parent / "src")) + +import rtde_control +import rtde_receive +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.config_manager import ( + config, + get_apriltag_family, + get_apriltag_size, + get_camera_calibration_file, +) +import logging +from pathlib import Path as _Path + + +robot_ip = config.get("robot.ip_address", "192.168.0.10") + +print("Connecting to UR robot at", robot_ip) + +rtde_c = rtde_control.RTDEControlInterface(robot_ip) +rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) +print("RTDE connection established") + +# Initialize camera (use configured PiCam server) +camera_config = PiCamConfig( + hostname=config.get("camera.server.host"), + port=config.get("camera.server.port"), + download_dir=str( + config.resolve_path(config.get("camera.client.download_directory", "photos")) + ), +) +camera = PiCam(camera_config) + +if not camera.test_connection(): + print("Cannot connect to camera server") + exit(1) + +print("Camera connected") + +# Initialize detector +detector = AprilTagDetector( + tag_family=get_apriltag_family(), + tag_size=get_apriltag_size(), + calibration_file=get_camera_calibration_file(), +) + +print("Detector initialized") + +# Load empirical camera->robot mapping (required) +mapping_file = Path.cwd() / "camera_to_robot_mapping.npy" +if not mapping_file.exists(): + print( + f"Required mapping file not found: {mapping_file}\nRun scripts/calibrate_camera_to_robot.py first" + ) + exit(1) + +M = np.load(mapping_file) +print(f"Loaded camera->robot mapping from: {mapping_file}") + +# Setup logging +logs_dir = _Path.cwd() / "logs" +logs_dir.mkdir(parents=True, exist_ok=True) +log_file = logs_dir / "visual_servo_last.log" +logger = logging.getLogger("visual_servo") +logger.setLevel(logging.DEBUG) +fh = logging.FileHandler(str(log_file), mode="w") +fmt = logging.Formatter("%(asctime)s %(levelname)s: %(message)s") +fh.setFormatter(fmt) +logger.addHandler(fh) +logger.info("Started visual servo script") +logger.info(f"Loaded mapping: {mapping_file}") + +# Control parameters (safer defaults) +gain = 0.06 # Velocity gain (further reduced) +max_speed = 0.006 # Maximum speed (m/s) (~6 mm/s) +tolerance = 0.005 # 5mm tolerance +max_iterations = 40 +speedl_duration = 0.15 # seconds per speedL command (short bursts) + +print("Control parameters:") +print("Gain:", gain) +print("Max speed:", max_speed * 1000, "mm/s") +print("Tolerance:", tolerance * 1000, "mm") +print("Max iterations:", max_iterations) +logger.info( + f"Control params: gain={gain}, max_speed={max_speed}, tolerance={tolerance}, max_iter={max_iterations}" +) + +# Get initial pose +initial_pose = np.array(rtde_r.getActualTCPPose()) +print("Initial pose:", initial_pose) +logger.info(f"Initial pose: {initial_pose}") + +print("Starting visual servoing...") + +# --- Pre-step: move to `pose-B-observe` to simulate a different starting pose --- +# Coordinates taken from src/ur_toolkit/positions/taught_positions.yaml +pose_B_observe = [0.025, -0.422, 0.353, -0.529, -2.253, 1.63] +logger.info("Moving to pose-B-observe to simulate unknown start (slow)") +logger.debug(f"pose-B-observe: {pose_B_observe}") +ok = rtde_c.moveL(pose_B_observe, 0.005, 0.01) +logger.info(f"moveL to B-observe returned: {ok}") +if not ok: + print("Move to pose-B-observe failed; aborting visual servo test") + rtde_c.disconnect() + rtde_r.disconnect() + exit(1) +time.sleep(0.5) +print("At pose-B-observe, starting visual servo loop toward the AprilTag") + +for iteration in range(max_iterations): + logger.info(f"Iteration {iteration + 1}/{max_iterations}") + + # Capture image + photo_path = camera.capture_photo() + if not photo_path: + print("Failed to capture photo") + continue + + # Load and detect + image = cv2.imread(photo_path) + if image is None: + print("Failed to load image") + continue + + detections = detector.detect_tags(image) + if not detections: + print("No AprilTags detected") + continue + + # Use first detection + tag = detections[0] + if not tag["pose"]: + print("No pose estimation") + continue + + # Get tag pose in camera frame + tvec = np.array(tag["pose"]["translation_vector"]) + logger.info(f"Tag distance (mm): {np.linalg.norm(tvec) * 1000:.3f}") + + # Check if we're close enough + if np.linalg.norm(tvec) < tolerance: + print("Converged to target!") + break + + # Simple control in camera frame: move toward the tag (camera->tag == -tvec) + # tvec from detector is tag->camera, so camera->tag = -tvec + camera_velocity = -gain * tvec + + # Map camera-frame velocity to robot TCP velocity using empirical mapping + # Apply sign flip consistent with calibration usage + robot_velocity_xyz = -M.dot(camera_velocity[:3]) + + velocity_cmd = np.zeros(6) + velocity_cmd[:3] = robot_velocity_xyz + + # Limit velocity (clamp robot TCP linear speed) + lin_speed = np.linalg.norm(velocity_cmd[:3]) + if lin_speed > max_speed: + velocity_cmd[:3] *= max_speed / lin_speed + # Also clamp each axis to avoid a single-axis runaway + for i in range(3): + if abs(velocity_cmd[i]) > max_speed: + velocity_cmd[i] = np.sign(velocity_cmd[i]) * max_speed + + # Only control position, keep orientation fixed + velocity_cmd[3:] = 0.0 + + logger.info(f"Velocity command (robot TCP mm/s): {velocity_cmd[:3] * 1000}") + + # Send velocity command in a short burst; catch RTDE errors + try: + success = rtde_c.speedL(velocity_cmd.tolist(), speedl_duration, 0.1) + logger.info(f"speedL returned: {success}") + except Exception: + logger.exception("speedL raised exception") + success = False + + if not success: + logger.warning("speedL failed or returned False; stopping visual servo loop") + try: + rtde_c.speedStop() + except Exception: + logger.exception("Error while calling speedStop") + break + + time.sleep(0.5) # Wait for movement + +# Stop movement +rtde_c.speedStop() +time.sleep(0.2) + +# Check final pose +final_pose = np.array(rtde_r.getActualTCPPose()) +print("Final pose:", final_pose) + +displacement = np.linalg.norm(final_pose[:3] - initial_pose[:3]) +print("Total displacement:", displacement * 1000, "mm") +logger.info(f"Final pose (before final move): {final_pose}") +logger.info(f"Total displacement (mm): {displacement * 1000}") + +# After convergence, attempt a final precise move to pose-A-observe +pose_A_observe = [-0.135, -0.588, 0.269, 0.006, -2.253, 2.139] +logger.info("Attempting final moveL to pose-A-observe (very slow)") +print("Performing final moveL to pose-A-observe for exact placement (very slow)") +try: + ok = rtde_c.moveL(pose_A_observe, 0.005, 0.01) + logger.info(f"moveL to A-observe returned: {ok}") +except Exception: + logger.exception("Final moveL raised exception") + ok = False + +if not ok: + logger.warning( + "Final move to pose-A-observe failed; attempting reconnect and retry" + ) + try: + rtde_c.disconnect() + except Exception: + logger.exception("Error during disconnect") + try: + rtde_c = rtde_control.RTDEControlInterface(robot_ip) + ok = rtde_c.moveL(pose_A_observe, 0.005, 0.01) + logger.info(f"retry moveL to A-observe returned: {ok}") + if ok: + logger.info("Final move to pose-A-observe succeeded after reconnect") + else: + logger.warning("Final move to pose-A-observe still failed after reconnect") + except Exception: + logger.exception("Reconnect + moveL attempt failed") + +try: + rtde_c.disconnect() +except Exception: + logger.exception("Error during final disconnect of control") +try: + rtde_r.disconnect() +except Exception: + logger.exception("Error during final disconnect of receive") + +logger.info("Disconnected from robot") +print(f"Wrote detailed log to: {log_file}") diff --git a/tests/debug_apriltag.py b/tests/debug_apriltag.py new file mode 100644 index 0000000..64b9c9b --- /dev/null +++ b/tests/debug_apriltag.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +""" +Debug AprilTag Detection +Test AprilTag detection on captured or saved images +""" + +import sys +import argparse +from pathlib import Path +import cv2 +import glob + +# Add src directory to path for ur_toolkit imports +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.config_manager import get_camera_host, get_camera_port + + +def process_image(image_path, detector, source_description=""): + """Process a single image for AprilTag detection""" + print(f"🔍 Processing image: {Path(image_path).name} {source_description}") + + # Load image + image = cv2.imread(str(image_path)) + if image is None: + print("❌ Failed to load image") + return False + + print(f" Image size: {image.shape[1]}x{image.shape[0]}") + + # Detect tags + detections = detector.detect_tags(image) + + print(f"🏷️ Found {len(detections)} tags:") + + if len(detections) == 0: + print(" No tags detected") + print(" Possible issues:") + print(" - Tag not visible in image") + print(" - Tag too small/large") + print(" - Poor lighting") + print(" - Wrong tag family (looking for tagStandard41h12)") + else: + for i, detection in enumerate(detections): + print(f" Tag {i+1}:") + print(f" ID: {detection.get('tag_id', 'unknown')}") + print(f" Family: {detection.get('tag_family', 'unknown')}") + print(f" Quality: {detection.get('decision_margin', 'unknown')}") + print(f" Hamming: {detection.get('hamming', 'unknown')}") + + if detection.get('pose') and detection.get('distance_mm'): + distance = detection['distance_mm'] + tvec = detection['pose']['translation_vector'] + print(f" Distance: {distance:.1f}mm") + print(f" Position: [{tvec[0]:.3f}, {tvec[1]:.3f}, {tvec[2]:.3f}]") + + # Save annotated image + annotated_image = detector.draw_detections(image, detections) + output_path = Path(image_path).parent / f"debug_apriltag_{Path(image_path).stem}.jpg" + cv2.imwrite(str(output_path), annotated_image) + print(f"💾 Annotated image saved to: {output_path.name}") + + return True + + +def main(): + parser = argparse.ArgumentParser(description='Debug AprilTag detection') + parser.add_argument('--photo', help='Path to saved photo file (if not provided, captures new photo)') + parser.add_argument('--photos', nargs='+', help='Process multiple photos or glob pattern (e.g., photos/*.jpg)') + + args = parser.parse_args() + + print("🏷️ AprilTag Detection Debug") + print("============================") + + # Initialize AprilTag detector + print("🏷️ Initializing AprilTag detector...") + detector = AprilTagDetector() + print(f" Family: {detector.tag_family}") + print(f" Tag size: {detector.tag_size}m") + print(f" Pose estimation: {'✅' if detector.pose_estimation_enabled else '❌'}") + + # Determine input mode + if args.photos: + # Process multiple photos/glob patterns + photo_paths = [] + for pattern in args.photos: + if '*' in pattern or '?' in pattern: + matches = glob.glob(pattern) + if not matches: + print(f"⚠️ No files match pattern: {pattern}") + else: + photo_paths.extend(matches) + else: + if Path(pattern).exists(): + photo_paths.append(pattern) + else: + print(f"⚠️ File not found: {pattern}") + + if not photo_paths: + print("❌ No valid photo files found") + return + + print(f"\n📁 Processing {len(photo_paths)} saved photo(s)...") + for i, photo_path in enumerate(photo_paths): + print(f"\n--- Photo {i+1}/{len(photo_paths)} ---") + process_image(photo_path, detector, "(saved)") + + elif args.photo: + # Process single saved photo + if not Path(args.photo).exists(): + print(f"❌ Photo file not found: {args.photo}") + return + + print("📁 Processing saved photo...") + process_image(args.photo, detector, "(saved)") + + else: + # Capture new photo from camera + host = get_camera_host() + port = get_camera_port() + config = PiCamConfig(hostname=host, port=port) + camera = PiCam(config) + + print("📸 Capturing new photo...") + photo_path = camera.capture_photo() + + if not photo_path: + print("❌ Failed to capture photo") + return + + print(f"✅ Photo saved to: {photo_path}") + process_image(photo_path, detector, "(captured)") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/tests/debug_apriltag_pose.py b/tests/debug_apriltag_pose.py new file mode 100644 index 0000000..7c90974 --- /dev/null +++ b/tests/debug_apriltag_pose.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python3 +""" +Debug AprilTag Pose Measurements +Observe pose readings without corrections to identify coordinate frame issues +""" + +import sys +import time +import numpy as np +from pathlib import Path + +# Add project root to path +sys.path.insert(0, str(Path(__file__).parent / "src")) + +from ur_toolkit.robots.ur.ur_controller import URController +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.pose_correction.robust_estimator import RobustPoseEstimator +from ur_toolkit.config_manager import config, get_camera_host, get_camera_port + +def rad_to_deg(rad_array): + """Convert radians to degrees""" + return np.rad2deg(rad_array) + +def unwrap_angles_deg(angles_deg): + """ + Unwrap angles to handle ±180° wraparound issues + For angles near ±180°, convert them to a consistent negative range + """ + unwrapped = np.copy(angles_deg) + for i in range(len(unwrapped)): + # If angle is > 90°, wrap to negative equivalent + if unwrapped[i] > 90: + unwrapped[i] = unwrapped[i] - 360 + return unwrapped + +def debug_pose_measurements(robot_ip="192.168.0.10", tag_id=2, num_measurements=10): + """ + Take multiple pose measurements without moving robot to check stability + """ + print("🔍 AprilTag Pose Measurement Diagnostic") + print("=" * 50) + + # Initialize systems + print("🔌 Connecting to robot...") + robot = URController(robot_ip) + # Robot auto-connects during initialization + + print("📷 Connecting to camera...") + try: + host = get_camera_host() + port = get_camera_port() + print(f"🔗 Camera target: {host}:{port}") + + cam_config = PiCamConfig(hostname=host, port=port) + camera = PiCam(cam_config) + + if not camera.test_connection(): + print("❌ Failed to connect to camera") + return + print("✅ Camera connected successfully") + except Exception as e: + print(f"❌ Camera connection failed: {e}") + return + + print("🏷️ Initializing detector...") + detector = AprilTagDetector() + if not detector.pose_estimation_enabled: + print("❌ Pose estimation not available") + return + + # Initialize robust estimator + estimator = RobustPoseEstimator( + camera_matrix=detector.camera_matrix, + dist_coeffs=detector.dist_coeffs, + tag_size=detector.tag_size + ) + + print(f"\n📍 Current robot TCP pose: {robot.get_tcp_pose()}") + print(f"🎯 Looking for AprilTag {tag_id}") + print(f"📊 Taking {num_measurements} measurements...\n") + + poses = [] + unwrapped_rotations = [] # Store unwrapped rotation angles for better statistics + + for i in range(num_measurements): + print(f"📸 Measurement {i+1}/{num_measurements}") + + # Capture image + image_path = camera.capture_photo() + if not image_path: + print("❌ Failed to capture image") + continue + + # Detect tags + import cv2 + image = cv2.imread(image_path) + detections = detector.detect_tags(image) + + # Find target tag + target_detection = None + for detection in detections: + if detection['tag_id'] == tag_id: + target_detection = detection + break + + if target_detection is None: + print(f"⚠️ Tag {tag_id} not detected") + continue + + # Get pose estimate + pose_result = estimator.estimate_pose_ransac(target_detection) + if pose_result is None: + print("❌ Pose estimation failed") + continue + + rvec, tvec, num_inliers = pose_result + pose_6dof = estimator.pose_to_6dof(rvec, tvec) + poses.append(pose_6dof) + + # Print current measurement + pos = pose_6dof[:3] + rot_deg = rad_to_deg(pose_6dof[3:]) + rot_deg_unwrapped = unwrap_angles_deg(rot_deg) + unwrapped_rotations.append(rot_deg_unwrapped) # Store for statistics + + print(f" Position: [{pos[0]:.4f}, {pos[1]:.4f}, {pos[2]:.4f}]m") + print(f" Rotation: [{rot_deg_unwrapped[0]:.1f}, {rot_deg_unwrapped[1]:.1f}, {rot_deg_unwrapped[2]:.1f}]deg") + print(f" Inliers: {num_inliers}/4") + + # Check for concerning values + if np.any(np.abs(rot_deg_unwrapped) > 45): + print(" ⚠️ Large rotation detected!") + if pos[2] < 0.1 or pos[2] > 2.0: + print(" ⚠️ Unusual distance detected!") + + print() + time.sleep(1) # Brief pause between measurements + + # Analysis + if len(poses) == 0: + print("❌ No valid poses detected") + return + + poses = np.array(poses) + print("\n📊 ANALYSIS:") + print("=" * 30) + + # Calculate statistics using unwrapped rotations for better accuracy + mean_pose = np.mean(poses, axis=0) + std_pose = np.std(poses, axis=0) + + # Calculate rotation statistics from unwrapped angles + unwrapped_rotations_array = np.array(unwrapped_rotations) + mean_rot_unwrapped = np.mean(unwrapped_rotations_array, axis=0) + std_rot_unwrapped = np.std(unwrapped_rotations_array, axis=0) + + print(f"📈 Mean pose over {len(poses)} measurements:") + print(f" Position: [{mean_pose[0]:.4f}, {mean_pose[1]:.4f}, {mean_pose[2]:.4f}]m") + print(f" Rotation: [{mean_rot_unwrapped[0]:.1f}, {mean_rot_unwrapped[1]:.1f}, {mean_rot_unwrapped[2]:.1f}]deg") + + print(f"\n📏 Standard deviation (stability):") + print(f" Position: [{std_pose[0]:.4f}, {std_pose[1]:.4f}, {std_pose[2]:.4f}]m") + print(f" Rotation: [{std_rot_unwrapped[0]:.1f}, {std_rot_unwrapped[1]:.1f}, {std_rot_unwrapped[2]:.1f}]deg") + + # Identify issues + print(f"\n🔍 POTENTIAL ISSUES:") + if np.any(np.abs(mean_rot_unwrapped) > 45): + print("❌ Large mean rotations suggest coordinate frame mismatch") + if np.any(std_rot_unwrapped > 10): + print("❌ High rotation variability suggests unstable pose estimation") + if np.any(std_pose[:3] > 0.01): + print("❌ High position variability suggests detection issues") + if mean_pose[2] < 0.2: + print("❌ Very close distance might cause perspective issues") + if mean_pose[2] > 1.5: + print("❌ Very far distance might cause detection issues") + + # Expected reasonable ranges + print(f"\n✅ EXPECTED RANGES:") + print(" Position X: -0.2 to +0.2m (left/right of camera center)") + print(" Position Y: -0.2 to +0.2m (up/down from camera center)") + print(" Position Z: 0.3 to 1.2m (distance from camera)") + print(" Rotation: -45° to +45° in each axis for normal tag orientation") + + try: + robot.disconnect() + except: + pass + +if __name__ == "__main__": + debug_pose_measurements() \ No newline at end of file diff --git a/tests/test_dynamic_pose_correction.py b/tests/test_dynamic_pose_correction.py new file mode 100644 index 0000000..d90de42 --- /dev/null +++ b/tests/test_dynamic_pose_correction.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +""" +Dynamic Pose Correction Test +Tests continuous pose correction as AprilTag or robot moves +""" + +import argparse +import sys +import time +from pathlib import Path + +# Add parent directories for imports +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +from ur_toolkit.robots.ur.ur_controller import URController +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.pose_correction.pose_correction_engine import PoseCorrectionEngine +from ur_toolkit.config_manager import config, get_camera_host, get_camera_port + + +def test_dynamic_pose_correction(robot_ip: str, tag_id: int = 2, cycles: int = 5): + """Test dynamic pose correction with multiple cycles""" + + print(f"🔄 Dynamic Pose Correction Test") + print(f"Robot IP: {robot_ip}") + print(f"Tag ID: {tag_id}") + print(f"Test cycles: {cycles}") + print("=" * 50) + + try: + # Initialize robot + print("🔌 Connecting to robot...") + robot = URController(robot_ip) + print("✅ Robot connected") + + # Initialize camera + host = get_camera_host() + port = get_camera_port() + camera_config = PiCamConfig(hostname=host, port=port) + camera = PiCam(camera_config) + + if not camera.test_connection(): + print("❌ Camera connection failed") + return False + print("✅ Camera connected") + + # Initialize AprilTag detector + detector = AprilTagDetector() + if not detector.pose_estimation_enabled: + print("❌ Camera calibration required for pose estimation") + return False + print("✅ AprilTag detector initialized") + + # Initialize pose correction engine + pose_engine = PoseCorrectionEngine(robot, camera, detector) + print("✅ Pose correction engine initialized") + + # Set initial target pose + print(f"\n🎯 Setting initial target pose from tag {tag_id}...") + if not pose_engine.set_target_from_current_detection(tag_id): + print("❌ Failed to set initial target pose") + return False + print("✅ Initial target pose set") + + print("\n⚠️ DYNAMIC TEST INSTRUCTIONS:") + print(" 1. The robot will perform multiple correction cycles") + print(" 2. Between cycles, you can:") + print(" - Move the AprilTag slightly (2-3cm)") + print(" - Manually jog the robot with teach pendant") + print(" - Leave everything stationary to test stability") + print(" 3. Each cycle will attempt to correct any detected errors") + print(" 4. Press Ctrl+C to stop the test early") + + input("\n📍 Press ENTER to start dynamic testing...") + + # Run correction cycles + successful_cycles = 0 + + for cycle in range(1, cycles + 1): + print(f"\n🔄 === Cycle {cycle}/{cycles} ===") + + # Small delay to allow for manual adjustments + print("⏳ 3 second pause for adjustments...") + time.sleep(3) + + # Perform pose correction + print(f"🚀 Starting pose correction cycle {cycle}...") + success, metrics = pose_engine.correct_to_target(tag_id) + + if success: + print(f"✅ Cycle {cycle} successful!") + print(f" Converged in {metrics['iterations']} iterations") + print(f" Final error: {metrics['final_error']:.4f}") + successful_cycles += 1 + else: + print(f"❌ Cycle {cycle} failed") + print(f" Iterations: {metrics['iterations']}") + print(f" Final error: {metrics.get('final_error', 'unknown')}") + + # Brief pause between cycles + if cycle < cycles: + print("⏸️ Brief pause before next cycle...") + time.sleep(2) + + # Results summary + print(f"\n🏁 Dynamic Test Complete!") + print(f" Successful cycles: {successful_cycles}/{cycles}") + print(f" Success rate: {successful_cycles/cycles*100:.1f}%") + + return successful_cycles == cycles + + except KeyboardInterrupt: + print("\n🛑 Test interrupted by user") + return False + except Exception as e: + print(f"❌ Test failed: {e}") + return False + finally: + # Clean up + if 'robot' in locals(): + robot.close() + print("🔌 Disconnected from robot") + + +def main(): + """Main test function""" + parser = argparse.ArgumentParser(description='Test Dynamic Pose Correction') + parser.add_argument('--robot-ip', default='192.168.0.10', help='Robot IP address') + parser.add_argument('--tag-id', type=int, default=2, help='AprilTag ID to track') + parser.add_argument('--cycles', type=int, default=5, help='Number of correction cycles') + + args = parser.parse_args() + + # Run dynamic test + success = test_dynamic_pose_correction(args.robot_ip, args.tag_id, args.cycles) + + if success: + print("\n🎉 All dynamic tests passed!") + sys.exit(0) + else: + print("\n💥 Dynamic tests failed!") + sys.exit(1) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/tests/test_pose_correction.py b/tests/test_pose_correction.py new file mode 100644 index 0000000..61b6a6e --- /dev/null +++ b/tests/test_pose_correction.py @@ -0,0 +1,209 @@ +#!/usr/bin/env python3 +""" +Test Robust Pose Correction Engine +Tests the new visual servoing approach with robust pose estimation and filtering +""" + +import argparse +import sys +from pathlib import Path + +# Add parent directories for imports +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +from ur_toolkit.robots.ur.ur_controller import URController +from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.pose_correction.pose_correction_engine import PoseCorrectionEngine +from ur_toolkit.config_manager import config, get_camera_host, get_camera_port + + +def test_pose_correction(robot_ip: str, tag_id: int = 0, dry_run: bool = False): + """ + Test the robust pose correction system + + Args: + robot_ip: Robot IP address + tag_id: AprilTag ID to use for testing + dry_run: If True, only test detection without robot movement + """ + print("🎯 Testing Robust Pose Correction Engine") + print("=" * 50) + + # Initialize components + print("\n🔌 Initializing systems...") + + try: + # Initialize robot + if not dry_run: + robot = URController(robot_ip) + if not robot.test_connection(): + print("❌ Robot connection failed") + return False + print("✅ Robot connected") + else: + robot = None + print("🔍 Dry run mode - no robot connection") + + # Initialize camera + host = get_camera_host() + port = get_camera_port() + camera_config = PiCamConfig(hostname=host, port=port) + camera = PiCam(camera_config) + + if not camera.test_connection(): + print("❌ Camera connection failed") + return False + print("✅ Camera connected") + + # Initialize AprilTag detector + detector = AprilTagDetector() + if not detector.pose_estimation_enabled: + print("❌ Camera calibration required for pose estimation") + return False + print("✅ AprilTag detector initialized with pose estimation") + + # Initialize pose correction engine + if not dry_run: + pose_engine = PoseCorrectionEngine(robot, camera, detector) + print("✅ Pose correction engine initialized") + + # Test 1: Basic detection and pose estimation + print(f"\n🔍 Test 1: Detecting AprilTag {tag_id}...") + image_path = camera.capture_photo() + if not image_path: + print("❌ Failed to capture test image") + return False + + import cv2 + image = cv2.imread(image_path) + detections = detector.detect_tags(image) + + target_detection = None + for detection in detections: + if detection['tag_id'] == tag_id: + target_detection = detection + break + + if target_detection is None: + print(f"❌ Tag {tag_id} not found in image") + print(f" Available tags: {[d['tag_id'] for d in detections]}") + return False + + print(f"✅ Tag {tag_id} detected") + print(f" Basic pose: {target_detection['pose']['translation_vector']}") + + # Test 2: Robust pose estimation + print("\n🎯 Test 2: Robust pose estimation...") + from ur_toolkit.pose_correction.robust_estimator import RobustPoseEstimator + + robust_estimator = RobustPoseEstimator( + camera_matrix=detector.camera_matrix, + dist_coeffs=detector.dist_coeffs, + tag_size=detector.tag_size + ) + + pose_result = robust_estimator.estimate_pose_ransac(target_detection) + if pose_result is None: + print("❌ Robust pose estimation failed") + return False + + rvec, tvec, num_inliers = pose_result + pose_6dof = robust_estimator.pose_to_6dof(rvec, tvec) + + print(f"✅ Robust pose estimated with {num_inliers}/4 inliers") + print(f" Position: [{pose_6dof[0]:.4f}, {pose_6dof[1]:.4f}, {pose_6dof[2]:.4f}]m") + print(f" Rotation: [{pose_6dof[3]:.4f}, {pose_6dof[4]:.4f}, {pose_6dof[5]:.4f}]rad") + + # Test 3: Kalman filtering + print("\n🔧 Test 3: Kalman filtering...") + from ur_toolkit.pose_correction.kalman_filter import PoseKalmanFilter + import numpy as np + + kalman = PoseKalmanFilter() + filtered_pose1 = kalman.update(pose_6dof) + + # Simulate second measurement with small noise + noisy_pose = pose_6dof + np.random.normal(0, 0.001, 6) + filtered_pose2 = kalman.update(noisy_pose) + + print(f"✅ Kalman filtering working") + print(f" Raw pose: [{pose_6dof[0]:.4f}, {pose_6dof[1]:.4f}, {pose_6dof[2]:.4f}]") + print(f" Filtered pose: [{filtered_pose2[0]:.4f}, {filtered_pose2[1]:.4f}, {filtered_pose2[2]:.4f}]") + + if dry_run: + print("\n✅ Dry run completed successfully") + print(" All components working correctly") + print(" Ready for full pose correction test") + return True + + # Test 4: Full pose correction (if not dry run) + print("\n🎯 Test 4: Setting target pose...") + if not pose_engine.set_target_from_current_detection(tag_id): + print("❌ Failed to set target pose") + return False + + print("✅ Target pose set from current detection") + + # Ask user if they want to proceed with movement + print("\n⚠️ The robot will now attempt to correct its pose") + print(" Make sure the workspace is clear and robot is in a safe position") + response = input(" Proceed with pose correction? (y/N): ").lower().strip() + + if response != 'y': + print("🛑 Test cancelled by user") + return True + + print("\n🚀 Test 5: Pose correction...") + success, metrics = pose_engine.correct_to_target(tag_id) + + if success: + print("✅ Pose correction successful!") + print(f" Converged in {metrics['iterations']} iterations") + print(f" Final error: {metrics['final_error']:.4f}") + else: + print("❌ Pose correction failed") + print(f" Iterations: {metrics['iterations']}") + print(f" Final error: {metrics.get('final_error', 'unknown')}") + + return success + + except Exception as e: + print(f"❌ Test failed: {e}") + return False + finally: + # Clean up + if 'robot' in locals() and robot is not None: + robot.close() + + +def main(): + """Main test function""" + parser = argparse.ArgumentParser(description='Test Robust Pose Correction Engine') + parser.add_argument('--robot-ip', default=None, + help='Robot IP address (overrides config)') + parser.add_argument('--tag-id', type=int, default=2, + help='AprilTag ID to use for testing (default: 2)') + parser.add_argument('--dry-run', action='store_true', + help='Test detection only, no robot movement') + + args = parser.parse_args() + + robot_ip = args.robot_ip or config.get('robot.ip_address', '192.168.1.100') + + print(f"Robot IP: {robot_ip}") + print(f"Tag ID: {args.tag_id}") + print(f"Dry run: {args.dry_run}") + + success = test_pose_correction(robot_ip, args.tag_id, args.dry_run) + + if success: + print("\n🎉 All tests passed!") + sys.exit(0) + else: + print("\n❌ Tests failed") + sys.exit(1) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/tests/test_visual_servo.py b/tests/test_visual_servo.py index f2fd3f7..dc4cd92 100644 --- a/tests/test_visual_servo.py +++ b/tests/test_visual_servo.py @@ -7,16 +7,15 @@ import sys from pathlib import Path -# Add parent directories to path -sys.path.insert(0, str(Path(__file__).parent.parent)) -sys.path.insert(0, str(Path(__file__).parent.parent / "setup")) - -from config_manager import config -from workflow.workflow_executor import WorkflowExecutor -from visual_servo.visual_servo_engine import VisualServoEngine -from visual_servo.config import visual_servo_config -from apriltag_detection import AprilTagDetector -from robots.ur.ur_controller import URController +# Add src directory to path for ur_toolkit imports +sys.path.insert(0, str(Path(__file__).parent.parent / "src")) + +from ur_toolkit.config_manager import config +from ur_toolkit.workflow.workflow_executor import WorkflowExecutor +from ur_toolkit.visual_servo.visual_servo_engine import VisualServoEngine +from ur_toolkit.visual_servo.config import visual_servo_config +from ur_toolkit.apriltag_detection import AprilTagDetector +from ur_toolkit.robots.ur.ur_controller import URController def test_visual_servo_components(): diff --git a/ur_rtde_test.py b/ur_rtde_test.py new file mode 100644 index 0000000..506656e --- /dev/null +++ b/ur_rtde_test.py @@ -0,0 +1,488 @@ +#!/usr/bin/env python3 +""" +UR Robot RTDE Test Script +Tests basic moveL commands and AprilTag-based positioning + +This script provides a progression from basic RTDE commands to +AprilTag-assisted positioning and visual servoing: + +1. Basic moveL testing with RTDE +2. AprilTag pose detection and moveL to detected pose +3. Visual servoing with speedL for precise positioning + +Usage: + python ur_rtde_test.py --robot-ip 192.168.0.10 --test-basic + python ur_rtde_test.py --robot-ip 192.168.0.10 --test-apriltag + python ur_rtde_test.py --robot-ip 192.168.0.10 --test-visual-servo +""" + +import sys +import time +import argparse +import numpy as np +from pathlib import Path + +# Add src to path for imports +sys.path.insert(0, str(Path(__file__).parent / "src")) + +try: + import rtde_control + import rtde_receive + + RTDE_AVAILABLE = True +except ImportError: + print("❌ RTDE libraries not available. Install UR RTDE:") + print(" pip install ur-rtde") + RTDE_AVAILABLE = False + +try: + from pupil_apriltags import Detector + + APRILTAG_AVAILABLE = True +except ImportError: + print("❌ pupil-apriltags not available. Install:") + print(" pip install pupil-apriltags") + APRILTAG_AVAILABLE = False + +# Import camera and detection modules +try: + from ur_toolkit.camera.picam.picam import PiCam, PiCamConfig + from ur_toolkit.apriltag_detection import AprilTagDetector + from ur_toolkit.config_manager import ( + get_apriltag_family, + get_apriltag_size, + get_camera_calibration_file, + ) + + FULL_SYSTEM_AVAILABLE = True +except ImportError as e: + print(f"⚠️ Full system not available: {e}") + FULL_SYSTEM_AVAILABLE = False + PiCam = None + PiCamConfig = None + AprilTagDetector = None + +from config_manager import config + + +class URRTDETest: + """Test class for UR robot RTDE operations""" + + def __init__(self, robot_ip): + self.robot_ip = robot_ip + self.rtde_c = None + self.rtde_r = None + + # Camera parameters for AprilTag detection + self.camera_params = [500, 500, 320, 240] # fx, fy, cx, cy + self.tag_size = 0.023 # 23mm AprilTag + + # Initialize AprilTag detector if available + self.camera = None + self.detector = None + if FULL_SYSTEM_AVAILABLE: + try: + # Initialize camera + camera_config = PiCamConfig() + self.camera = PiCam(camera_config) + + # Initialize detector + self.detector = AprilTagDetector( + tag_family=get_apriltag_family(), + tag_size=get_apriltag_size(), + calibration_file=get_camera_calibration_file(), + ) + print("✅ Camera and detector initialized") + except Exception as e: + print(f"⚠️ Camera/detector initialization failed: {e}") + else: + print("⚠️ Full system not available - AprilTag features disabled") + + def connect(self): + """Connect to UR robot via RTDE""" + if not RTDE_AVAILABLE: + raise RuntimeError("RTDE libraries not available") + + print(f"🤖 Connecting to UR robot at {self.robot_ip}...") + + try: + self.rtde_c = rtde_control.RTDEControlInterface(self.robot_ip) + self.rtde_r = rtde_receive.RTDEReceiveInterface(self.robot_ip) + print("✅ RTDE connection established") + return True + except Exception as e: + print(f"❌ RTDE connection failed: {e}") + return False + + def disconnect(self): + """Disconnect from robot""" + if self.rtde_c: + self.rtde_c.disconnect() + if self.rtde_r: + self.rtde_r.disconnect() + print("🔌 Disconnected from robot") + + def get_current_pose(self): + """Get current TCP pose""" + if not self.rtde_r: + return None + pose = self.rtde_r.getActualTCPPose() + return np.array(pose) + + def print_pose(self, pose, label="Pose"): + """Print pose in readable format""" + if pose is None: + print(f"{label}: None") + return + print( + f"{label}: [{pose[0]:.3f}, {pose[1]:.3f}, {pose[2]:.3f}, " + f"{pose[3]:.3f}, {pose[4]:.3f}, {pose[5]:.3f}]" + ) + + def test_basic_movel(self): + """Test basic moveL commands""" + print("\n🧪 Testing Basic moveL Commands") + print("=" * 40) + + # Get current pose + current_pose = self.get_current_pose() + self.print_pose(current_pose, "Current pose") + + # Define test poses (small movements from current position) + test_poses = [ + current_pose + np.array([0.05, 0.0, 0.0, 0.0, 0.0, 0.0]), # +5cm in X + current_pose + np.array([0.0, 0.05, 0.0, 0.0, 0.0, 0.0]), # +5cm in Y + current_pose + np.array([0.0, 0.0, 0.05, 0.0, 0.0, 0.0]), # +5cm in Z + current_pose, # Back to start + ] + + print("\n📍 Moving to test positions...") + + for i, target_pose in enumerate(test_poses): + print(f"\nMove {i+1}: Target pose") + self.print_pose(target_pose) + + try: + # moveL with default speed/accel + success = self.rtde_c.moveL(target_pose.tolist(), 0.1, 0.2) + if success: + print("✅ Move completed successfully") + else: + print("⚠️ Move returned False") + + # Wait a moment and check final pose + time.sleep(0.5) + final_pose = self.get_current_pose() + self.print_pose(final_pose, "Final pose") + + # Calculate error but don't use it (for now) + _ = np.linalg.norm(final_pose - target_pose) + + except Exception as e: + print(f"❌ Move failed: {e}") + + time.sleep(1) # Pause between moves + + print("\n✅ Basic moveL test complete") + + def test_apriltag_movel(self): + """Test AprilTag-based moveL positioning""" + print("\n🏷️ Testing AprilTag-based moveL") + print("=" * 40) + + if not FULL_SYSTEM_AVAILABLE or not self.camera or not self.detector: + print("❌ AprilTag system not available") + return + + # Test camera connection + if not self.camera.test_connection(): + print("❌ Cannot connect to camera server") + return + + print("📷 Capturing image...") + photo_path = self.camera.capture_photo() + if not photo_path: + print("❌ Failed to capture photo") + return + + # Load image + import cv2 + + image = cv2.imread(photo_path) + if image is None: + print("❌ Failed to load image") + return + + print("🔍 Detecting AprilTags...") + detections = self.detector.detect_tags(image) + + if not detections: + print("❌ No AprilTags detected") + return + + print(f"✅ Detected {len(detections)} AprilTags") + + # Use first detection + tag = detections[0] + print(f"🎯 Using Tag ID {tag['tag_id']}") + + if not tag["pose"]: + print("❌ No pose estimation available (missing calibration)") + return + + # Get tag pose in camera frame + tvec = np.array(tag["pose"]["translation_vector"]) + rvec = np.array(tag["pose"]["rotation_vector"]) + + print("📍 Tag pose in camera frame:") + print( + f" X: {tvec[0]*1000:.1f}mm, Y: {tvec[1]*1000:.1f}mm, Z: {tvec[2]*1000:.1f}mm" + ) + # Simple transformation: assume camera is roughly aligned with robot + # For now, just offset in Z direction (camera looks down) + # TODO: Use proper hand-eye calibration matrix + camera_to_robot_offset = np.array([0.0, 0.0, 0.1, 0.0, 0.0, 0.0]) # 10cm offset + + tag_pose_camera = np.concatenate([tvec, rvec]) + tag_pose_robot = tag_pose_camera + camera_to_robot_offset + + print("🤖 Tag pose in robot frame (simple offset):") + self.print_pose(tag_pose_robot) + + # Move to a position above the tag + approach_offset = np.array([0.0, 0.0, 0.05, 0.0, 0.0, 0.0]) # 5cm above + target_pose = tag_pose_robot + approach_offset + + print("🎯 Target pose (5cm above tag):") + self.print_pose(target_pose) + + # Confirm movement + response = input("🤖 Execute moveL to tag position? (y/N): ") + if response.lower() != "y": + print("❌ Movement cancelled") + return + + try: + print("🚀 Moving to tag position...") + success = self.rtde_c.moveL(target_pose.tolist(), 0.05, 0.1) + if success: + print("✅ Move completed successfully") + else: + print("⚠️ Move returned False") + + # Check final pose + time.sleep(0.5) + final_pose = self.get_current_pose() + self.print_pose(final_pose, "Final pose") + + except Exception as e: + print(f"❌ Move failed: {e}") + + print("\n✅ AprilTag-based moveL test complete") + + def test_visual_servo_speedl(self): + """Test visual servoing with speedL""" + print("\n👁️ Testing Visual Servoing with speedL") + print("=" * 40) + + if not FULL_SYSTEM_AVAILABLE or not self.camera or not self.detector: + print("❌ AprilTag system not available") + return + + # Test camera connection + if not self.camera.test_connection(): + print("❌ Cannot connect to camera server") + return + + print("🎯 Starting visual servoing test...") + print(" This will move the robot using speedL based on AprilTag detection") + + # Get initial pose + initial_pose = self.get_current_pose() + self.print_pose(initial_pose, "Initial pose") + + # Control parameters + gain = 0.3 # Velocity gain + max_speed = 0.02 # Maximum speed (m/s) + tolerance = 0.005 # 5mm tolerance + max_iterations = 20 + + print("🎛️ Control parameters:") + print(f" Gain: {gain}") + print(f" Max speed: {max_speed*1000:.0f}mm/s") + print(f" Tolerance: {tolerance*1000:.0f}mm") + print(f" Max iterations: {max_iterations}") + + # Confirm start + response = input("🤖 Start visual servoing? (y/N): ") + if response.lower() != "y": + print("❌ Visual servoing cancelled") + return + + try: + for iteration in range(max_iterations): + print(f"\n🔄 Iteration {iteration + 1}/{max_iterations}") + + # Capture image + photo_path = self.camera.capture_photo() + if not photo_path: + print("❌ Failed to capture photo") + continue + + # Load and detect + import cv2 + + image = cv2.imread(photo_path) + if image is None: + print("❌ Failed to load image") + continue + + detections = self.detector.detect_tags(image) + if not detections: + print("❌ No AprilTags detected") + continue + + # Use first detection + tag = detections[0] + if not tag["pose"]: + print("❌ No pose estimation") + continue + + # Get tag pose in camera frame + tvec = np.array(tag["pose"]["translation_vector"]) + + print(f" Tag distance: {np.linalg.norm(tvec)*1000:.1f}mm") + # For visual servoing, we want to move toward the tag + # Simple control: move in direction opposite to tag position + # (negative feedback control) + velocity_cmd = -gain * tvec + + # Limit velocity + speed = np.linalg.norm(velocity_cmd[:3]) + if speed > max_speed: + velocity_cmd[:3] *= max_speed / speed + + # Only control position, keep orientation fixed + velocity_cmd[3:] = 0.0 + + print(f" Velocity command: {velocity_cmd[:3]*1000}") + # Check if we're close enough + if np.linalg.norm(tvec) < tolerance: + print("✅ Converged to target!") + break + + # Send velocity command + print("🚀 Sending speedL command...") + success = self.rtde_c.speedL( + velocity_cmd.tolist(), 1.0, 0.1 + ) # 1s duration + + if not success: + print("⚠️ speedL command failed") + break + + time.sleep(0.5) # Wait for movement + + # Stop movement + print("🛑 Stopping movement...") + self.rtde_c.speedStop() + time.sleep(0.2) + + # Check final pose + final_pose = self.get_current_pose() + self.print_pose(final_pose, "Final pose") + + displacement = np.linalg.norm(final_pose[:3] - initial_pose[:3]) + print(f" Total displacement: {displacement*1000:.1f}mm") + except KeyboardInterrupt: + print("\n🛑 Interrupted by user") + self.rtde_c.speedStop() + except Exception as e: + print(f"❌ Visual servoing failed: {e}") + try: + self.rtde_c.speedStop() + except Exception: + pass + + print("\n✅ Visual servoing test complete") + + def run_test(self, test_type): + """Run specified test""" + if not self.connect(): + return False + + try: + if test_type == "basic": + self.test_basic_movel() + elif test_type == "apriltag": + self.test_apriltag_movel() + elif test_type == "visual-servo": + self.test_visual_servo_speedl() + else: + print(f"❌ Unknown test type: {test_type}") + return False + + finally: + self.disconnect() + + return True + + +def main(): + """Main test function""" + parser = argparse.ArgumentParser(description="UR Robot RTDE Test Script") + parser.add_argument( + "--robot-ip", default=None, help="Robot IP address (overrides config)" + ) + parser.add_argument( + "--test-basic", action="store_true", help="Test basic moveL commands" + ) + parser.add_argument( + "--test-apriltag", action="store_true", help="Test AprilTag-based positioning" + ) + parser.add_argument( + "--test-visual-servo", + action="store_true", + help="Test visual servoing with speedL", + ) + + args = parser.parse_args() + + # Get robot IP + robot_ip = args.robot_ip or config.get("robot.ip_address", "192.168.0.10") + + print("=" * 60) + print("UR Robot RTDE Test Script") + print("=" * 60) + print(f"Robot IP: {robot_ip}") + print() + + # Determine test type + test_type = None + if args.test_basic: + test_type = "basic" + elif args.test_apriltag: + test_type = "apriltag" + elif args.test_visual_servo: + test_type = "visual-servo" + + if not test_type: + print("❌ Please specify a test type:") + print(" --test-basic : Test basic moveL commands") + print(" --test-apriltag : Test AprilTag positioning") + print(" --test-visual-servo : Test visual servoing") + return + + # Run test + tester = URRTDETest(robot_ip) + success = tester.run_test(test_type) + + if success: + print("\n✅ Test completed successfully") + else: + print("\n❌ Test failed") + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/visual-servo-transcript.txt b/visual-servo-transcript.txt new file mode 100644 index 0000000..87bf02c --- /dev/null +++ b/visual-servo-transcript.txt @@ -0,0 +1,431 @@ +Skip to content +Chat history + +This is a copy of a conversation between ChatGPT & Anonymous. + +Report conversation +You said: +In visual servo'ing with AprilTags, is it set up in a way where the algorithm is controlling the six joint angles to try to minimize the mismatch in the desired x,y,z,roll,pitch,yaw of the final state (i.e., where the desired x,y,z,roll,pitch,yaw is manually determined in advance by manually moving the robot arm into position and capturing a picture of the nearby apriltag) +ChatGPT said: +Yes. In a common visual servoing setup with AprilTags (or other fiducials), the loop works this way: + +Pose estimation: The AprilTag is detected in the camera feed. Its relative 6-DoF pose (x, y, z, roll, pitch, yaw) with respect to the camera frame is estimated. + +Desired pose specification: A reference pose is defined. Often this comes from moving the robot end effector into the desired position and orientation, recording the AprilTag’s observed pose at that moment. That becomes the “goal” state. + +Error computation: Each new camera frame gives a current pose estimate. The error is the difference between the current and desired pose in SE(3). + +Control law: The error is mapped into joint commands. Standard image-based or pose-based visual servoing uses the manipulator’s Jacobian (camera-to-end-effector mapping) to compute the joint velocity vector that reduces error. + +Joint updates: The algorithm does not directly try to control x,y,z,roll,pitch,yaw. It computes joint velocity (or incremental position) commands that move the arm so the observed AprilTag pose converges to the reference. + +So your description is essentially correct: the desired state is manually set (often by demonstration), and the servoing loop tries to minimize pose mismatch. The nuance is that the optimization happens in the space of robot joint commands, not directly in task space, though task-space error is what is being reduced. + +Do you want me to sketch the difference between image-based visual servoing (IBVS) and position-based visual servoing (PBVS)? That distinction matters here. + +You said: +Yes +ChatGPT said: +Two main variants: + +Position-Based Visual Servoing (PBVS) +Pipeline: Camera → AprilTag detection → Full 6-DoF pose estimate (tag wrt camera) → Error wrt goal pose → Convert to joint velocities via Jacobian. + +What it minimizes: Pose error in SE(3) (x,y,z,roll,pitch,yaw). + +Strengths: Intuitive, uses explicit geometry, goal pose defined once by demonstration. + +Weaknesses: Sensitive to calibration errors (camera intrinsics/extrinsics, robot kinematics). Noise in pose estimation directly affects control. + +Image-Based Visual Servoing (IBVS) +Pipeline: Camera → Track 2D AprilTag image features (corners, center) → Error in pixel coordinates relative to reference → Use image Jacobian to map error to joint velocities. + +What it minimizes: Feature error in the image plane. + +Strengths: Robust to some calibration errors. Works without reconstructing full 3D pose. + +Weaknesses: Less intuitive, potential singularities, trajectory in 3D may be unpredictable. + +Key Distinction +PBVS: Minimizes 6-DoF geometric error directly. + +IBVS: Minimizes 2D feature error, with the 3D pose only implicit. + +Both approaches ultimately generate velocity or torque commands for the joints, not direct commands in task space. The “manual placement and snapshot” you described matches PBVS. + +Want me to show how the AprilTag pose estimate is mathematically converted into joint velocity commands in PBVS? + +You said: +Yes +ChatGPT said: +Here’s the PBVS chain, step by step: + +1. Tag Pose Estimate +Camera detects AprilTag. Solve PnP → get homogeneous transform + +c +T +t +∈ +S +E +( +3 +) +c + T +t +​ + ∈SE(3) +(tag frame wrt camera frame). + +If end effector and camera transform are known ( +b +T +e +b + T +e +​ + , +e +T +c +e + T +c +​ + ), you can map tag pose into base frame. + +2. Desired Pose +Record desired tag pose +c +T +t +∗ +c + T +t +∗ +​ + when robot is in the correct configuration (by demonstration). + +3. Pose Error in SE(3) +Compute error transform: + +T +e +r +r += +( +c +T +t +) +− +1 +  +c +T +t +∗ +T +err +​ + =( +c + T +t +​ + ) +−1 + +c + T +t +∗ +​ + +Extract twist error (linear + angular velocity needed to correct): + +e += +[ +t +e +r +r +θ +  +u +] +e=[ +t +err +​ + +θu +​ + ] +where +t +e +r +r +t +err +​ + is translation, +θ +u +θu is rotation vector from axis–angle. + +4. Control Law +Robot’s spatial velocity of the end effector: + +v += +− +λ +  +e +v=−λe +with gain +λ +> +0 +λ>0. + +5. Map to Joint Velocities +Use robot geometric Jacobian +J +( +q +) +J(q): + +q +˙ += +J +† +( +q +) +  +v +q +˙ +​ + =J +† + (q)v +where +J +† +J +† + is pseudoinverse. + +This produces instantaneous joint velocity commands +q +˙ +q +˙ +​ + . Integrating yields new joint angles. + +6. Feedback Loop +Execute +q +˙ +q +˙ +​ + for a short control period. + +Update tag detection → recompute pose → update error → recompute +q +˙ +q +˙ +​ + . + +Repeat until error is small. + +So: AprilTag → SE(3) pose → SE(3) error → 6D twist → Jacobian inverse → joint velocities. + +Do you want me to write out a compact Python-style pseudocode loop for this? + +You said: +Put this into perspective for me compared with doing something simpler with multi-objective gradient descent +ChatGPT said: +Comparison: + +Visual Servoing (PBVS with AprilTags) +Structure: +Uses known robot kinematics and camera geometry. The Jacobian provides an analytic mapping from 6D pose error → joint velocities. + +Objective: +Explicitly reduce task-space error (Δx, Δy, Δz, Δroll, Δpitch, Δyaw). + +Dynamics: +Online, feedback-driven, continuous loop. Small corrections at control frequency. + +Pros: +Deterministic, interpretable, well-conditioned if calibration is correct. + +Cons: +Needs good models of camera and robot. Sensitive to noise in pose estimation. + +Multi-Objective Gradient Descent (simpler alternative) +Setup: +Define a cost function + +J +( +q +) += +w +t +∥ +x +( +q +) +− +x +∗ +∥ +2 ++ +w +r +∥ +R +( +q +) +− +R +∗ +∥ +2 +J(q)=w +t +​ + ∥x(q)−x +∗ + ∥ +2 + +w +r +​ + ∥R(q)−R +∗ + ∥ +2 + +where +x +( +q +) +x(q) and +R +( +q +) +R(q) are end-effector position and orientation from forward kinematics. + +Computation: +Numerically approximate gradients of +J +( +q +) +J(q) wrt joints and update +q +← +q +− +α +∇ +J +( +q +) +q←q−α∇J(q). + +Dynamics: +Batch optimization style. Not naturally coupled to control loop unless wrapped in one. + +Pros: +Easy to implement without explicit Jacobian. Flexible for arbitrary cost terms. + +Cons: +Slow convergence, may get stuck in local minima, no guarantee of smooth or stable motion. Not inherently reactive to sensor updates. + +Perspective +Visual servoing is essentially a control-theoretic gradient descent but with structure. The Jacobian acts as the analytic gradient of the error wrt joints, which makes convergence fast and stable. + +Plain gradient descent treats it like a black-box optimization. That’s fine for offline planning or coarse corrections, but in real-time closed-loop control it’s fragile. + +Want me to show the explicit equivalence, i.e. how the Jacobian transpose control law is mathematically a gradient descent step on the squared error cost? + +You said: +Maybe hill climbing is more what I meant +ChatGPT said: +Then the contrast is sharper: + +Visual Servoing +Uses analytic mapping (Jacobian) between joint space and task-space error. + +Updates all joints in a coordinated way to reduce the 6D error vector. + +Guarantees smooth convergence under small steps and correct calibration. + +Continuous feedback, not trial-and-error. + +Hill Climbing +Very simple: perturb one or a few joint angles, observe whether task-space error improves. + +Keep changes that decrease error, discard those that don’t. + +No explicit Jacobian, no global sense of how joints interact. + +May require many evaluations, motion looks jittery, convergence is slower. + +Prone to local minima (e.g., different arm postures giving same tag view). + +Perspective +Visual servoing is essentially “hill climbing with a map.” Hill climbing samples locally and guesses direction. Servoing computes the best local direction explicitly. Both are gradient-like, but servoing leverages geometry to avoid blind search. + +Do you want me to sketch a toy loop that shows side-by-side how a servo controller update differs from a naive hill-climbing update? + +You said: +What's typical for frequency when doing PBVS with AprilTags (acquisition/decision rate) +ChatGPT said: +Typical numbers: + +Camera frame rate: AprilTag detection usually runs at 20–60 Hz on a standard RGB camera, depending on resolution and CPU/GPU. Lower if you push high resolution or have many tags in view. + +Pose estimation: Once the tag is detected, PnP solve is negligible (≪1 ms on CPU). + +Control loop: PBVS controllers usually close the loop at the camera rate (so 20–60 Hz). Some implementations decouple vision from low-level servo and run vision at 30 Hz, inner robot joint control at 500–1000 Hz. + +Effective bandwidth: You get smooth behavior if the visual loop is ≥10 Hz. Below that, motion is jerky. 30 Hz is a common sweet spot. + +So: AprilTags → 20–60 Hz vision updates → PBVS loop runs at that rate → robot executes velocity commands at its faster servo rate. + +Do you want me to give you rule-of-thumb latency budgets (camera, detection, control, actuation) to show what dominates? + + + + + +No file chosenNo file chosen +ChatGPT can make mistakes. Check important info. See Cookie Preferences. diff --git a/visual_servo_animation.gif b/visual_servo_animation.gif new file mode 100644 index 0000000..f191060 Binary files /dev/null and b/visual_servo_animation.gif differ diff --git a/visual_servo_simulation.py b/visual_servo_simulation.py new file mode 100644 index 0000000..68be8fa --- /dev/null +++ b/visual_servo_simulation.py @@ -0,0 +1,434 @@ +#!/usr/bin/env python3 +""" +Visual Servoing Simulation with speedL Control +Demonstrates PBVS approach using Cartesian velocity control + +This simulation demonstrates the speedL() implementation recommended for +visual servoing with UR robots. It shows how Cartesian velocity control +can achieve smooth convergence to a target pose. + +Usage: + python3 visual_servo_simulation.py + +Output: + - visual_servo_trajectory.png: Full trajectory with error/velocity plots + - visual_servo_snapshots.png: Snapshots at different stages + +The simulation models: + - PBVS control law with gain λ + - speedL() Cartesian velocity commands + - Measurement noise (realistic sensor noise) + - 30 Hz control rate (typical for vision feedback) +""" + +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.patches import FancyArrowPatch +from mpl_toolkits.mplot3d import proj3d +from scipy.spatial.transform import Rotation as R +import time + +class Arrow3D(FancyArrowPatch): + """3D arrow for visualization""" + def __init__(self, xs, ys, zs, *args, **kwargs): + FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs) + self._verts3d = xs, ys, zs + + def do_3d_projection(self, renderer=None): + xs3d, ys3d, zs3d = self._verts3d + xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.M) + self.set_positions((xs[0], ys[0]), (xs[1], ys[1])) + return np.min(zs) + +class VisualServoSimulation: + """Simulates PBVS with speedL control""" + + def __init__(self): + # Target AprilTag pose (goal) + self.target_pose = np.array([0.5, 0.3, 0.4, 0.0, 0.0, 0.0]) + + # Initial robot pose (offset from target with significant rotation) + # Large rotation offsets make orientation convergence more visible + self.current_pose = np.array([0.4, 0.25, 0.35, 0.5, -0.4, 0.3]) + + # Control parameters + self.control_gain = 0.5 # Increased for faster convergence + self.dt = 1.0/30.0 # 30 Hz + self.max_velocity = 0.1 # m/s (increased) + + # History tracking + self.pose_history = [self.current_pose.copy()] + self.error_history = [] + self.velocity_history = [] + + # Noise simulation + self.measurement_noise_std = 0.001 # 1mm std dev + + def add_measurement_noise(self, pose): + """Add realistic measurement noise""" + noise = np.random.normal(0, self.measurement_noise_std, 6) + noise[3:] *= 0.01 # Less noise on orientation + return pose + noise + + def compute_pose_error(self, current, target): + """Compute SE(3) pose error""" + # Simple difference for simulation + error = target - current + return error + + def speedL_control(self, pose_error): + """Simulate speedL Cartesian velocity control""" + # Compute velocity command + v_task = self.control_gain * pose_error + + # Apply velocity limits + trans_vel = np.linalg.norm(v_task[:3]) + if trans_vel > self.max_velocity: + v_task[:3] *= self.max_velocity / trans_vel + + return v_task + + def step(self): + """Execute one control iteration""" + # Measure current pose (with noise) + measured_pose = self.add_measurement_noise(self.current_pose) + + # Compute error + pose_error = self.compute_pose_error(measured_pose, self.target_pose) + self.error_history.append(np.linalg.norm(pose_error[:3])) + + # Compute velocity command using speedL + v_task = self.speedL_control(pose_error) + self.velocity_history.append(np.linalg.norm(v_task[:3])) + + # Integrate velocity to update pose + self.current_pose += v_task * self.dt + self.pose_history.append(self.current_pose.copy()) + + return np.linalg.norm(pose_error[:3]) + + def run_simulation(self, max_iters=50, tolerance=0.002): + """Run visual servoing simulation""" + print("🎯 Visual Servoing Simulation Starting") + print(f"Initial error: {np.linalg.norm(self.current_pose[:3] - self.target_pose[:3]):.4f}m") + + for i in range(max_iters): + error = self.step() + + if i % 10 == 0: + print(f"Iter {i:2d}: Error = {error:.4f}m, Velocity = {self.velocity_history[-1]:.4f}m/s") + + if error < tolerance: + print(f"✅ Converged at iteration {i+1}") + print(f"Final error: {error:.4f}m") + return True + + print(f"⚠️ Did not converge within {max_iters} iterations") + print(f"Final error: {error:.4f}m") + return False + + def visualize_trajectory(self, filename='visual_servo_trajectory.png'): + """Create 3D trajectory visualization""" + fig = plt.figure(figsize=(15, 10)) + + # 3D trajectory plot + ax1 = fig.add_subplot(2, 2, 1, projection='3d') + poses = np.array(self.pose_history) + + # Plot trajectory + ax1.plot(poses[:, 0], poses[:, 1], poses[:, 2], + 'b-', linewidth=2, label='Robot trajectory') + ax1.scatter(poses[0, 0], poses[0, 1], poses[0, 2], + c='green', s=100, marker='o', label='Start') + ax1.scatter(poses[-1, 0], poses[-1, 1], poses[-1, 2], + c='blue', s=100, marker='o', label='End') + ax1.scatter(self.target_pose[0], self.target_pose[1], self.target_pose[2], + c='red', s=200, marker='*', label='Target') + + # Draw coordinate frames at start and end + scale = 0.05 + # Start frame (green) + start_pos = poses[0, :3] + for i, color in enumerate(['red', 'green', 'blue']): + vec = np.zeros(3) + vec[i] = scale + arrow = Arrow3D([start_pos[0], start_pos[0]+vec[0]], + [start_pos[1], start_pos[1]+vec[1]], + [start_pos[2], start_pos[2]+vec[2]], + mutation_scale=10, lw=2, arrowstyle='->', color=color, alpha=0.5) + ax1.add_artist(arrow) + + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('3D Trajectory (speedL Control)', fontsize=12, fontweight='bold') + ax1.legend() + ax1.grid(True, alpha=0.3) + + # Error over time + ax2 = fig.add_subplot(2, 2, 2) + iterations = np.arange(len(self.error_history)) + ax2.plot(iterations, np.array(self.error_history) * 1000, 'r-', linewidth=2) + ax2.axhline(y=2, color='g', linestyle='--', label='Tolerance (2mm)') + ax2.set_xlabel('Iteration') + ax2.set_ylabel('Position Error (mm)') + ax2.set_title('Convergence Plot', fontsize=12, fontweight='bold') + ax2.grid(True, alpha=0.3) + ax2.legend() + + # Velocity over time + ax3 = fig.add_subplot(2, 2, 3) + ax3.plot(iterations, np.array(self.velocity_history) * 1000, 'b-', linewidth=2) + ax3.set_xlabel('Iteration') + ax3.set_ylabel('Velocity (mm/s)') + ax3.set_title('Control Velocity (speedL)', fontsize=12, fontweight='bold') + ax3.grid(True, alpha=0.3) + + # XYZ position over time + ax4 = fig.add_subplot(2, 2, 4) + ax4.plot(iterations, poses[:-1, 0], 'r-', label='X', linewidth=2) + ax4.plot(iterations, poses[:-1, 1], 'g-', label='Y', linewidth=2) + ax4.plot(iterations, poses[:-1, 2], 'b-', label='Z', linewidth=2) + ax4.axhline(y=self.target_pose[0], color='r', linestyle='--', alpha=0.5) + ax4.axhline(y=self.target_pose[1], color='g', linestyle='--', alpha=0.5) + ax4.axhline(y=self.target_pose[2], color='b', linestyle='--', alpha=0.5) + ax4.set_xlabel('Iteration') + ax4.set_ylabel('Position (m)') + ax4.set_title('Position Components', fontsize=12, fontweight='bold') + ax4.legend() + ax4.grid(True, alpha=0.3) + + plt.tight_layout() + plt.savefig(filename, dpi=150, bbox_inches='tight') + print(f"📊 Saved visualization to {filename}") + plt.close() + + def visualize_snapshots(self, filename='visual_servo_snapshots.png'): + """Create snapshots at different stages""" + fig, axes = plt.subplots(1, 4, figsize=(20, 5), subplot_kw={'projection': '3d'}) + + poses = np.array(self.pose_history) + n_poses = len(poses) + snapshot_indices = [0, n_poses//3, 2*n_poses//3, n_poses-1] + titles = ['Start', 'Early', 'Mid', 'Final'] + + for ax, idx, title in zip(axes, snapshot_indices, titles): + # Current pose + current = poses[idx] + + # Plot target + ax.scatter(self.target_pose[0], self.target_pose[1], self.target_pose[2], + c='red', s=300, marker='*', label='Target', alpha=0.8) + + # Plot current + ax.scatter(current[0], current[1], current[2], + c='blue', s=200, marker='o', label='Robot', alpha=0.8) + + # Draw line to target + ax.plot([current[0], self.target_pose[0]], + [current[1], self.target_pose[1]], + [current[2], self.target_pose[2]], + 'k--', alpha=0.5, linewidth=2) + + # Draw coordinate frame + scale = 0.08 + colors = ['red', 'green', 'blue'] + for i, color in enumerate(colors): + vec = np.zeros(3) + vec[i] = scale + arrow = Arrow3D([current[0], current[0]+vec[0]], + [current[1], current[1]+vec[1]], + [current[2], current[2]+vec[2]], + mutation_scale=15, lw=2, arrowstyle='->', color=color) + ax.add_artist(arrow) + + error = np.linalg.norm(current[:3] - self.target_pose[:3]) + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_zlabel('Z (m)') + ax.set_title(f'{title}\nError: {error*1000:.1f}mm', + fontsize=12, fontweight='bold') + ax.legend() + + # Set consistent limits + ax.set_xlim([0.3, 0.6]) + ax.set_ylim([0.2, 0.4]) + ax.set_zlim([0.3, 0.5]) + + plt.tight_layout() + plt.savefig(filename, dpi=150, bbox_inches='tight') + print(f"📸 Saved snapshots to {filename}") + plt.close() + + def create_animation_frames(self, output_dir='animation_frames'): + """Create individual frames for animation""" + import os + os.makedirs(output_dir, exist_ok=True) + + poses = np.array(self.pose_history) + n_poses = len(poses) + + # Create frames at regular intervals + frame_interval = max(1, n_poses // 30) # Target ~30 frames + + for frame_idx, pose_idx in enumerate(range(0, n_poses, frame_interval)): + fig = plt.figure(figsize=(12, 9)) + ax = fig.add_subplot(111, projection='3d') + + current = poses[pose_idx] + + # Plot trajectory up to current point + ax.plot(poses[:pose_idx+1, 0], poses[:pose_idx+1, 1], poses[:pose_idx+1, 2], + 'b-', linewidth=2, alpha=0.6, label='Trajectory') + + # Plot target with coordinate frame + ax.scatter(self.target_pose[0], self.target_pose[1], self.target_pose[2], + c='red', s=400, marker='*', label='Target', alpha=0.8) + + # Draw TARGET coordinate frame (larger and more visible) + scale = 0.08 # Reduced from 0.12 for less obtrusive arrows + colors = ['red', 'green', 'blue'] + labels = ['X', 'Y', 'Z'] + + # Target frame - use target orientation (0,0,0 in this case) + from scipy.spatial.transform import Rotation as R + target_rot = R.from_rotvec(self.target_pose[3:]) + target_rot_matrix = target_rot.as_matrix() + + for i, (color, label) in enumerate(zip(colors, labels)): + vec = np.zeros(3) + vec[i] = scale + # Apply rotation + vec_rotated = target_rot_matrix @ vec + arrow = Arrow3D([self.target_pose[0], self.target_pose[0]+vec_rotated[0]], + [self.target_pose[1], self.target_pose[1]+vec_rotated[1]], + [self.target_pose[2], self.target_pose[2]+vec_rotated[2]], + mutation_scale=20, lw=3, arrowstyle='->', color=color, alpha=0.7) + ax.add_artist(arrow) + + # Plot current position + ax.scatter(current[0], current[1], current[2], + c='blue', s=300, marker='o', label='Robot', alpha=0.9) + + # Draw CURRENT coordinate frame (larger and rotated) + current_rot = R.from_rotvec(current[3:]) + current_rot_matrix = current_rot.as_matrix() + + for i, (color, label) in enumerate(zip(colors, labels)): + vec = np.zeros(3) + vec[i] = scale + # Apply rotation + vec_rotated = current_rot_matrix @ vec + arrow = Arrow3D([current[0], current[0]+vec_rotated[0]], + [current[1], current[1]+vec_rotated[1]], + [current[2], current[2]+vec_rotated[2]], + mutation_scale=20, lw=3, arrowstyle='->', color=color, alpha=0.9) + ax.add_artist(arrow) + + # Draw line to target + ax.plot([current[0], self.target_pose[0]], + [current[1], self.target_pose[1]], + [current[2], self.target_pose[2]], + 'k--', alpha=0.5, linewidth=2) + + position_error = np.linalg.norm(current[:3] - self.target_pose[:3]) + rotation_error = np.linalg.norm(current[3:] - self.target_pose[3:]) + + ax.set_xlabel('X (m)', fontsize=11) + ax.set_ylabel('Y (m)', fontsize=11) + ax.set_zlabel('Z (m)', fontsize=11) + ax.set_title(f'Visual Servoing: Iteration {pose_idx}\n' + f'Position Error: {position_error*1000:.1f}mm | ' + f'Rotation Error: {rotation_error:.3f}rad', + fontsize=14, fontweight='bold') + ax.legend(loc='upper right', fontsize=10) + + # Set consistent limits + ax.set_xlim([0.3, 0.6]) + ax.set_ylim([0.2, 0.4]) + ax.set_zlim([0.3, 0.5]) + + # Adjust viewing angle for better visibility + ax.view_init(elev=20, azim=45) + + # Save frame + frame_path = os.path.join(output_dir, f'frame_{frame_idx:03d}.png') + plt.savefig(frame_path, dpi=100, bbox_inches='tight') + plt.close() + + print(f"🎬 Created {frame_idx+1} animation frames in {output_dir}/") + return frame_idx + 1 + +def main(): + """Run visual servoing simulation""" + print("=" * 60) + print("Visual Servoing Simulation with speedL Control") + print("Demonstrates PBVS with Cartesian velocity commands") + print("=" * 60) + print() + + # Create and run simulation + sim = VisualServoSimulation() + converged = sim.run_simulation(max_iters=50, tolerance=0.002) + + print() + print("=" * 60) + print("Generating visualizations...") + print("=" * 60) + + # Generate visualizations + sim.visualize_trajectory('visual_servo_trajectory.png') + sim.visualize_snapshots('visual_servo_snapshots.png') + + # Create animation frames + print() + print("=" * 60) + print("Creating animation...") + print("=" * 60) + n_frames = sim.create_animation_frames('animation_frames') + + # Create GIF from frames + try: + import imageio + import glob + + frames = [] + frame_files = sorted(glob.glob('animation_frames/frame_*.png')) + + print(f"📹 Compiling {len(frame_files)} frames into GIF...") + for frame_file in frame_files: + frames.append(imageio.imread(frame_file)) + + # Save as GIF with good quality + imageio.mimsave('visual_servo_animation.gif', frames, + duration=0.1, loop=0) + print("✅ Saved animation to visual_servo_animation.gif") + + # Clean up frame files + import shutil + shutil.rmtree('animation_frames') + print("🧹 Cleaned up temporary frames") + + except ImportError: + print("⚠️ imageio not available - install with: pip install imageio") + print(" Animation frames saved in animation_frames/ directory") + except Exception as e: + print(f"⚠️ Could not create GIF: {e}") + print(" Animation frames saved in animation_frames/ directory") + + print() + print("✅ Simulation complete!") + print() + print("Key Findings:") + print(f" • Converged: {converged}") + print(f" • Iterations: {len(sim.error_history)}") + print(f" • Final error: {sim.error_history[-1]*1000:.2f}mm") + print(f" • Initial error: {sim.error_history[0]*1000:.2f}mm") + print(f" • Control gain: {sim.control_gain}") + print(f" • Control rate: {1/sim.dt:.0f} Hz") + print() + print("This demonstrates how speedL() enables smooth") + print("convergence through Cartesian velocity control.") + +if __name__ == "__main__": + main()