Add comprehensive AprilTag functionality review, analysis, dual-perspective visual servoing simulations, and 6-DOF moveL-based visual servoing with direct-move-then-refine strategy

[Copilot]

• Explore ur_toolkit repository structure for AprilTag functionality
• Analyze AprilTag detection module (apriltag_detection.py)
• Review visual servoing implementation
• Examine workflow integration patterns
• Research ac-dev-lab related issues and resources
• Create comprehensive AprilTag functionality review document
• Identify positive patterns and implementation strengths
• Document missing functionality and improvement areas
• Provide recommendations for visual servoing enhancements
• Analyze PBVS theory and Jacobian-based control requirements
• Explain UR robot velocity control interfaces (speedL, speedJ)
• Create visual servoing simulation demonstrating speedL implementation
• Generate animated GIF showing convergence process
• Enhance visualization to show orientation changes more clearly
• Add camera perspective simulation showing AprilTag detection convergence
• Improve camera perspective with realistic AprilTag rendering and target reference
• Triple iterations for extended convergence demonstration
• Integrate pupil-apriltags library for authentic AprilTag detection
• Fix detection issues by using actual AprilTag images
• Add XYZ position and roll-pitch-yaw angle display from AprilTag detection
• Regenerate animation with complete 6-DOF pose visualization
• Optimize control parameters for complete convergence demonstration
• Add 6-DOF moveL-based visual servoing simulator with direct error correction approach
• Implement optimal direct-move-then-refine strategy for real-world visual servoing
• Add realistic noise model and adjust momentum for improved first-move behavior
• Add minimal working example with gradient approximation

Minimal Working Example with Gradient Approximation

Updated iterative_movel_minimal.py with full gradient approximation via jogging:

Algorithm:

1. First moveL: Go directly to target (gain=1.0) to get in the ballpark
2. Subsequent moveL: Jog in ±xyz and ±roll-pitch-yaw directions to estimate gradient, then correct

Key Functions:

• estimate_gradient() - Perturbs each of 6 DOF (±x, ±y, ±z, ±roll, ±pitch, ±yaw) and measures error change to approximate gradient
• iterative_movel_servo_with_gradient() - Main algorithm with gradient-based correction

Usage:

python iterative_movel_minimal.py

Uses scipy for rotation utilities.

Fixes #4

Fixes #4

---

✨ Let Copilot coding agent set things up for you — coding agent works faster and does higher quality work when set up for your repo.

[Copilot]

Pull Request Overview

This PR adds a comprehensive technical review of the AprilTag functionality within the UR Toolkit repository. The review analyzes the current implementation's architecture, validates design decisions (particularly the exclusion of hand-eye calibration), identifies enhancement opportunities, and provides strategic recommendations for future development.

Key changes:

• Documents current AprilTag detection, visual servoing, and workflow integration components
• Validates the architectural decision to exclude hand-eye calibration as sound engineering judgment
• Identifies missing functionality and provides enhancement recommendations while respecting the project's simplicity-first philosophy

[sgbaird]

@copilot, following up on #3 (comment), help me understand the current visual servoing implementation. I'm unsure if the implementation is on main or not, but it seems more likely that it's on apriltag-implementation branch

EDIT: cc @kelvinchow23 (lmk if I'm missing the most recent code you're working on)

[Copilot]

@copilot, following up on #3 (comment), help me understand the current visual servoing implementation. I'm unsure if the implem...

The visual servoing implementation is present on both branches:

Current status (as of 230a747):

• Main branch: Contains full visual servoing implementation (src/ur_toolkit/visual_servo/)
• apriltag-implementation branch: Also contains the same visual servoing code (last updated 9d4a1e5)

The visual servoing system includes:

• visual_servo_engine.py - Eye-in-hand IBVS implementation with PID control
• detection_filter.py - Detection quality filtering
• pose_history.py - Pose tracking and history management
• config.py - Visual servo configuration management

Both branches have identical visual servo functionality. The implementation includes a complete Eye-in-Hand Visual Servoing system with PID controllers, AprilTag integration, and workflow integration as documented in my review.

[sgbaird]

@copilot what might be the issue?

currently trying to get visual servoing (or some form of correction robot pose basde on apriltag pose estimation information). There's a position error (rotation + translation) that it spits out, but it cant seem to converge to the corrected position.

