Point tracking pick loop

Use calibrated RGB-D data to turn a caller-owned red-rectangle detection into a robust 3D observation, resolve it into the planning frame, and apply a bounded in-plane Cartesian correction. The example treats a side-on can as an elongated red region and uses its short axis as the yaw channel in the same re-observation loop.

Servoing starts from a fixed home pose with the tool pointing straight down, and only ever corrects x, y, and yaw: height and the straight-down orientation are held fixed throughout the loop, so it never drifts out of plane. Once aligned, a separate pick step uses the last measured depth to descend, grasp, and retreat back to the aligned pose.

Python

import math

import cv2
import numpy as np

from olo import Client
from olo.core import Image
from olo.spatial import Point, Pose, Quaternion, Transform, Vector, wrap_error
from olo.vision import camera_info_to_intrinsics, deproject_region
# @olo-playground: autofill-sim-topics

# Magenta target detection
HSV_LOWER = np.array([133, 100, 80])
HSV_UPPER = np.array([163, 255, 255])
MIN_AREA_PX = 200
DEBUG_MASK_TOPIC = "point_tracking/debug"

# Positioning parameters
MAX_ITERATIONS = 10
POSITION_TOLERANCE_M = 0.005
YAW_TOLERANCE_RAD = math.radians(3)
MAX_TRANSLATION_STEP_M = 0.3
APPROACH_AXIS_EE = Vector(x=0.0, z=1.0)
APPROACH_DEPTH_OFFSET_EE = Vector(x=0.0, y=0.0, z=0.01)
HOME_POSE = Pose(
    position=Point(x=0.40, y=0.0, z=0.30),
    orientation=Quaternion.from_rpy(math.pi, 0.0, 0.0),
)


def cap_translation_step(delta: Vector, max_step_m: float) -> Vector:
    magnitude = delta.magnitude()
    if magnitude == 0.0 or magnitude <= max_step_m:
        return delta
    return delta * (max_step_m / magnitude)


def camera_error_to_servo_plane(camera_to_planning: Transform, camera_error: Vector) -> Vector:
    """Map camera-frame XY error to a planning-frame XY correction."""
    planning_to_camera = camera_to_planning.inverse()
    x_axis = planning_to_camera.rotate(Vector(x=1.0))
    y_axis = planning_to_camera.rotate(Vector(y=1.0))
    projection = np.array(
        [
            [x_axis.x, y_axis.x],
            [x_axis.y, y_axis.y],
        ],
        dtype=np.float64,
    )
    camera_xy = np.array([camera_error.x, camera_error.y], dtype=np.float64)
    delta_xy, *_ = np.linalg.lstsq(projection, camera_xy, rcond=None)
    return Vector(x=float(delta_xy[0]), y=float(delta_xy[1]), z=0.0)


def detect_target(image: Image) -> tuple[np.ndarray, float, Image] | None:
    """Detect a coloured object as ``(region, short_axis_yaw_rad, debug_image)``."""
    hsv = cv2.cvtColor(image.array, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, HSV_LOWER, HSV_UPPER)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((9, 9), np.uint8))
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8))

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None
    contour = max(contours, key=cv2.contourArea)
    if cv2.contourArea(contour) < MIN_AREA_PX:
        return None

    (_, _), (width, height), _ = rect = cv2.minAreaRect(contour)
    box = cv2.boxPoints(rect)
    edges = [box[(i + 1) % 4] - box[i] for i in range(4)]
    short_edge = min(edges, key=lambda edge: float(np.hypot(edge[0], edge[1])))
    yaw_rad = float(np.arctan2(short_edge[1], short_edge[0]))

    region = np.zeros_like(mask, dtype=bool)
    cv2.drawContours(region, [contour], -1, 1, -1)
    debug_image = Image(
        array=cv2.cvtColor(region.astype(np.uint8) * 255, cv2.COLOR_GRAY2BGR),
        encoding="bgr8",
        header=image.header,
    )
    return region, yaw_rad, debug_image


