Pick the fiducial cube

The office world’s 50 mm cube carries six DICT_4X4_50 markers, IDs 0–5. Each marker’s 36 mm outer black edge is 25 mm from the cube centre. This example detects the visible faces with a calibrated wrist camera, prefers the top face when available, and aligns the tool with the detected tag using the nearest of four equivalent cube-face yaws. It approaches along the tag’s -z axis to grasp the cube. The nearby ChArUco board uses a different marker dictionary, so its IDs cannot be confused with the cube’s. Short pauses after arm and gripper actions let the camera and grasp settle before the next step.

Python

import math
import time

from olo import Client
from olo.spatial import Point, Pose, Quaternion
from olo.vision import camera_info_to_intrinsics

RGB_TOPIC = "wrist_rgbd/image"
CAMERA_INFO_TOPIC = "wrist_rgbd/camera_info"
TAG_FAMILY = "aruco_4x4_50"
MARKER_SIZE_M = 0.036
CUBE_HALF_SIZE_M = 0.025
ALIGN_STANDOFF_M = 0.05
GRASP_CLEARANCE_M = 0.01
MAX_ITERATIONS = 10
POSITION_TOLERANCE_M = 0.005
ORIENTATION_TOLERANCE_RAD = math.radians(5)
MAX_TRANSLATION_STEP_M = 0.1
SETTLE_S = 0.5


with Client() as client:
    robot = client.robot()
    arm = robot.kinematics
    gripper = arm.ee
    if gripper is None:
        raise RuntimeError("This example requires a gripper end effector")

    rgb_topic = robot.core.topic(RGB_TOPIC, "sensor_msgs/msg/Image")
    info_topic = robot.core.topic(CAMERA_INFO_TOPIC, "sensor_msgs/msg/CameraInfo")
    intrinsics = camera_info_to_intrinsics(robot.core.get_latest(info_topic))
    detector = robot.vision.tags(TAG_FAMILY, size_m=MARKER_SIZE_M)

    gripper.open()
    time.sleep(SETTLE_S)

    face_id = None
    for _ in range(MAX_ITERATIONS):
        frame = detector.detect(robot.core.get_image(rgb_topic), intrinsics=intrinsics)
        faces = [tag for tag in frame.tags if 0 <= tag.id <= 5 and tag.transform is not None]
        if not faces:
            raise RuntimeError("Cube not detected; pick stopped")
        if face_id is None:
            face_id = next((tag.id for tag in faces if tag.id == 4), faces[0].id)
        face = next((tag for tag in faces if tag.id == face_id), None)
        if face is None or face.transform is None:
            raise RuntimeError(f"Cube face {face_id} lost; pick stopped")

        tag = face.to_frame(robot.frames, arm.planning_frame)
        ee_pose = robot.frames.lookup(
            arm.planning_frame, arm.end_effector_link, at=face.transform.stamp
        ).as_pose()
        # Tag +Z points out of the face; tool +Z points into it. The cube has
        # four equivalent jaw orientations around that approach axis.
        tool_down = tag.rotation * Quaternion.from_rpy(math.pi, 0.0, 0.0)
        alignment_orientation = max(
            (tool_down * Quaternion.from_rpy(0.0, 0.0, turn * math.pi / 2) for turn in range(4)),
            key=lambda orientation: abs(
                sum(a * b for a, b in zip(orientation.array(), ee_pose.orientation.array()))
            ),
        )
        alignment_pose = Pose(
            position=tag.apply(Point(z=ALIGN_STANDOFF_M)),
            orientation=alignment_orientation,
        )
        error = alignment_pose.position - ee_pose.position
        distance = error.magnitude()
        rotation_dot = sum(
            a * b for a, b in zip(alignment_pose.orientation.array(), ee_pose.orientation.array())
        )
        angle = 2 * math.acos(min(1.0, abs(rotation_dot)))
        print(f"Face {face_id}: position error {distance:.3f} m, orientation error {math.degrees(angle):.1f} deg")
        if distance <= POSITION_TOLERANCE_M and angle <= ORIENTATION_TOLERANCE_RAD:
            break

        step = error if distance <= MAX_TRANSLATION_STEP_M else error * (MAX_TRANSLATION_STEP_M / distance)
        arm.move_pose(
            Pose(
                position=ee_pose.position + step,
                orientation=alignment_pose.orientation,
            ),
            avoid_obstacles=True,
        )
        time.sleep(SETTLE_S)  # Wait for a fresh image after the arm stops.
    else:
        raise RuntimeError(f"Cube alignment did not converge within {MAX_ITERATIONS} iterations")

    # Move from the standoff through the face toward the cube centre along tag -Z.
    pick_pose = Pose(
        position=tag.apply(Point(z=-CUBE_HALF_SIZE_M + GRASP_CLEARANCE_M)),
        orientation=alignment_pose.orientation,
    )
    print(f"Aligned with face {face_id}; advancing along tag -Z to grasp")

    arm.move_linear(pick_pose)
    time.sleep(SETTLE_S)
    gripper.close()
    time.sleep(SETTLE_S)
    arm.move_linear(ee_pose)
    time.sleep(SETTLE_S)
    gripper.open()

TypeScript

import { connect } from "olo/web";
import { Point, Pose, Quaternion } from "olo/spatial";
import { cameraInfoToIntrinsics, tagToFrame } from "olo/vision";