[Copilot]

@copilot what might be the issue?

currently trying to get visual servoing (or some form of correction robot pose basde on apriltag pose estimation information). There's a position error (rotation...

Several factors could cause visual servoing convergence issues. Based on the implementation analysis:

Primary Potential Issues:

1. Control Loop Parameters

The PID gains are very conservative (lines 109-118):

# Translation: kp=0.3, ki=0.0, kd=0.0, output_limit=0.015m
# Rotation: kp=0.2, ki=0.0, kd=0.0, output_limit=0.05rad

Problem: Low gains + no integral term = slow convergence, may never reach tight tolerances

2. Tolerances vs. Control Authority

• Position tolerance: 2mm (line 36)
• Max translation correction: 15mm/iteration (line 109)
• But with 70% damping factor (line 61), effective correction = ~10mm/iteration

Problem: Small corrections may not overcome system noise/deadband

3. Error Calculation Method

Simple direct correction (lines 297-298):

robot_translation_correction = -tag_translation_error
robot_rotation_correction = -tag_rotation_error  

Problem: Assumes perfect camera-robot frame alignment; doesn't account for:

• Camera mounting errors
• Robot kinematic errors
• Coordinate frame transformations

4. Detection Inconsistency Checks Disabled

Lines 262-278 show detection consistency checks are commented out for testing.
Problem: May be applying corrections based on noisy measurements

Troubleshooting Steps:

1. Check detection quality: Review Hamming distance and decision margin from AprilTag detections
2. Increase damping: Try damping_factor: 0.3-0.5 instead of 0.7
3. Add integral term: Set ki=0.05 for translation controllers to eliminate steady-state error
4. Relax tolerances: Try position_tolerance: 0.005 (5mm) initially
5. Enable debug logging: Add error magnitude tracking between iterations

The system appears designed for stability over performance. For better convergence, consider increasing control gains gradually while monitoring for oscillation.

[sgbaird]