with Client() as client:
    # Resolve the robot interfaces required by this example.
    robot = client.robot()
    core = robot.core
    frames = robot.frames
    arm = robot.kinematics
    gripper = arm.ee
    if gripper is None:
        raise RuntimeError("This example requires a gripper end effector")

    # Configure topics
    rgb_topic = core.topic("wrist_rgbd/image", "sensor_msgs/msg/Image")
    depth_topic = core.topic("wrist_rgbd/depth_image", "sensor_msgs/msg/Image")
    camera_info_topic = core.topic("wrist_rgbd/camera_info", "sensor_msgs/msg/CameraInfo")
    debug_mask_topic = core.topic(DEBUG_MASK_TOPIC, "sensor_msgs/msg/Image")

    # Move to the initial pose and cache fixed camera calibration.
    arm.move_pose(HOME_POSE)
    gripper.open()
    intrinsics = camera_info_to_intrinsics(core.get_latest(camera_info_topic))
    camera_to_ee = frames.lookup(arm.end_effector_link, intrinsics.frame_id)

    # Servo loop.
    for _ in range(MAX_ITERATIONS):
        # Detect the target
        image = core.get_image(rgb_topic)
        depth = core.get_image(depth_topic)
        detection = detect_target(image)
        if detection is None:
            raise RuntimeError("Target not detected; robot was not moved")
        region, yaw_rad, debug_image = detection
        core.publish(debug_mask_topic, debug_image, timeout=1.0)

        # Deproject the target region into the planning frame.
        camera_observation = deproject_region(depth, region, intrinsics, aggregation="median", min_valid_pixels=20)
        camera_to_planning = frames.lookup(arm.planning_frame, camera_observation.frame_id, at=camera_observation.stamp)
        ee_to_planning = frames.lookup(arm.planning_frame, arm.end_effector_link, at=camera_observation.stamp)
        target_in_ee = camera_to_ee.apply(camera_observation.point)
        camera_error = Vector(x=camera_observation.point.x, y=camera_observation.point.y, z=0.0)
        error = camera_error_to_servo_plane(camera_to_planning, camera_error)

        # Stop once both alignment errors are within tolerance.
        horizontal_error_m = math.hypot(camera_error.x, camera_error.y)
        yaw_error = wrap_error(yaw_rad, math.pi)
        if horizontal_error_m <= POSITION_TOLERANCE_M and abs(yaw_error) <= YAW_TOLERANCE_RAD:
            print("Target position and yaw are aligned")
            break

        # Apply the bounded planar correction and rotate about tool +Z.
        delta = cap_translation_step(error, MAX_TRANSLATION_STEP_M)
        print(
            f"Servo: pixel=({camera_observation.pixel.x:.0f}, {camera_observation.pixel.y:.0f}), "
            f"xy_error={horizontal_error_m:.4f} m, yaw_error={math.degrees(yaw_error):.1f} deg, "
            f"delta=({delta.x:.4f}, {delta.y:.4f}, {delta.z:.4f}) m, "
            f"yaw_step={math.degrees(yaw_error):.1f} deg"
        )
        current_pose = ee_to_planning.as_pose()
        yaw_delta = Quaternion.from_axis_angle(APPROACH_AXIS_EE.array(), yaw_error)
        target_pose = Pose(
            position=Point(
                x=current_pose.position.x + delta.x,
                y=current_pose.position.y + delta.y,
                z=HOME_POSE.position.z,
            ),
            orientation=current_pose.orientation * yaw_delta,
        )
        arm.move_pose(target_pose, avoid_obstacles=True)
    else:
        raise RuntimeError(f"Did not converge within {MAX_ITERATIONS} iterations")

    # Descend along tool +Z by the target's hand-eye-resolved axial distance.
    aligned_pose = ee_to_planning.as_pose()
    approach_axis_planning = aligned_pose.rotate(APPROACH_AXIS_EE)
    approach_distance_m = Vector.from_point(target_in_ee + APPROACH_DEPTH_OFFSET_EE).dot(APPROACH_AXIS_EE)
    if approach_distance_m <= 0:
        raise RuntimeError(
            "Observed target is not in front of the end-effector approach axis: "
            f"target_in_ee={target_in_ee!r}, approach_distance={approach_distance_m:.4f} m"
        )

    # Keep the aligned orientation while moving the TCP to the target.
    pick_pose = Pose(
        position=aligned_pose.position + approach_axis_planning * approach_distance_m,
        orientation=aligned_pose.orientation,
    )
    print(
        "Pick descent: "
        f"target_in_ee=({target_in_ee.x:.4f}, {target_in_ee.y:.4f}, {target_in_ee.z:.4f}) m, "
        f"approach_distance={approach_distance_m:.4f} m"
    )

    # Grasp, retreat to the aligned pose, and release.
    arm.move_linear(pick_pose)
    gripper.close()
    arm.move_linear(aligned_pose)
    gripper.open()

