visual-servoing
Installation
SKILL.md
Eye-in-Hand Visual Servoing (Ibvs)
Use bundled Ibvs (opencv-python numpy, CPU only, no GPU or torch) with calibrated camera intrinsics (fx_px, fy_px, cx_px, cy_px) and hand-eye extrinsics T_ee_cam:
- Choose the Visual Frontend by Target Texture:
- Smooth / Low-Texture Objects: Segment the target foreground mask (passing an optional
roi_maskto exclude any robot/gripper geoms visible in the FOV) and extract pose features viaIbvs.ray_plane_contour_poseorIbvs.pca_contour_target(..., min_aspect=1.0, roi_mask=roi_mask, prev_theta_rad=...). Usedet["anisotropy"](eigval_max / eigval_min) to decide whether wrist yaw alignment is required vs. keeping neutral wrist roll on symmetric objects. - Textured Objects (CPU
match_keypoints): Callm = match_keypoints(img_cur, img_goal, detector="sift", mask_cur=target_mask)("orb"is faster; pass masks to exclude gripper fingers). It keeps Lowe-ratio matches with RANSAC homography inliers and returnsm["u_px"], m["v_px"]->m["u_goal_px"], m["v_goal_px"]forcompute_step, orNonewhen fewer thanmin_inlierssurvive (re-detect the target instead of servoing on bad matches).
- Smooth / Low-Texture Objects: Segment the target foreground mask (passing an optional
- Analytical Hover-Plane Alignment (Known Support Plane
z_plane_m, 2 Steps to< 1 mm):- Compute the optimal unoccluded hover clearance analytically from
T_ee_camviaIbvs.principal_ray_hover_distance(T_ee_cam, approach_axis_ee)["hover_distance_m"](clamped to safe workspace clearanceh_hover). - At hover, compute
T_world_cam = T_world_ee(q_actual) @ T_ee_camfrom measured joint states and calltarget = Ibvs.ray_plane_contour_pose(mask, fx_px, fy_px, cx_px, cy_px, T_world_cam, z_plane_m, roi_mask=roi_mask, prev_yaw_rad=...)(which back-projects the contour ontoz_plane_mfirst with arc-length weighting so tilted-camera perspective never biases the 3D midpoint). - Command the robot's IK solver to
(target["x_m"], target["y_m"])(aligning jaw yaw totarget["yaw_rad"]whentarget["anisotropy"] >= 2.0) until metric XY error||(x_ee, y_ee) - (target["x_m"], target["y_m"])|| < pos_tol_m(or pixel error againstservo.project_ee_point(h_actual * approach_axis_ee, T_ee_cam)converges).
- Compute the optimal unoccluded hover clearance analytically from
- Full 6-DOF
IBVSVelocity Step (Unknown Plane or Non-Planar Approach):- Pair each frame timestamp
t_imgwithq_actual(t_img), instantiateservo = Ibvs(fx_px, fy_px, cx_px, cy_px)with depthz_depth_m(from aligned RGB-D or two-view triangulation), callstep = servo.compute_step(u_px, v_px, z_depth_m, u_goal_px, v_goal_px, T_ee_cam), and integrateT_target = T_world_ee @ Ibvs.se3_exp(step["v_ee"], dt_s)into differential IK untilstep["converged"].
- Pair each frame timestamp
- Loop Protocol:
- Fresh frame per step: Discard buffered frames before each capture, recompute
T_world_camfromq_actualat capture time, and wait for each commanded step to finish before the next capture. - Iteration cap: Stop after
~10hover-plane corrections or~200IBVS velocity steps without convergence; re-localize and re-plan instead of continuing. - Target lost: If detection returns
Noneor the contour touches the image border, never extrapolate the last target. Retract3–5 cmalong-approach_axis_eeto widen the view and re-detect; if still lost, return to the look pose and re-localize with an object-localization skill. - Camera occlusion near the goal: If the gripper, tool, or held object will block the camera before the goal, stop servoing at the last pose where the target is fully visible and finish with a short, task-appropriate open-loop move. If the camera stays clear, servo all the way to the goal.
- Fresh frame per step: Discard buffered frames before each capture, recompute