@kelvinchow23 could you include a video of running one of the workflows? (and also include a permalink to the code that you're running)

[sgbaird]

@copilot fetch https://chatgpt.com/share/68ddddf5-d048-8007-bc6d-2a3fbc9794d5. In light of this, do your recommendations still apply?

[Copilot]

@copilot fetch https://chatgpt.com/share/68ddddf5-d048-8007-bc6d-2a3fbc9794d5. In light of this, do your recommendations still apply?

I'm unable to access the ChatGPT conversation link (blocked by client). Could you share the key insights or findings from that conversation? This will help me determine if my previous recommendations about the visual servoing convergence issues need to be revised.

[sgbaird]

@copilot I added copy-paste of transcript as a txt file

[Copilot]

@copilot I added copy-paste of transcript as a txt file

After reviewing the transcript, my recommendations remain valid with additional context:

Key Insights from Transcript:

The conversation confirms this is Position-Based Visual Servoing (PBVS) which:

• Uses 6-DoF pose error in SE(3) space
• Maps error to joint velocities via robot Jacobian
• Should run at 20-60 Hz (camera rate)
• Is "gradient descent with a map" - structured, not blind search

How This Relates to Convergence Issues:

1. Missing Jacobian Usage

The current implementation (lines 297-298) does direct pose correction:

robot_correction = -tag_error

Problem: This assumes identity Jacobian mapping, which is rarely correct. True PBVS needs:

joint_velocity = J_pseudoinverse @ (-lambda * pose_error)

2. Control Rate Mismatch

Transcript recommends 20-60 Hz control loop. Current implementation likely runs much slower (depends on camera capture + detection filtering).

3. No Velocity-Based Control

Current system sends position commands, not velocity commands. PBVS theory assumes continuous velocity control at camera rate.

Updated Recommendations:

For proper PBVS convergence:

1. Implement Jacobian-based control (requires robot kinematics)
2. Increase control loop frequency to match camera rate
3. Switch to velocity commands instead of position steps
4. Use smaller gain (λ=0.1-0.5) with proper Jacobian

For current simplified approach:

• My original recommendations still apply (tune gains, add integral term, relax tolerances)
• Consider this is approximating PBVS without full kinematic model
• May need hand-eye calibration for better frame alignment

[sgbaird]

@copilot wait, so what was the noise set to before?

[sgbaird]

Let's merge soon and start a new PR

[sgbaird]

@copilot What was the noise set to before?

[Copilot]

@copilot What was the noise set to before?

The simulator has no explicit noise model. Looking at the code (lines 30-33), the moveL function simply sets self.tcp_pose = np.array(target_pose) - it executes commands perfectly without any noise.

The "0.154m error after the first move" in the animation occurs because of momentum damping (line 121): velocity = momentum * velocity + (1 - momentum) * correction. With momentum=0.5, the first move only applies 50% of the intended correction (since initial velocity is zero), not because of noise.

So there's no noise - just momentum smoothing that effectively reduces the first move to 50% of the commanded correction.

@copilot What was the noise set to before?

The simulator has no explicit noise added. The ~50% first-move error shown in the animation comes from momentum damping (0.5 weight), not noise. With momentum=0.5, the velocity accumulation causes the first move to effectively apply only about 50% of the intended correction, even though gain=1.0. This simulates realistic robot dynamics where commanded motions don't execute instantaneously at full magnitude.

[sgbaird]

@copilot ok, add a small amount of uniformly distributed noise up to 0.05% in xyz and roll-pitch-yaw. Let's adjust so that the momentum is 1 for the first move, but can be less for later. We just want to get it in the right ballpark first.

Btw, again. we're just treating these as iterative moveL's to try to converge really accurately at the desired xyz roll-pitch-yaw relative to an AprilTag. The 2nd and on moveL's can be more gradually converging

[Copilot]

@copilot ok, add a small amount of uniformly distributed noise up to 0.05% in xyz and roll-pitch-yaw. Let's adjust so that the momentum is 1 for the first move, but can be less for later. We just want...

Implemented in commit 8a82866:

1. Added realistic noise model: Uniformly distributed noise up to 0.05% applied to all 6 DOF (xyz and roll-pitch-yaw), proportional to commanded change
2. Adjusted momentum strategy:
   • First move: momentum = 1.0 (no smoothing) - goes directly to commanded position
   • Subsequent moves: momentum = 0.5 (gradual convergence with smoothing)

The simulator now converges in ~18 iterations from 0.308m to 0.0005m (sub-millimeter). The first moveL gets in the right ballpark immediately, then iterative moveL commands gradually converge to precise accuracy.

[sgbaird]

@kelvinchow23 I had forgotten about AccelerationConsortium/ac-dev-lab#229 (comment), in particular visp

[sgbaird]

@copilot I need a minimal working example, a single, standalone script with just the logic and algorithms for doing these iterative moves.

[Copilot]

@copilot I need a minimal working example, a single, standalone script with just the logic and algorithms for doing these iterative moves.

Created iterative_movel_minimal.py in commit 7420aa6.

Single standalone script with just the core algorithm:

# First moveL: Go directly to target (gain=1.0, no smoothing)
# Subsequent moveL: Gradual convergence (gain=0.2, momentum smoothing)

iterative_movel_servo(
    current_pose=[0.3, 0.2, 0.5, 0.1, -0.2, 0.15],
    target_pose=[0.4, 0.3, 0.55, 0.0, 0.0, 0.0],
    first_gain=1.0,
    subsequent_gain=0.2,
    momentum=0.5,
    noise_percent=0.0005,
)

Run with: python iterative_movel_minimal.py

[sgbaird]

@copilot scipy is OK as a dependency btw. Also, you missed the whole part around jogging in different dimensions and perturbing orientation to get an approximate gradient to use to head in the correct direction

[Copilot]

@copilot scipy is OK as a dependency btw. Also, you missed the whole part around jogging in different dimensions and perturbing orientation to get an approximate gradient to use to head in the correct...

Added gradient approximation via jogging in ±xyz and ±roll-pitch-yaw directions in commit 40677ef.

The estimate_gradient() function perturbs each of the 6 DOF (±x, ±y, ±z, ±roll, ±pitch, ±yaw) and measures error change to approximate the gradient direction. The main algorithm now:

1. First moveL: Go directly to target (gain=1.0) to get in ballpark
2. Subsequent moveL: Jog in 12 directions (±6 DOF) to estimate gradient, then correct in optimal direction

Uses scipy.spatial.transform.Rotation for rotation utilities.