TypeScript

import { connect } from "olo/web";
import type { Image } from "olo/core";
import { Point, Pose, Quaternion, Vector, wrapError, type Transform } from "olo/spatial";
import { cameraInfoToIntrinsics, deprojectRegion } from "olo/vision";
// @olo-playground: autofill-sim-topics

// Magenta target detection
const HSV_LOWER = { hueDeg: 266, saturation: 100 / 255, value: 80 / 255 };
const HSV_UPPER = { hueDeg: 326, saturation: 1, value: 1 };
const MIN_AREA_PX = 200;
const DEBUG_MASK_TOPIC = "point_tracking/debug";

// Positioning parameters
const MAX_ITERATIONS = 10;
const POSITION_TOLERANCE_M = 0.005;
const YAW_TOLERANCE_RAD = (3 * Math.PI) / 180;
const MAX_TRANSLATION_STEP_M = 0.3;
const APPROACH_AXIS_EE = new Vector(0, 0, 1);
const APPROACH_DEPTH_OFFSET_EE = new Vector(0, 0, 0.01);
const HOME_POSE = new Pose(new Point(0.4, 0, 0.3), Quaternion.fromRpy(Math.PI, 0, 0));

function capTranslationStep(delta: Vector, maxStepM: number): Vector {
  const magnitude = delta.magnitude();
  if (magnitude === 0 || magnitude <= maxStepM) {
    return delta;
  }
  return delta.scale(maxStepM / magnitude);
}

function cameraErrorToServoPlane(cameraToPlanning: Transform, cameraError: Vector): Vector {
  // Map camera-frame XY error to a planning-frame XY correction.
  const planningToCamera = cameraToPlanning.inverse();
  const xAxis = planningToCamera.rotate(new Vector(1));
  const yAxis = planningToCamera.rotate(new Vector(0, 1));
  const determinant = xAxis.x * yAxis.y - yAxis.x * xAxis.y;
  if (Math.abs(determinant) < 1e-9) {
    throw new Error("Camera/planning frame projection is singular");
  }
  return new Vector(
    (cameraError.x * yAxis.y - yAxis.x * cameraError.y) / determinant,
    (xAxis.x * cameraError.y - cameraError.x * xAxis.y) / determinant,
    0,
  );
}

function rgbToHsv(r: number, g: number, b: number): { hueDeg: number; saturation: number; value: number } {
  const rn = r / 255;
  const gn = g / 255;
  const bn = b / 255;
  const max = Math.max(rn, gn, bn);
  const min = Math.min(rn, gn, bn);
  const delta = max - min;

  let hueDeg = 0;
  if (delta !== 0) {
    if (max === rn) {
      hueDeg = 60 * (((gn - bn) / delta) % 6);
    } else if (max === gn) {
      hueDeg = 60 * ((bn - rn) / delta + 2);
    } else {
      hueDeg = 60 * ((rn - gn) / delta + 4);
    }
  }
  if (hueDeg < 0) {
    hueDeg += 360;
  }

  return {
    hueDeg,
    saturation: max === 0 ? 0 : delta / max,
    value: max,
  };
}

