3. Implementation
Hardware Setup
CAD Models
End Effector with USB-C Attachment
Baseplate with Bigger Holes & Tolerance USB-C
To Spec USB-C Baseplate
Mechanical Architecture
We opted for a Fixed-Tool, Eye-in-Hand configuration.
- Calibration Board: A 9x6 checkerboard pattern (23.25mm squares) provides the "World Frame" and solves scale ambiguity.
- The End Effector: A custom 3D-printed end effector that is held by the gripper of the UR7e. This also serves as a bracket tp rigidly mount a Logitech C922 webcam parallel to the tool flange as well as contains the USB-C shaped stub at the bottom to serve as a connector.
- The USB-C Port: A 3D-printed USB-C port on a baseplate.
Software A. Perception: The 'Calibration' Node
The core perception logic relies on a robust OpenCV pipeline encapsulated within the
CalibrationNode. We utilize cv2.findChessboardCorners to identify the
grid pattern in the raw video feed, followed immediately by cv2.cornerSubPix to
refine these detections to sub-pixel accuracy. This refinement step is crucial for high-precision
insertion tasks, as it minimizes quantization error before any geometric calculations occur.
Once the 2D pixel coordinates are locked, cv2.solvePnP matches them against the
known 3D geometry of the chessboard (obj_points).
This algorithm solves the Perspective-n-Point problem, outputting the rotation and translation
vectors that define exactly where the chessboard sits relative to the camera's optical center.
To make this spatial data actionable for the robot, we perform a rigorous coordinate transformation
sequence. The raw output from the vision pipeline is in the camera's optical frame, which is distinct
from the robot's coordinate system. The code converts the rotation vectors into quaternions using
scipy.spatial.transform and leverages the ROS 2 tf2 buffer to transform
the pose from the camera_link frame to the base_link frame.
This ensures that the detection is not just relative to the sensor, but rooted in the robot's fixed
base frame, allowing for consistent motion planning regardless of how the camera is mounted or moved.
Finally, the node functions as a managed state machine rather than a continuous stream, ensuring
system stability. It utilizes a ROS service (/calibrate_height) to trigger detection
only when the robot is static, preventing motion blur from corrupting the measurement. The integration
of the PinholeCameraModel validates the intrinsics (focal length and principal point),
allowing us to project rays from 2D pixels (u, v) into 3D space.
Visual debugging tools embedded in the code—drawing the principal point and board center—allow
operators to visually verify that the physical camera matches the mathematical model before
calibration data is published.
def process_frame(image):
# 1. Detection: Find grid pattern in 2D image
corners_2d = cv2.findChessboardCorners(image)
if found:
# 2. Refinement: Improve accuracy to sub-pixel level
refined_corners = cv2.cornerSubPix(corners_2d)
# 3. PnP Solver: Match 2D pixels to known 3D geometry
# Returns position/rotation of board relative to Camera
pose_in_camera = cv2.solvePnP(refined_corners, known_3d_points)
# 4. Transformation: Convert to Robot Base Frame
# Crucial for motion planning
pose_in_base = tf_buffer.transform(pose_in_camera,
from="camera_link",
to="base_link")
publish(pose_in_base)
Software B. Motion Planning: The 'Move' Node
This node acts as the system's "Brain," using a Finite State Machine to command the UR7e via MoveIt. It orchestrates the transition from visual alignment to tactile insertion.
1. The Control Loop (FSM)
- ALIGN: Uses closed-loop visual servoing to minimize error to <1.0mm.
- DIVE: Descends to a "pre-insert" height (Z=12cm) while locking X/Y alignment.
- SHIFT: Executes a "blind shift" using TF2 transforms to physically swap the camera position with the gripper tip.
- SPIRAL: Triggers the
/spiral_searchservice, handing off control to force feedback.
2. Cartesian Path Planning
We prioritize Cartesian Path Planning over joint interpolation. By enforcing strictly linear motion, we prevent the arc-induced "perspective drift" common in joint moves. This ensures the camera remains perpendicular to the target throughout the descent, validating our coordinate transforms.
3. The "Shift"
The SHIFT state handles the critical hand-eye coordination. The code calculates the
real-time inverse transform between camera_link and tool0, determining
exactly how to position the gripper where the camera lens just was. This maneuver relies entirely
on the accuracy of our calibration node, as the target becomes occluded by the robot itself.
state = "ALIGN"
while node_is_running:
if state == "ALIGN":
# Closed-loop Visual Servoing
error_vector = get_usb_c_position()
if magnitude(error_vector) < 1.0mm:
state = "DIVE"
else:
# Move linearly to correct error
move_cartesian(dx=error_vector.x, dy=error_vector.y)
elif state == "DIVE":
# Move straight down 12cm, maintaining X/Y lock
move_cartesian(dz=-0.12)
state = "SHIFT"
elif state == "SHIFT":
# The "Blind" Hand-Eye Swap
# Calculate physical offset between camera lens and gripper tip
offset = get_transform(from="camera_link", to="tool0")
move_cartesian(offset)
state = "SPIRAL"
elif state == "SPIRAL":
# Hand off control to force feedback system
trigger_service("/spiral_search")
stop_cartesian_control()
Software C. Object Detection: The 'CV' Node
While the Calibration Node handles static alignment, the CV node is responsible for
real-time target detection. Because the robot may approach the USB-C port from varying heights
(changing the apparent size of the port), standard template matching is insufficient.
To solve this, we implemented a Multi-Scale Template Matching pipeline. The algorithm
iteratively resizes the template across a defined range (0.1% to 1.0% of the image area) to ensure
scale invariance. We utilize Normalized Cross-Correlation (NCC)
(cv2.TM_CCOEFF_NORMED) as our matching metric, which makes the system robust to
global lighting changes—a critical feature for detecting metallic ports that reflect light unpredictably.
Figure C.1: Correlation Heatmap. Red areas indicate high probability matches (NCC > 0.8), while blue areas indicate low correlation.
Finally, to prevent false positives from cluttering the data, we apply Non-Maximum Suppression
(NMS).
This filters out overlapping bounding boxes, ensuring that only the single strongest detection candidate
is published to the /usbc_pose topic.
def find_usbc_port(image, template):
all_matches = []
# 1. Multi-Scale Search
# Iterate scale from 0.05% to 1.5% of image size
for scale in linspace(min_scale, max_scale, 20):
# Resize template to current scale
resized_template = cv2.resize(template, scale)
# 2. Normalized Cross-Correlation
# Robust against brightness differences
heatmap = cv2.matchTemplate(image, resized_template, TM_CCOEFF_NORMED)
# 3. Thresholding
locations = where(heatmap > 0.8)
all_matches.append(locations, score)
# 4. Non-Maximum Suppression (NMS)
# Remove overlapping boxes, keep only the highest score
best_match = non_max_suppression(all_matches, overlap_thresh=0.3)
return best_match.center_x, best_match.center_y
Software D. Force Control: The 'SpiralSearch' Node
Standard ROS topics (and our attempted use of a load cell) have too much latency (50-100ms) for force insertion. We solved this by injecting URScript directly into the controller to toggle force mode on the UR7e.
The Algorithm
- Apply 4N downward force (Force Mode).
- Move in a square spiral pattern (0.5mm step size).
- Monitor Z-position. If Z drops > 3mm, trigger SUCCESS.
# Snippet of our URScript Injection
def _perform_spiral_search(self):
ur_script = """
force_mode(p[0,0,0,0,0,0], [0,0,1,0,0,0], [0,0,-5.0,0,0,0], 2, ...)
while step_count < max_steps:
# Move in expanding spiral (0.5mm steps)
# Check Z-height
if drop > 0.003: # 3mm drop detected
textmsg("HOLE FOUND!")
break
"""
self.script_publisher.publish(ur_script)
We put all of our services, MoveIt, camera initialization into launch files for easier management and reproducibility.