const RGB_TOPIC = "wrist_rgbd/image";
const CAMERA_INFO_TOPIC = "wrist_rgbd/camera_info";
const TAG_FAMILY = "aruco_4x4_50";
const MARKER_SIZE_M = 0.036;
const CUBE_HALF_SIZE_M = 0.025;
const ALIGN_STANDOFF_M = 0.05;
const GRASP_CLEARANCE_M = 0.01;
const MAX_ITERATIONS = 10;
const POSITION_TOLERANCE_M = 0.005;
const ORIENTATION_TOLERANCE_RAD = (5 * Math.PI) / 180;
const MAX_TRANSLATION_STEP_M = 0.1;
const SETTLE_MS = 500;
const sleep = (ms: number) => new Promise((resolve) => setTimeout(resolve, ms));

const robot = await connect().robot();
const arm = robot.kinematics;
const gripper = await arm.getEndEffector();
if (gripper === undefined) {
  throw new Error("This example requires a gripper end effector");
}

const rgbTopic = robot.core.topic(RGB_TOPIC, "sensor_msgs/msg/Image");
const infoTopic = robot.core.topic(
  CAMERA_INFO_TOPIC,
  "sensor_msgs/msg/CameraInfo",
);
const intrinsics = cameraInfoToIntrinsics(
  await robot.core.getLatest(infoTopic),
);
const detector = robot.vision.tags(TAG_FAMILY, { sizeM: MARKER_SIZE_M });
const planningFrame = await arm.getPlanningFrame();
const endEffectorLink = await arm.getEndEffectorLink();

await gripper.open();
await sleep(SETTLE_MS);

let faceId: number | undefined;
let tagInPlanning: Awaited<ReturnType<typeof tagToFrame>> | undefined;
let alignmentPose: Pose | undefined;
let eePose: Pose | undefined;
let aligned = false;
for (let i = 0; i < MAX_ITERATIONS; i += 1) {
  const frame = await detector.detect(await robot.core.getImage(rgbTopic), {
    intrinsics,
  });
  const faces = frame.tags.filter(
    (tag) => tag.id >= 0 && tag.id <= 5 && tag.transform !== undefined,
  );
  if (faces.length === 0) {
    throw new Error("Cube not detected; pick stopped");
  }
  faceId ??= faces.find((tag) => tag.id === 4)?.id ?? faces[0]!.id;
  const face = faces.find((tag) => tag.id === faceId);
  if (!face?.transform) {
    throw new Error(`Cube face ${faceId} lost; pick stopped`);
  }

  tagInPlanning = await tagToFrame(face, {
    frames: robot.frames,
    targetFrame: planningFrame,
  });
  const stamp = face.transform.stamp;
  const currentEePose = (
    await robot.frames.lookup(
      planningFrame,
      endEffectorLink,
      stamp ? { at: stamp } : {},
    )
  ).asPose();
  eePose = currentEePose;
  // Tag +Z points out of the face; tool +Z points into it. The cube has
  // four equivalent jaw orientations around that approach axis.
  const toolDown = tagInPlanning.rotation.multiply(
    Quaternion.fromRpy(Math.PI, 0, 0),
  );
  const currentRotation = currentEePose.orientation.array();
  let alignmentOrientation = toolDown;
  let closestDot = -1;
  for (let turn = 0; turn < 4; turn += 1) {
    const candidate = toolDown.multiply(
      Quaternion.fromRpy(0, 0, (turn * Math.PI) / 2),
    );
    const dot = Math.abs(
      candidate
        .array()
        .reduce(
          (sum, value, index) => sum + value * currentRotation[index]!,
          0,
        ),
    );
    if (dot > closestDot) {
      alignmentOrientation = candidate;
      closestDot = dot;
    }
  }
  alignmentPose = new Pose(
    tagInPlanning.apply(new Point(0, 0, ALIGN_STANDOFF_M)),
    alignmentOrientation,
  );
  const error = alignmentPose.position.subtract(currentEePose.position);
  const distance = error.magnitude();
  const rotationDot = alignmentPose.orientation
    .array()
    .reduce((sum, value, index) => sum + value * currentRotation[index]!, 0);
  const angle = 2 * Math.acos(Math.min(1, Math.abs(rotationDot)));
  console.log(
    `Face ${faceId}: position error ${distance.toFixed(3)} m, orientation error ${((angle * 180) / Math.PI).toFixed(1)} deg`,
  );
  if (distance <= POSITION_TOLERANCE_M && angle <= ORIENTATION_TOLERANCE_RAD) {
    aligned = true;
    break;
  }

  const step =
    distance <= MAX_TRANSLATION_STEP_M
      ? error
      : error.scale(MAX_TRANSLATION_STEP_M / distance);
  await arm.movePose(
    new Pose(currentEePose.position.add(step), alignmentPose.orientation),
    { avoidObstacles: true },
  );
  await sleep(SETTLE_MS); // Wait for a fresh image after the arm stops.
}

if (
  !aligned ||
  tagInPlanning === undefined ||
  alignmentPose === undefined ||
  eePose === undefined
) {
  throw new Error(
    `Cube alignment did not converge within ${MAX_ITERATIONS} iterations`,
  );
}

// Move from the standoff through the face toward the cube centre along tag -Z.
const pickPose = new Pose(
  tagInPlanning.apply(new Point(0, 0, -CUBE_HALF_SIZE_M + GRASP_CLEARANCE_M)),
  alignmentPose.orientation,
);
console.log(`Aligned with face ${faceId}; advancing along tag -Z to grasp`);

await arm.moveLinear(pickPose);
await sleep(SETTLE_MS);
await gripper.close();
await sleep(SETTLE_MS);
await arm.moveLinear(eePose);
await sleep(SETTLE_MS);
await gripper.open();