function magentaMask(image: Image): Uint8Array {
  if (image.encoding !== "bgr8" && image.encoding !== "rgb8") {
    throw new Error(`Magenta rectangle example expects bgr8/rgb8 images, got ${image.encoding}`);
  }
  const mask = new Uint8Array(image.width * image.height);
  const rIndex = image.encoding === "rgb8" ? 0 : 2;
  const gIndex = 1;
  const bIndex = image.encoding === "rgb8" ? 2 : 0;
  const step = image.step || image.width * 3;
  for (let y = 0; y < image.height; y += 1) {
    for (let x = 0; x < image.width; x += 1) {
      const offset = y * step + x * 3;
      const r = image.data[offset + rIndex] ?? 0;
      const g = image.data[offset + gIndex] ?? 0;
      const b = image.data[offset + bIndex] ?? 0;
      const hsv = rgbToHsv(r, g, b);
      if (
        hsv.hueDeg >= HSV_LOWER.hueDeg &&
        hsv.hueDeg <= HSV_UPPER.hueDeg &&
        hsv.saturation >= HSV_LOWER.saturation &&
        hsv.saturation <= HSV_UPPER.saturation &&
        hsv.value >= HSV_LOWER.value &&
        hsv.value <= HSV_UPPER.value
      ) {
        mask[y * image.width + x] = 1;
      }
    }
  }
  return mask;
}

function maskDebugImage(mask: Uint8Array, source: Image): Image {
  const data = new Uint8Array(mask.length * 3);
  mask.forEach((value, index) => {
    const offset = index * 3;
    const intensity = value ? 255 : 0;
    data[offset] = intensity;
    data[offset + 1] = intensity;
    data[offset + 2] = intensity;
  });
  return {
    data,
    encoding: "bgr8",
    format: "",
    width: source.width,
    height: source.height,
    step: source.width * 3,
    isBigEndian: false,
    header: source.header,
  };
}

function magentaPixelStats(mask: Uint8Array, width: number): { yawRad: number } | undefined {
  const points: Array<[number, number]> = [];
  mask.forEach((value, index) => {
    if (value) {
      points.push([index % width, Math.floor(index / width)]);
    }
  });
  if (points.length < MIN_AREA_PX) {
    return undefined;
  }

  const meanX = points.reduce((sum, [x]) => sum + x, 0) / points.length;
  const meanY = points.reduce((sum, [, y]) => sum + y, 0) / points.length;
  let cxx = 0;
  let cyy = 0;
  let cxy = 0;
  for (const [x, y] of points) {
    const dx = x - meanX;
    const dy = y - meanY;
    cxx += dx * dx;
    cyy += dy * dy;
    cxy += dx * dy;
  }
  cxx /= points.length;
  cyy /= points.length;
  cxy /= points.length;
  const majorYaw = 0.5 * Math.atan2(2 * cxy, cxx - cyy);
  return { yawRad: majorYaw + Math.PI / 2 };
}

function detectTarget(image: Image): { region: Uint8Array; yawRad: number; debugImage: Image } | undefined {
  // Detect a coloured object as (region, short-axis yaw, debug image).
  const region = magentaMask(image);
  const stats = magentaPixelStats(region, image.width);
  if (stats === undefined) {
    return undefined;
  }
  return { region, yawRad: stats.yawRad, debugImage: maskDebugImage(region, image) };
}

const client = connect();
// Resolve the robot interfaces required by this example.
const robot = await client.robot();
const core = robot.core;
const frames = robot.frames;
const arm = robot.kinematics;
const gripper = await arm.getEndEffector();
if (gripper === undefined) {
  throw new Error("This example requires a gripper end effector");
}

// Configure topics
const rgbTopic = core.topic("wrist_rgbd/image", "sensor_msgs/msg/Image");
const depthTopic = core.topic("wrist_rgbd/depth_image", "sensor_msgs/msg/Image");
const cameraInfoTopic = core.topic("wrist_rgbd/camera_info", "sensor_msgs/msg/CameraInfo");
const debugMaskTopic = core.topic(DEBUG_MASK_TOPIC, "sensor_msgs/msg/Image");

// Move to the initial pose and cache fixed camera calibration.
await arm.movePose(HOME_POSE);
await gripper.open();
const intrinsics = cameraInfoToIntrinsics(await core.getLatest(cameraInfoTopic));
const endEffectorLink = await arm.getEndEffectorLink();
const planningFrame = await arm.getPlanningFrame();
const cameraToEe = await frames.lookup(endEffectorLink, intrinsics.frameId);

// Servo loop.
let targetInEe: Point | undefined;
let eeToPlanning: Transform | undefined;
let aligned = false;
for (let i = 0; i < MAX_ITERATIONS; i += 1) {
  // Detect the target
  const image = await core.getImage(rgbTopic);
  const depth = await core.getImage(depthTopic);
  const detection = detectTarget(image);
  if (detection === undefined) {
    throw new Error("Target not detected; robot was not moved");
  }
  await core.publish(debugMaskTopic, detection.debugImage, { timeoutMs: 1000 });

  // Deproject the target region into the planning frame.
  const cameraObservation = deprojectRegion(depth, detection.region, intrinsics, {
    aggregation: "median",
    minValidPixels: 20,
  });
  const lookupOptions = cameraObservation.stamp !== undefined ? { at: cameraObservation.stamp } : {};
  const cameraToPlanning = await frames.lookup(planningFrame, cameraObservation.frameId, lookupOptions);
  eeToPlanning = await frames.lookup(planningFrame, endEffectorLink, lookupOptions);
  targetInEe = cameraToEe.apply(cameraObservation.point);
  const cameraError = new Vector(cameraObservation.point.x, cameraObservation.point.y, 0);
  const error = cameraErrorToServoPlane(cameraToPlanning, cameraError);

  // Stop once both alignment errors are within tolerance.
  const horizontalErrorM = Math.hypot(cameraError.x, cameraError.y);
  const yawError = wrapError(detection.yawRad, Math.PI);
  if (horizontalErrorM <= POSITION_TOLERANCE_M && Math.abs(yawError) <= YAW_TOLERANCE_RAD) {
    console.log("Target position and yaw are aligned");
    aligned = true;
    break;
  }

  // Apply the bounded planar correction and rotate about tool +Z.
  const delta = capTranslationStep(error, MAX_TRANSLATION_STEP_M);
  console.log(
    "Servo: " +
      `pixel=(${cameraObservation.pixel.x.toFixed(0)}, ${cameraObservation.pixel.y.toFixed(0)}), ` +
      `xy_error=${horizontalErrorM.toFixed(4)} m, ` +
      `yaw_error=${((yawError * 180) / Math.PI).toFixed(1)} deg, ` +
      `delta=(${delta.x.toFixed(4)}, ${delta.y.toFixed(4)}, ${delta.z.toFixed(4)}) m, ` +
      `yaw_step=${((yawError * 180) / Math.PI).toFixed(1)} deg`,
  );
  const currentPose = eeToPlanning.asPose();
  const yawDelta = Quaternion.fromAxisAngle(APPROACH_AXIS_EE.array(), yawError);
  const targetPose = new Pose(
    new Point(
      currentPose.position.x + delta.x,
      currentPose.position.y + delta.y,
      HOME_POSE.position.z,
    ),
    currentPose.orientation.multiply(yawDelta),
  );
  await arm.movePose(targetPose, { avoidObstacles: true });
}

if (!aligned) {
  throw new Error(`Did not converge within ${MAX_ITERATIONS} iterations`);
}

if (eeToPlanning === undefined || targetInEe === undefined) {
  throw new Error("Target alignment state was not recorded");
}

// Descend along tool +Z by the target's hand-eye-resolved axial distance.
const alignedPose = eeToPlanning.asPose();
const approachAxisPlanning = alignedPose.rotate(APPROACH_AXIS_EE);
const approachDistanceM = Vector.fromPoint(targetInEe.add(APPROACH_DEPTH_OFFSET_EE)).dot(
  APPROACH_AXIS_EE,
);
if (approachDistanceM <= 0) {
  throw new Error(
    "Observed target is not in front of the end-effector approach axis: " +
      `targetInEe=${targetInEe.toString()}, approachDistance=${approachDistanceM.toFixed(4)} m`,
  );
}

// Keep the aligned orientation while moving the TCP to the target.
const pickPose = new Pose(
  alignedPose.position.add(approachAxisPlanning.scale(approachDistanceM)),
  alignedPose.orientation,
);
console.log(
  "Pick descent: " +
    `target_in_ee=(${targetInEe.x.toFixed(4)}, ${targetInEe.y.toFixed(4)}, ${targetInEe.z.toFixed(4)}) m, ` +
    `approach_distance=${approachDistanceM.toFixed(4)} m`,
);

// Grasp, retreat to the aligned pose, and release.
await arm.moveLinear(pickPose);
await gripper.close();
await arm.moveLinear(alignedPose);
await gripper.open();