OmArm Zero Series Part 1: Build the arm and ESP32 web control · Part 2: URDF, RViz and Gazebo · Part 3: MoveIt 2 and real robot · Part 4: Computer vision pick-and-place (this post)
At a glance
What you’ll build: A complete vision-guided pick-and-place pipeline for the OmArm Zero. You’ll calibrate a USB camera, detect an ArUco-marked cube in real time, transform its pixel position into robot-frame meters, solve a custom 5-DOF inverse kinematics for a vertical top-down grasp, and run the full pick-carry-place cycle in both Gazebo simulation and on the real hardware.
Prerequisites: Completed Parts 1 through 3 (assembled arm with ROS 2 serial firmware, working URDF, MoveIt config and hardware bridge). You should be able to launch real_moveit.launch.py and move the real arm from RViz.
Hardware: Any USB webcam (720 p or higher), a rigid overhead mount about 0.5 m above the table looking straight down, a printer for the ArUco markers, a small cube (ours is 3 x 3 x 10 cm with a 31 mm marker glued on top).
Software: Ubuntu 24.04, ROS 2 Jazzy, Gazebo Harmonic, MoveIt 2, OpenCV (with ArUco), NumPy, SciPy
Time: A weekend, including both calibrations and real-robot tuning
Difficulty: Advanced, but every hard part comes with a story of how it failed first and how we fixed it.
Files: Full ROS 2 workspace in the shop (free download, link at the end)
Get the package and run it
Everything in this tutorial ships as one complete ROS 2 workspace: OmArm Zero ROS 2 Workspace — free download. It contains every package, launch file, calibration tool, and parameter file from all four Parts, so you can run each command below instead of copying code from this page.
Unpack the archive and build once:
cd omarm_zero_ws
source /opt/ros/jazzy/setup.bash
rosdep install --from-paths src --ignore-src -y # pulls OpenCV, MoveIt, etc.
colcon build --symlink-install
source install/setup.bashFrom there, the whole Part 4 pipeline is five commands, in this order:
# 1. Print the five ArUco markers (section 3)
ros2 run computer_vision_aruco generate_markers \
--marker-ids 0,1,2,3,4 --size 600 --dictionary 4X4_50
# 2. Intrinsic calibration — once per camera (section 4)
ros2 launch computer_vision_aruco camera_calibration.launch.py \
camera_id:=0 checkerboard_width:=9 checkerboard_height:=6 square_size:=0.025
# 3. Extrinsic calibration — after mounting the camera (section 5)
ros2 run computer_vision_aruco calibrate_extrinsics --ros-args \
--params-file src/computer_vision_aruco/config/aruco_params.yaml \
-p output_file:=src/computer_vision_aruco/config/camera_extrinsics.yaml
# 4. Pick-and-place in SIMULATION, perception real (section 10)
ros2 launch omarm_zero_moveit_config cube_gazebo.launch.py \
camera_info_url:=/absolute/path/to/your_calibration.yaml run_demo:=true
# 5. Pick-and-place on the REAL robot (section 11)
ros2 launch omarm_zero_moveit_config cube_real.launch.py \
transport:=serial camera_info_url:=/absolute/path/to/your_calibration.yamlTwo things to do before step 3, both explained in section 5: tape the four corner markers to your table, and enter their measured positions in table_markers_expected inside src/computer_vision_aruco/config/aruco_params.yaml. The sections below explain what each command does, what good output looks like, and what to check when something misbehaves.
Table of contents
- From motion planning to perception
- The perception problem: from pixels to world coordinates
- ArUco markers: cheap, robust 6-DoF poses
- Camera setup and intrinsic calibration
- Extrinsic calibration: the four-marker table rig
- The vision pipeline: three nodes, one launch
- A custom 5-DOF IK for top-down grasping
- The two wrist families
- The pick-and-place task
- Simulation first: Gazebo digital twin with real camera
- Going real: full pipeline on hardware
- Troubleshooting
- FAQ
- Series wrap-up
1. From motion planning to perception
Think about what we did at the end of Part 3. MoveIt planned collision-free trajectories, the ESP32 bridge executed them on the real servos, and you could move the arm to any reachable pose by dragging an interactive marker in RViz or by typing joint angles into a Python script. That worked, but the targets were always numbers that you provided. A robot that only goes where you tell it is a fancy CNC machine.
The moment a robot reacts to what it sees, it becomes something else. That is what this final part is about.
Here is the plan. A USB camera looks straight down at the table. It spots a small cube with a printed marker on top. An ArUco detector identifies the marker and estimates its 6-DoF pose relative to the camera. A calibration transform converts that pose into the robot’s own coordinate frame. The result is a position in meters, published on a ROS 2 topic that the pick-and-place task subscribes to. The task feeds that position to a custom inverse kinematics solver, plans a vertical approach, descends straight down onto the cube, grasps, lifts, carries, and places the cube at a marked target. Then the arm goes home and waits for the next cycle.
Two design decisions shaped everything that follows.
First, markers instead of deep learning. A printed ArUco marker gives you a full 6-DoF pose from a single cheap camera, with sub-centimeter accuracy, at hundreds of frames per second, with zero training data and zero GPU. For a desktop robot arm that needs to find one known object on a table, that is the right engineering tool. Nothing stops you from swapping the detector later. The interface to the rest of the system is just a ROS 2 topic (/cup_pose — named for the cup we originally planned to pick; we switched to a cube but kept the topic name), so any detector that can publish a PoseStamped message will plug in.
Second, simulation first, but with the real camera. The Gazebo scene mirrors the real table, and the cube in Gazebo spawns at the position the real camera reports for the real cube. You debug the entire pipeline (perception, planning, grasping) with zero risk to hardware, and the perception half is already the real thing. When you switch to the real arm, the only change is which launch file you run.
2. The perception problem: from pixels to world coordinates
A camera measures pixels. The robot needs meters in its own base frame. Getting from one to the other is the core problem of robot vision, and it takes three ingredients.
Intrinsics describe how your specific camera maps 3-D rays to 2-D pixels. The two focal lengths (fx, fy) tell you how many pixels one radian spans, horizontally and vertically. The optical center (cx, cy) tells you which pixel the camera’s central axis hits. And the distortion coefficients describe how the lens bends the image, especially near the edges. Every camera unit is slightly different, even two copies of the same model, because of manufacturing tolerances in the lens and sensor placement. You have to measure these numbers for your camera. That process is called intrinsic calibration, and we will walk through it in section 4.
Marker geometry and PnP give you the marker’s pose relative to the camera. PnP stands for Perspective-n-Point. If you know the physical size of the marker (its real-world side length in meters), and the camera has detected the four corners in pixels, then you have four 3-D-to-2-D correspondences. That is enough to solve for the rigid transform (rotation and translation) from the marker to the camera. OpenCV’s solvePnP does this automatically when you pass in the intrinsics, the corner pixels, and the marker size. The output is a rotation vector and a translation vector that together describe the marker’s full 6-DoF pose in the camera’s optical frame.
Extrinsics tell you where the camera sits relative to the robot. This is one static transform from base_link to camera_optical_frame. If you know where the camera is, and PnP tells you where the marker is relative to the camera, then by chaining the transforms you know where the marker is relative to the robot. TF2 (which we set up in Part 2) handles the bookkeeping. The chain looks like this:
world == base_link --> camera_optical_frame --> aruco_marker_0 (the cube)
(extrinsics, static) (detector, live)Each ingredient has its own failure mode. Wrong intrinsics bend the world so that positions drift toward the image edges. A wrong marker size scales everything (5% error in the printed marker width means 5% error in the estimated distance). Wrong extrinsics shift or rotate the entire scene. The classic symptom of bad extrinsics is the robot confidently driving to a position that is consistently 5 to 10 cm away from the actual object. You will meet all three failure modes in this tutorial, and we will fix each one.
3. ArUco markers: cheap, robust 6-DoF poses
ArUco markers are binary square fiducials. Each marker is a black square containing a grid of black and white cells that encode a unique integer ID. They were designed specifically for camera-based pose estimation in robotics. OpenCV’s ArUco module detects their four corners at sub-pixel accuracy, identifies them by ID (so you can tell which marker is which even when multiple markers are visible), and with the physical side length it solves PnP for a full 6-DoF pose.
The detection pipeline works like this. The image is converted to grayscale. OpenCV runs adaptive thresholding to find all black square candidates. For each candidate, it checks whether the internal cell pattern matches a valid ID in the chosen dictionary. If it does, the four corner positions are refined to sub-pixel accuracy, and the marker ID is decoded. From those four corners plus the known physical size, solvePnP (using the SOLVEPNP_IPPE_SQUARE algorithm, which is optimized for square markers) computes the marker’s rotation and translation in the camera frame. The whole pipeline runs at hundreds of frames per second on a modern CPU. No GPU needed.
We use the DICT_4X4_50 dictionary, which contains 50 markers with a 4×4 internal grid. A smaller grid means fewer bits per marker, which means each cell is physically larger and easier to detect at longer distances or with lower resolution cameras. For a desktop setup with five markers, 50 is more than enough.
Our marker inventory:
| ID | Location | Size | Purpose |
|---|---|---|---|
| 0 | On top of the cube | 31 mm | The object to pick |
| 1 | Table corner (front right) | 31 mm | Extrinsic calibration rig |
| 2 | Table corner (front left) | 31 mm | Extrinsic calibration rig |
| 3 | Table corner (back left) | 31 mm | Extrinsic calibration rig |
| 4 | Table corner (back right) | 31 mm | Extrinsic calibration rig / place target |
Generate and print them with:
ros2 run computer_vision_aruco generate_markers \
--marker-ids 0,1,2,3,4 --size 600 --dictionary 4X4_50
Printing tips that will save you real debugging time later:
Measure the printed black square with calipers and use that value in the config, not the value you asked the printer for. Printers scale. If your printer shrinks the marker by 5%, every distance estimate from that marker will be off by 5%. We set all five markers to 31 mm (marker_size: 0.031 in the config), but your printer might produce 30 mm or 32 mm. Measure once, update the config, and never think about it again.
Use matte paper. Glossy paper under ceiling lights creates specular reflections that wipe out the black cells. The detector sees a white blob instead of a marker and drops the detection entirely. Matte inkjet paper is fine.
Keep a white border (the “quiet zone”) around the black square. The detector needs contrast between the marker edge and its surroundings to find the square outline.
Check your resolution. A 31 mm marker at 0.5 m distance produces roughly 45 pixels of width at 1280×720. That works, but it is the lower limit for reliable detection. If you drop to 640×480, the marker shrinks to about 23 pixels wide and detection becomes flaky. Use 720 p or higher, and add more light.
4. Camera setup and intrinsic calibration
Mount the webcam rigidly, looking straight down, roughly centered over the workspace, at about 0.5 m height. Rigidly is the operative word. Every millimeter the camera moves after calibration is a millimeter of grasp error. We used a small aluminum photo stand clamped to the desk, with the camera zip-tied to the crossbar. Anything solid enough that it does not wobble when you touch the table will work.
What intrinsic calibration actually does
When you point a camera at a perfectly straight line in the real world, the line appears slightly curved in the image, especially near the edges. That is lens distortion. Every lens has it. The pinhole camera model assumes light rays travel in perfectly straight lines through a single point, but real lenses are pieces of curved glass, and they bend the rays.
Intrinsic calibration measures exactly how your lens bends those rays. The output is a set of numbers: the focal lengths (fx and fy), the optical center (cx and cy), and five distortion coefficients. With these numbers, OpenCV can “undistort” the image so that straight lines in the world look straight in the image again. More importantly, PnP needs these numbers to compute accurate 3-D poses from 2-D pixel positions.
The calibration procedure
You calibrate with a printed checkerboard pattern. The board has a known grid of alternating black and white squares. OpenCV detects the inner corners of the grid (where four squares meet) and uses them as calibration points. By seeing the same board at many different angles and positions across the image, the algorithm (Zhang’s method) solves for all the intrinsic parameters simultaneously.
Print a checkerboard (9 columns by 6 rows of inner corners is standard), glue it to something flat and rigid (a piece of cardboard works), and run:
ros2 launch computer_vision_aruco camera_calibration.launch.py \
camera_id:=0 checkerboard_width:=9 checkerboard_height:=6 square_size:=0.025The calibration window opens and shows the live camera feed. Hold the checkerboard in front of the camera and tilt it in different orientations. Press SPACE to capture each pose. Collect at least 20 poses. Cover the entire field of view, especially the corners and edges, because that is where distortion is worst.
Two rules that matter:
Calibrate at the resolution you will run at. Intrinsic parameters are specific to the image resolution. If you calibrate at 1920×1080 and then run the detector at 1280×720, the focal lengths will be wrong and your poses will drift. Our config runs at 1280×720 (frame_width: 1280, frame_height: 720), so we calibrate at 1280×720.
Cover the whole field of view with board poses. If all your captures are in the center of the image, the algorithm has no data to estimate the distortion at the edges. The cube will always be at the edges at some point, and bad distortion correction there means bad pose estimates there.
Press c to run the calibration and s to save. The result is a YAML file in ./calibration/camera_calibration_<timestamp>.yaml. This file becomes your camera_info_url for all subsequent launches.
Verify that the camera node actually loaded your calibration by checking:
ros2 topic echo /camera/camera_info --onceThe output must show your focal lengths, not zeros or defaults. If it shows a 3×3 identity matrix as the camera matrix, the calibration file was not loaded. Check the path.
5. Extrinsic calibration: the four-marker table rig
Here is where our first attempt failed, and the failure taught us something important.
We initially measured the camera pose by hand. It was about 50 cm up, roughly centered, pointing straight down. We typed roll, pitch, and yaw into a static transform publisher and launched the pipeline. The result: the detected cube position was confidently wrong, and the arm drove to a spot 10 cm away from the cube every single time. The error was consistent, which is the hallmark of a calibration problem rather than random noise.
Here is why eyeballing does not work for this. One degree of camera tilt at 0.5 m height produces about 9 mm of position error at the table surface. You cannot see one degree of tilt. And the camera’s optical axis is invisible, so even getting the tilt direction right is guesswork. The translation (XY offset of the camera from the robot base) is easier to measure with a ruler, but the rotation is not.
The fix: let the system calibrate itself
The four table-corner markers have known positions in the robot frame. You measure each marker’s center distance from the robot base with a ruler. These go into aruco_params.yaml as the table_markers_expected parameter:
table_markers_expected: '{"1":[0.272,-0.1645],"2":[-0.272,-0.1645],
"3":[-0.272,0.1345],"4":[0.272,0.1345]}'
Each entry is the marker center’s XY position in the robot’s base_link frame, in meters. Positive X is in front of the robot, positive Y is to the left. The Z coordinate is zero because the markers lie flat on the table, and base_link sits on the table surface.
The camera sees those same markers in its own optical frame. So now you have four 3-D point correspondences: four points whose positions you know in both the camera frame and the robot frame. Finding the rigid transform (rotation and translation) that maps one set of points to the other is the Kabsch algorithm, also known as the orthogonal Procrustes problem.
How Kabsch/SVD works (for beginners)
You have two sets of points: P_cam (marker positions in the camera frame, averaged over about 200 frames to reduce noise) and P_base (the positions you measured with a ruler). You want to find the rotation R and translation t such that P_base = R * P_cam + t, minimizing the sum of squared distances between the predicted and measured positions.
The algorithm works in three steps. First, center both point sets by subtracting their respective means. Second, compute the cross-covariance matrix H between the centered point sets and take its Singular Value Decomposition (SVD). Third, construct the rotation from the SVD factors, with a sign correction to ensure a proper rotation (not a reflection). The translation falls out from the centroids and the rotation.
Here is the actual implementation from calibrate_extrinsics.py:
@staticmethod
def _kabsch(P_src, P_dst):
"""Rigid transform mapping P_src -> P_dst: dst = R @ src + t."""
c_s = P_src.mean(axis=0)
c_d = P_dst.mean(axis=0)
H = (P_src - c_s).T @ (P_dst - c_d)
U, _, Vt = np.linalg.svd(H)
d = np.sign(np.linalg.det(Vt.T @ U.T))
D = np.diag([1.0, 1.0, d])
R = Vt.T @ D @ U.T
t = c_d - R @ c_s
return R, tThe D = np.diag([1.0, 1.0, d]) line is the reflection correction. If det(V * U^T) is negative, the SVD found a reflection instead of a rotation. Flipping the sign of the third column fixes it. Without this correction, the transform would mirror the scene, and the arm would reach for positions on the wrong side.
Running the calibration
Make sure the camera is mounted, all four table markers are visible, and the ArUco detector is running. Then:
ros2 run computer_vision_aruco calibrate_extrinsics --ros-args \
--params-file <install>/config/aruco_params.yaml \
-p output_file:=src/computer_vision_aruco/config/camera_extrinsics.yamlThe node collects 200 TF samples per marker (the samples: 200 parameter in aruco_params.yaml), averages them to reduce jitter, runs the Kabsch solve, and writes the result to camera_extrinsics.yaml. It reports the residual RMS, which tells you how well the computed transform reproduces the known marker positions:
Marker 1: Residuum 0.78 cm
Marker 2: Residuum 1.12 cm
Marker 3: Residuum 0.85 cm
Marker 4: Residuum 1.03 cm
RMS = 0.95 cm (Marker: 1,2,3,4)Our residual came out at 0.95 cm, meaning every marker is reprojected to within a centimeter. For a 3 cm cube, that is good enough for a reliable grasp.
The calibration also sanity-checks the result. If the camera Z is negative (meaning the camera is below the table), or if the optical axis is not pointing downward, or if the RMS exceeds 2 cm, it logs a warning. These checks catch sign errors in the measured marker positions, which are the most common mistake.
Continuous verification
The extrinsic calibration keeps paying off after it runs. The cube_pose_publisher node continuously re-checks the four table markers against their expected positions and logs the RMS every few seconds:
Table-marker verification (base_link X/Y):
ID 1: detected (+0.278,-0.163) expected (+0.272,-0.165) err=0.6 cm
ID 2: detected (-0.270,-0.166) expected (-0.272,-0.165) err=0.2 cm
ID 3: detected (-0.271,+0.133) expected (-0.272,+0.135) err=0.2 cm
ID 4: detected (+0.271,+0.136) expected (+0.272,+0.135) err=0.1 cm
RMS error = 1.0 cm <-- healthy. > 3 cm? Recalibrate.If someone bumps the camera stand, the RMS jumps immediately. A rule that saved us more than once: the corner markers are your reference rig. Never move them. If you need a marker for something else (for example, marker 4 doubles as the place target), print a second copy.
6. The vision pipeline: three nodes, one launch
The vision system consists of three ROS 2 nodes that run together in a single launch file. Each node has one job, and they communicate through the standard ROS 2 mechanisms: topics for data, TF for transforms.
camera_node --image--> aruco_detector_node --TF--> cube_pose_publisher --> /cup_pose
| |
/aruco/debug_image table collision object,
RViz markers, RMS logLaunch the entire vision pipeline with:
ros2 launch computer_vision_aruco vision_bringup.launch.py \
camera_info_url:=/absolute/path/to/your_calibration.yamlNode 1: camera_node
The camera node opens the USB webcam, captures frames at 30 fps, and publishes them as ROS 2 Image messages along with CameraInfo messages (which contain the intrinsic calibration). The config in aruco_params.yaml specifies the camera device:
camera_node:
ros__parameters:
camera_device: "/dev/v4l/by-id/usb-HD_Web_Camera_HD_Web_Camera_Ucamera001-video-index0"
camera_id: 0 # fallback only
frame_rate: 30.0
frame_width: 1280
frame_height: 720
camera_frame: "camera_optical_frame"
camera_info_url: "" # path to calibrated intrinsics (override at launch)A practical tip: use the camera_device path (the /dev/v4l/by-id/... symlink) instead of camera_id. Laptops have built-in webcams that also claim video indices, and the indices can swap every time you replug the USB camera. The by-id path is stable because it is based on the camera’s USB descriptor, not its enumeration order.
Node 2: aruco_detector_node
This node subscribes to the camera images, detects ArUco markers, estimates their poses, and broadcasts TF transforms. Here is the setup from the actual source code:
class ArucoDetectorNode(Node):
def __init__(self):
super().__init__('aruco_detector_node')
self.declare_parameter('marker_size', 0.05)
self.declare_parameter('marker_sizes', '') # per-ID JSON
self.declare_parameter('dictionary', '4X4_50')
self.declare_parameter('camera_frame', 'camera_link')
self.declare_parameter('publish_tf', True)
self.declare_parameter('tf_prefix', 'aruco_marker_')
# ... parameter loading ...
# Initialize ArUco detector (compatible with OpenCV 4.7+)
self.aruco_dict = self.get_aruco_dictionary(self.dictionary_name)
self.aruco_params = self.get_detector_parameters()
cv_version = tuple(map(int, cv2.__version__.split('.')[:2]))
if cv_version >= (4, 7):
self.aruco_detector = cv2.aruco.ArucoDetector(
self.aruco_dict, self.aruco_params)
self.use_new_api = TrueThe config supports per-ID marker sizes as a JSON string, so you can use different physical sizes for different markers. In our setup, all five markers are 31 mm:
aruco_detector_node:
ros__parameters:
marker_size: 0.031
marker_sizes: '{"0": 0.031, "1": 0.031, "2": 0.031, "3": 0.031, "4": 0.031}'
dictionary: "4X4_50"
camera_frame: "camera_optical_frame"
publish_tf: true
tf_prefix: "aruco_marker_"For each detected marker, the node broadcasts a TF transform from camera_optical_frame to aruco_marker_<id>. It also publishes a debug image on /aruco/debug_image that shows the detected markers with their bounding boxes and IDs drawn on top. Open this in rqt_image_view to verify that detection is working.
Node 3: cube_pose_publisher
This is the node that turns raw detections into something the pick-and-place task can actually use. A raw TF lookup gives you the marker position, but that position jitters frame to frame, can have single-frame outliers, and gives no indication of whether the marker is still visible or whether the data is stale. The cube pose publisher adds four things on top of the raw detection.
EWMA smoothing. A 31 mm marker at 0.5 m distance jitters a few millimeters from frame to frame due to image noise, corner detection uncertainty, and slight camera vibration. An Exponentially Weighted Moving Average (EWMA) filter steadies the position. EWMA is a simple filter where each new position is blended with the previous smoothed position using a weight alpha:
smoothed = alpha * new + (1 - alpha) * smoothed_previousWith smoothing_alpha: 0.3, each new detection contributes 30% and the history contributes 70%. This produces a smooth position that responds to real movement within a few frames but filters out single-frame noise. Here is the filtering logic from the source:
if self.smoothed is not None and self._reject_count < 5:
jump = math.dist((x, y, z), self.smoothed)
if jump > self.min_jump:
self._reject_count += 1
self.get_logger().warn(
f'Cube pose jump {jump*100:.1f} cm > '
f'{self.min_jump*100:.1f} cm — ignoring outlier '
f'({self._reject_count}/5)')
else:
self._reject_count = 0
x = self.alpha * x + (1 - self.alpha) * self.smoothed[0]
y = self.alpha * y + (1 - self.alpha) * self.smoothed[1]
z = self.alpha * z + (1 - self.alpha) * self.smoothed[2]
self.smoothed = (x, y, z)Outlier rejection with self-recovery. If a new detection jumps more than min_position_jump (5 cm by default) from the smoothed position, it is rejected as an outlier. This handles single-frame detection glitches where the marker corner localization goes wrong. But if the cube was actually moved to a new position, consecutive “outliers” will agree with each other. After 5 consecutive rejections, the filter assumes the cube really did move and re-seeds to the new position.
Freshness. The /cup_pose topic is only published while the marker is actively seen and the TF transform is fresh (less than pose_timeout_sec seconds old). If the cube is out of view or the marker is occluded, /cup_pose stops updating. The pick task checks for this and never acts on a stale pose.
Table collision object. The publisher also adds a box collision object representing the table to the MoveIt planning scene. This prevents MoveIt from planning trajectories that drive the arm through the table surface. The table dimensions and position come from the config:
cube_pose_publisher_node:
ros__parameters:
marker_frame: "aruco_marker_0"
target_frame: "world"
cube_size_x: 0.03
cube_size_y: 0.03
cube_size_z: 0.10
cube_center_z: 0.05
publish_rate: 10.0
pose_timeout_sec: 3.0
smoothing_alpha: 0.3
min_position_jump: 0.05
enable_planning_scene: false # cube collision OFF (blocks grasp plans)
enable_table: true
table_size_x: 0.60
table_size_y: 0.35
table_size_z: 0.02Notice enable_planning_scene: false for the cube itself. We tried adding the cube as a collision object, but it blocked the grasp plan. When the fingers need to wrap around the cube, a collision object at the cube position prevents MoveIt from planning the approach. Instead, the cube is shown as a blue RViz marker (purely visual, no collision), and the table stays as a collision object to protect the arm.
7. A custom 5-DOF IK for top-down grasping
The camera says the cube is at position (x, y). What joint angles put the gripper there, with the fingers pointing straight down?
In Part 3, we configured MoveIt with position_only_ik: true, because a 5-DOF arm cannot satisfy an arbitrary 6-DoF pose request. The KDL solver would fail on most targets if you asked for both position and orientation. Position-only worked fine for “go to this XYZ,” but a grasp needs more than just position. The fingers must approach vertically, pointing straight down. If they come in at an angle, they knock the cube over instead of grasping it.
Instead of giving up, count the degrees of freedom. A top-down grasp needs:
- The tool position: 3 constraints (X, Y, Z)
- The approach axis pointing straight down: 2 constraints (the axis direction is defined by two angles)
That is 5 constraints total. The rotation about the vertical axis is completely free. You can grip a cube at any yaw angle and it does not matter. So 3 position + 2 orientation = 5 constraints, and we have 5 joints. The problem is not over-constrained. It is exactly constrained. We just need a solver that understands this specific problem, and MoveIt’s stock KDL solver does not.
The arm_ik solver
The arm_ik.py module implements this directly. It parses the URDF to build a forward kinematics chain from base_link through all five revolute joints to the tcp frame (a frame defined in the URDF at the fingertip midpoint, rotated so that tcp +Z is the approach/finger axis). Then it uses SciPy’s bounded least-squares optimizer to find joint angles that minimize two residuals simultaneously: the position error (3 components) and the axis alignment error (2 components).
Here is how the solver is set up:
JOINT_CHAIN = ['Revolute 1', 'Revolute 2', 'Revolute 3', 'Revolute 4',
'Revolute 5', 'gripper_base_to_tcp']
class ArmIK:
def __init__(self, xacro_path=None):
# Parse the URDF chain base_link -> tcp
if xacro_path is None:
xacro_path = os.path.join(
os.path.dirname(__file__), '..', '..', '..',
'omarm_zero_description', 'urdf', 'omarm_zero.xacro')
# ... parse joint origins, axes, limits from the URDF ...
self.limits = np.array(self.limits) # (5,2) lower/upper bounds
def fk(self, q):
"""Forward kinematics base_link -> tcp. Returns 4x4 transform."""
T = np.eye(4)
qi = 0
for e in self.chain:
T = T @ _T(e['R0'], e['xyz'])
if e['type'] == 'revolute':
T = T @ _T(_axis_R(e['axis'], q[qi]), np.zeros(3))
qi += 1
return TThe forward kinematics chain is built directly from the URDF joint descriptions, so any change to the URDF (different link lengths, different joint axis directions) propagates automatically. No magic numbers.
The solver itself uses a multi-seed strategy. Instead of starting from a single initial guess (which might converge to a local minimum or a bad solution branch), it generates many candidate starting configurations and picks the best result:
def ik_solve(self, target_xyz, approach_dir=(0, 0, -1), seed=None,
align_axis=2, w_orient=0.1, pos_tol=0.01, ang_tol=15.0,
quick=False):
"""Solve for [Rev1..Rev5] via bounded least-squares from many seeds.
Places the tcp at target_xyz AND aligns its axis 'align_axis'
(2=Z = finger/approach axis) with 'approach_dir' (default straight
down). Returns list[5] joints, or None if no seed converges."""
from scipy.optimize import least_squares
# ... 1500 random samples scored by FK, best become solver seeds ...
# ... structured sweeps of the joint space ...
# ... current arm state as a seed for continuity ...Sample-then-refine seeding: 1500 random joint configurations are scored cheaply by forward kinematics (just a position and axis direction check). The most promising candidates become solver seeds, alongside structured sweeps and the current joint state. This finds solution branches that a single seed would miss.
quick=True mode: For reachability scans (“is this height solvable at all?”), a stripped-down solve with fewer samples answers “no” in 1 to 2 seconds instead of 15 seconds. The task uses quick scans first for broad searches and falls back to the full solver before trusting a “no.”
Solution scoring: Among all valid solutions (those that meet both pos_tol and ang_tol), the solver prefers solutions that are close to the current joint state (for smooth, small motions between waypoints) and that have a healthy wrist posture. That second criterion deserves its own section, because it fixed our most stubborn bug.
8. The two wrist families
On the real robot, the grasp kept landing a centimeter beside the cube. Always the same side. The camera calibration checked out (RMS 1 cm). The IK position error said 0 to 2 mm. The cube was clearly visible and well-detected. So who was lying?
Nobody. Geometry was.
For a vertical approach axis (fingers pointing straight down), this wrist has exactly two solution families. There is nothing in between:
| Family | Wrist roll (Revolute 4) | Wrist pitch (Revolute 5) | What it looks like |
|---|---|---|---|
| “twisted” | approximately -180 degrees (at its end stop) | approximately -18 degrees | Wrist fully rotated, offset pointing sideways |
| “neutral” | approximately 0 degrees | approximately -162 degrees | Wrist straight, offset pointing correctly |
The solver happened to always find the twisted family first, because its structured seeds started near the joint limits where the twisted configuration lives. And because the tcp frame has a small lateral offset from the wrist axis (the fingertip midpoint is not exactly on the rotation center), the two families mirror that offset. Pick the wrong family and the calibrated offset points the wrong way. The fingers land beside the cube instead of around it.
The user-visible symptom was that the wrist looked completely twisted around. That twisted appearance was literally the explanation for the offset error.
The fix
Two changes in arm_ik.py solved the problem:
Seed the neutral family explicitly. The solver cannot prefer a branch it never finds. Prototype seeds for the neutral wrist configuration (Revolute 4 near zero, Revolute 5 near -162 degrees) were added to the seed pool, so the solver always explores both families.
Posture preference in the score. A small penalty on the absolute value of Revolute 4 makes the neutral family win whenever it exists. The solver still finds the twisted family as a valid solution, but it ranks lower than the neutral one.
One caveat: the neutral family does not exist everywhere. Joint limits kill it in the back-right zone behind the robot. In the comfortable front/center zone (where you would normally place the cube), you get precise neutral-wrist grasps. At the edges, the task warns you (“coarse solution here, grasp may be off”) instead of failing silently. That is an honest limitation of a 5-DOF arm with a non-zero tcp offset.
9. The pick-and-place task
cup_pick_and_place.py orchestrates the whole pick-carry-place cycle. The interesting parts are the places where the obvious approach failed and had to be redesigned.
The full sequence
open gripper --> fly over (adaptive height) --> descend linearly --> close gripper
--> lift linearly --> carry --> fly over place target --> settle linearly
--> open gripper --> retreat linearly --> homeWhy linear descent matters
Our first version sent the pre-grasp position and the grasp position as two separate MoveIt joint-space goals. That is the obvious approach: plan to above the cube, then plan to the grasp position. The problem is that joint-space paths are arcs in Cartesian space. The arm does not move in a straight line. It sweeps through a curve, and on the way down the fingers swept sideways and pushed the cube away before ever closing.
The fix has two halves.
Adaptive fly-over. The task searches downward from grasp_z + approach_height for the highest vertically reachable height above the cube. It uses quick IK scans (the quick=True mode), with a full-solver fallback for any heights the quick mode rejects. Then it plans one normal MoveIt joint-space motion to that fly-over point. Because the fly-over is high above the cube, there is nothing to collide with, and the arc shape does not matter.
Linear descent. From the fly-over point, the task samples Z in 1 cm steps from the fly-over height down to the grasp height. For each step, it solves the IK using the previous step’s solution as the seed. Using the previous solution as the seed guarantees that the solver stays in the same solution branch (no wrist family flips mid-descent) and produces a continuous joint path. Each step is sent as its own single-point trajectory command to the joint trajectory controller. The tcp rides a perfectly vertical line over the cube.
The same trick runs in reverse for the lift. An arc during the lift would drag the grasped cube sideways. Linear up, linear down.
The node setup
class CupPickAndPlace(Node):
def __init__(self):
super().__init__('cup_pick_and_place')
self.declare_parameter('grasp_z', 0.04)
self.declare_parameter('approach_height', 0.10)
self.declare_parameter('lift', 0.06)
self.declare_parameter('approach_dir', [0.0, 0.0, -1.0])
self.declare_parameter('place_x', 0.15)
self.declare_parameter('place_y', 0.10)
self.declare_parameter('place_z', 0.10)
self.declare_parameter('gripper_open', [-1.2, -1.2])
self.declare_parameter('gripper_closed', [0.9, 0.9])
self.declare_parameter('home_joints', [0.0, 0.0, 0.0, 0.0, 0.0])
# 5-DOF IK solver (loads the URDF chain base_link -> tcp)
self.ik = arm_ik.ArmIK()
# MoveGroup action client for arm planning
self._mg_client = None
# Gripper: direct FollowJointTrajectory action
self._gripper_client = ActionClient(
self, FollowJointTrajectory,
'/joint_trajectory_controller/follow_joint_trajectory')
# Direct arm-trajectory publisher for LINEAR descent/ascent
self._arm_traj_pub = self.create_publisher(
JointTrajectory,
'/joint_trajectory_controller/joint_trajectory', 10)Config walkthrough: pick_place_params.yaml (simulation)
The simulation parameter file controls every aspect of the pick-and-place behavior:
cup_pick_and_place:
ros__parameters:
grasp_z: 0.07 # fingertip height at grasp
approach_height: 0.10 # fly-over height ABOVE grasp_z
lift: 0.06 # linear lift after closing
approach_dir: [0.0, 0.0, -1.0] # tcp +Z points straight DOWN
place_x: 0.15
place_y: 0.10
place_z: 0.07
gripper_open: [-1.2, -1.2]
gripper_closed: [0.16, 0.16]
gripper_time_sec: 2.0
gripper_joints: ['Revolute 6', 'Revolute 7']grasp_z: 0.07 is the height of the fingertips (the tcp frame) at the moment of grasping. Our cube is 10 cm tall, so the top surface is at Z = 0.10. Setting grasp_z to 0.07 means the fingertips descend to 3 cm below the top of the cube, which centers the grip on the upper portion of the cube.
approach_height: 0.10 is added to grasp_z to get the initial fly-over height. The task searches adaptively downward from this height, so setting it generously is safe. The arm will fly to the highest reachable point within this range.
approach_dir: [0.0, 0.0, -1.0] specifies that the tcp +Z axis (the finger/approach axis) should point straight down in the world frame. This gives you a clean vertical top-down grasp.
gripper_open and gripper_closed are the joint values for the two gripper joints (Revolute 6 and 7). Negative values open the fingers, positive values close them. The exact values depend on your gripper geometry and servo calibration.
Place at a marker, with honest tolerance
The place target can be a fixed position or the live-detected pose of another ArUco marker. The real-robot config uses marker 4 (one of the table-corner calibration markers):
place_marker_frame: 'aruco_marker_4'Put a copy of marker 4 wherever you want the cube placed. The task reads its live position from TF and uses that as the place target. But our marker 4 is at the table corner, outside the arm’s reach. Instead of failing, the task slides the target inward along the base-to-marker line until the vertical IK solves:
# Slide the target radially inward on the base->marker line
# until BOTH the fly-over AND the settle height are solvable.
r = math.hypot(mx, my)
ux, uy = (mx / r, my / r) if r > 1e-6 else (1.0, 0.0)
rr = min(r, 0.22) # beyond ~22 cm nothing is reachable
while rr >= 0.10:
tx, ty = ux * rr, uy * rr
if (self.ik.ik_solve((tx, ty, pz + 0.05), ...) is not None
and self.ik.ik_solve((tx, ty, pz), ...) is not None):
found = (tx, ty)
break
rr -= 0.01The task tells you exactly what happened: “marker out of reach, placing 10 cm short of it.”
Settle before you open
Releasing the cube at hover height caused it to drop and topple. The task descends until the cube stands on the table. On the real robot, the fingers slide down the cube for the last centimeter (the grip rides up naturally), pressing the cube flat onto the wood. Only then do the fingers open. Then the arm retreats straight up so the fingers cannot knock the placed cube over.
Fail safely
Every waypoint is IK-checked before any motion starts. An unreachable cube position aborts with a human-readable hint (“move the cube a few centimeters toward the table center”). Gazebo’s transient START_STATE_INVALID error (MoveIt error code -26, which happens when the simulation’s PID controller briefly overshoots a joint limit after a motion) is settled and retried automatically instead of killing the run.
10. Simulation first: Gazebo digital twin with real camera
One command starts the entire simulation pipeline:
ros2 launch omarm_zero_moveit_config cube_gazebo.launch.py \
camera_info_url:=<your-intrinsics>.yaml run_demo:=true
This single launch file starts:
- Gazebo with the robot model and a table with corner markers
- MoveIt and RViz for motion planning and visualization
- The real camera pipeline (camera node, ArUco detector, cube pose publisher)
- A cube spawned in Gazebo at the position the real camera reports
- The grasp-attach helper (for simulated grasping)
- The pick-and-place task itself
Put your real cube on the real table. Its Gazebo twin appears at the same coordinates, and the simulated arm picks it. Perception is real, actuation is simulated. That is the perfect debugging split, because you test the full perception chain (camera, calibration, detection, filtering, transform chain) with zero risk to hardware.
Three simulation tricks
Grasping without grasping physics. Simulating a realistic friction grip of a light object with hobby-servo PID gains is extremely unreliable. The simulated fingers either squeeze too hard and launch the cube, or not hard enough and it falls through. Instead of fighting the physics, a helper node teleports the cube model to the tcp frame at 50 Hz while the gripper is commanded closed. The cube stays exactly where the fingertips are, with no physics involved.
The 50 Hz update rate matters. We first used the gz service CLI command to set the cube pose on each update, but each CLI call took 150 to 300 ms. At that rate, the cube visibly lagged behind the arm and looked like it was falling off during every motion. The fix was a ROS-Gazebo service bridge (/world/empty/set_pose mapped to the SetEntityPose service) with an async ROS client. The latency dropped to single-digit milliseconds, and the cube stuck to the gripper smoothly.
A gravity-free, collision-free cube. The teleported cube does not need physics. With collisions enabled, the closed fingers pushed the cube on every tick, and it accumulated velocity. When we released it (stopped teleporting), the built-up velocity launched the cube across the Gazebo scene. The fix: disable gravity on the cube model (<gravity>0</gravity> in the SDF) and remove its collision elements entirely. The cube is purely visual during the grasp. When placed, it stays exactly where the task puts it.
The simulation audits your URDF. This might be the most valuable thing about the Gazebo digital twin. Our gripper never closed fully in simulation. The URDF said upper=0.3 rad and velocity=0.1 rad/s for the gripper joints, while the commanded close pose was 0.9 rad. Gazebo’s physics engine enforced the joint limits strictly, clamping the commanded position at 0.3 rad. The real servo had silently tolerated the out-of-range command for weeks. The 0.1 rad/s velocity limit made the gripper take 21 seconds to close even to its clamped position. Fix the URDF and both worlds behave.
Watching the simulation cycle
When run_demo:=true, the task runs automatically. You’ll see the arm in both RViz and Gazebo move through the full sequence:
- The gripper opens
- The arm flies to a position above the detected cube
- The arm descends vertically (you can see the straight-line path in RViz)
- The gripper closes around the cube
- The arm lifts vertically
- The arm carries the cube to the place position
- The arm descends to settle the cube on the table
- The gripper opens
- The arm retreats upward and returns home
Each step is logged to the terminal with timing and IK metrics. If something fails, the log tells you exactly which step failed and why.
11. Going real: full pipeline on hardware
Three terminals (or the bundled cube_real.launch.py):
# Terminal 1: robot (MoveIt + ESP32 bridge)
ros2 launch omarm_zero_moveit_config real_moveit.launch.py \
transport:=serial use_rviz:=true start_at_ros_zero:=true home_on_start:=false
# Terminal 2: vision (same pipeline as in simulation)
ros2 launch computer_vision_aruco vision_bringup.launch.py \
camera_info_url:=<your-intrinsics>.yaml
# Terminal 3: the task, with the REAL parameter file
ros2 run omarm_zero_moveit_config cup_pick_and_place.py --ros-args \
--params-file src/omarm_zero_moveit_config/config/pick_place_params_real.yaml
What is different on hardware
Each of these differences cost us at least one debugging session, so they are built into the package as separate configurations.
Two parameter files, never mixed. The real-robot parameters live in pick_place_params_real.yaml, completely separate from the simulation parameters. Here is why this matters:
# pick_place_params_real.yaml (real robot)
cup_pick_and_place:
ros__parameters:
grasp_z: 0.07
approach_height: 0.10
lift: 0.06
approach_dir: [0.0, 0.0, -1.0]
place_marker_frame: 'aruco_marker_4'
place_z: 0.01 # settle until cube stands on table
gripper_open: [-1.57, -1.57] # full servo range: 180 degrees
gripper_closed: [1.57, 1.57] # full servo range: 0 degrees
gripper_time_sec: 1.0
home_joints: [0.8, -0.8, -1.2, -0.8, -2.6] # safe real homeCompare the gripper values: simulation uses [-1.2, -1.2] open / [0.16, 0.16] closed (smaller range, ros2_control joint limits). The real robot uses [-1.57, -1.57] / [1.57, 1.57] (full servo range from 0 to 180 degrees, mapped through the hardware bridge).
The home_joints difference is the most dangerous one. The simulation default of [0, 0, 0, 0, 0] is harmless in Gazebo (it is just the URDF zero pose). On the real robot, those zeros go through the servo mapping formula deg = 90 + direction * (rad - zero_rad) * 57.3, which puts every servo at either its 0 or 180 degree end stop. Servos stall at end stops, the 5V rail sags, and all servos misbehave simultaneously. The real config uses the proven safe pose [0.8, -0.8, -1.2, -0.8, -2.6], which is the same home pose from Part 3.
Speed lives in the firmware, not in MoveIt. The bridge forwards only the final trajectory point to the ESP32. The ESP32 firmware ramps the servos toward that point at its own speed setting. MoveIt’s velocity scaling changes nothing on the real hardware. The bridge switches the firmware speed per command: arm moves use speed 5 (slow and smooth), gripper-only moves use speed 20 (snap shut quickly).
Calibrate the gripper with your eyes. A slow servo sweep (180 degrees to 0 degrees, pausing at each step) while watching the gripper answers two questions in one minute: where is fully open and where is cleanly closed. Our gripper ended up using the full servo range after re-seating the finger on the servo horn one tooth wider. Those two numbers become gripper_open and gripper_closed in the config.
The wrist-family fix is a real-robot fix. In simulation, both wrist families produce a successful grasp because the simulated gripper is perfectly symmetric and the teleportation-based grasping does not care about the lateral offset. Only on the real hardware, where physical fingers have real geometry and the tcp offset matters, does the twisted family miss the cube.
Watching the real cycle
Place the cube with its marker visible to the camera, in the workspace ring roughly 15 to 19 cm from the base. Launch all three terminals. The task waits for /cup_pose (up to 15 seconds), then begins.
The terminal log tells you everything:
Cup pose received. Planning a vertical top-down grasp (5-DOF)...
/cup_pose: Marker 0 @ (+0.165, -0.042, +0.050) [world==base_link]
1. Opening gripper (start state)...
2. Pre-grasp (vertically above the cube, z=0.17 = 7 cm above the marker)...
IK ok: pos_err=1mm, tcp-Z aligned 0 deg from vertical
3. Grasp (LINEAR vertical descent)...
Descent: 10 steps, z=0.17 -> z=0.07
4. Closing gripper...
5. Lifting (LINEAR up to z=0.13)...
6. Flying OVER the place position (z=0.14)...
Marker "aruco_marker_4" (+0.272,+0.135) out of reach
— placing 9 cm short of it: (+0.195,+0.097)
7. Opening gripper (placing)...
8. Returning to the home position...
Home position reached
=== Pick-and-place demo DONE! ===
Watch the fingers come straight down around the cube without touching it during the descent. The vertical linear approach keeps the fingers centered. After closing, the lift is also vertical, so the cube does not get dragged sideways.
The carry phase is a normal MoveIt joint-space motion. The arm swings from the pick position to the place position in an arc. The cube is gripped securely, so the arc shape does not matter here.
At the place position, the arm descends linearly again until the cube is firmly on the table. The settle height (place_z: 0.01 in the real config) is set low enough that the cube’s bottom surface contacts the table. The fingers slide down the cube slightly as the grip rides up, which presses the cube flat.
The fingers open, the arm retreats straight up (linear, so the fingers do not knock the placed cube over), and then returns to the home position.
12. Troubleshooting
The robot moves to the wrong place, consistently.
This is almost always an extrinsics problem. Check the table-marker RMS in the vision pipeline log output. If it exceeds 3 cm, run calibrate_extrinsics again. Also re-measure the table_markers_expected values. A sign error in one measured corner (positive instead of negative Y, for example) poisons the entire Kabsch solve.
/cup_pose does not update even though the cube moved.
Compare the raw detection with the filtered output. Run ros2 run tf2_ros tf2_echo world aruco_marker_0 to see the raw TF, and ros2 topic echo /cup_pose to see the filtered position. If the raw TF is current but /cup_pose is stuck, the outlier filter is still re-seeding. Give it a second (it needs 5 consecutive “outliers” before it re-seeds), or restart the vision pipeline. If the raw TF is also stale, the marker is not being detected. Check /aruco/debug_image for lighting problems or a toppled cube (a cube lying on its side hides the marker on top).
Camera fails to open or opens the wrong camera.
The camera re-enumerated after replugging. Run ls /dev/video* to see available devices. Laptops with built-in webcams also claim video indices. After replugging, your USB camera may have moved from video0 to video2. Set camera_id accordingly, or use the stable camera_device path from aruco_params.yaml.
The real robot does not react at all. The bridge says it finished cleanly after one second.
Something else is holding the serial port. The Arduino IDE’s serial monitor is the most common offender. Run fuser /dev/ttyUSB0 to find the process holding the port, close it, and relaunch. Also check that the ESP32 is powered and the USB cable supports data (some USB cables are charge-only).
“No vertically reachable height” or the task aborts before moving.
This is honest kinematics telling you the truth. At that cube position, a full vertical approach does not exist for this 5-DOF arm. The vertical-grasp workspace is a ring roughly 15 to 19 cm from the base. Closer positions hit the arm’s minimum reach. Farther positions exceed the maximum reach for a vertical approach. The task probes the neighborhood and prints a concrete suggestion: “push the cube 3 cm toward the marker-2 corner (reachable at (-0.03, -0.18)).” Keep the cube on the grasp ring and you will never see this message.
Gripper barely closes or takes forever in simulation.
Your URDF gripper joint limits do not cover the commanded close pose. Gazebo enforces limits strictly. If the URDF says upper=0.3 rad but the close command sends 0.9 rad, the physics engine clamps at 0.3 rad. A tiny velocity limit (0.1 rad/s) makes the clamped close take 21 seconds. Fix the URDF (joint_limits.yaml too), rebuild with colcon build, and relaunch.
Grasp consistently lands beside the cube on the real robot.
This is the wrist-family problem from section 8. Check the wrist roll (Revolute 4) in the log or RViz. If it is near -180 degrees, you are in the twisted family. The posture-preferring IK in this package chooses the neutral family wherever it exists. Keep the cube in the front/center zone (positive X, small Y) where the neutral family is available.
Power loss on the robot side during operation.
Restart real_moveit.launch.py with start_at_ros_zero:=true. This re-syncs the physical arm with the software state. The bridge re-applies its speed setting. If the 5V rail sagged during the power event, check that all servos are still responsive before running the task again.
RViz shows the cube at a different position than the real cube.
Check the camera calibration first: ros2 topic echo /camera/camera_info --once should show your focal lengths, not defaults. Then check the marker size in the config. If the printed marker is 30 mm but the config says 31 mm, all distances will be off by about 3%.
13. FAQ
Why ArUco markers instead of a neural network?
Full 6-DoF pose, sub-centimeter accuracy, one cheap camera, zero training, runs on anything without a GPU. For “find the labeled object on my desk,” markers are simply the right engineering answer for this project. If you want to swap in a YOLO detector or a point cloud pipeline later, the interface is just the /cup_pose topic. Any node that publishes a PoseStamped message will work.
Can I skip the simulation and go straight to hardware?
You can, but you probably should not. The Gazebo twin found a broken gripper model (wrong joint limits), a mis-tuned grasp-attach loop (150 ms update latency), and two task-logic bugs (joint-space arcs knocking the cube, cube launching on release) before any servo was at risk. The simulation shares every line of task code with the real run. Bugs found there are bugs fixed everywhere.
Does this work with a different camera height or table?
Yes. That is exactly what the two calibrations absorb. Re-run the intrinsic calibration if the camera model or resolution changes. Re-run the extrinsic calibration (four markers, one command) if the camera mount moves. Re-measure the marker corner positions if the table changes.
Why does the arm refuse to grasp at some spots on the table?
Five joints minus five grasp constraints (position + approach direction) leaves zero slack. Where joint limits bite, there is no vertical-approach solution at all. This is a fundamental limitation of a 5-DOF arm, not a software bug. The task’s reachability pre-check turns that from a mid-motion surprise into an upfront message with a suggested alternative position.
My gripper geometry is different. What do I retune?
Three things: the tcp frame origin in the URDF (set it to your fingertip midpoint), the gripper_open / gripper_closed values (one servo sweep with your eyes), and grasp_z for your object height. Everything else adapts through the URDF-based FK chain.
What happens if the cube is moved during the grasp?
The task reads the cube position once at the start and plans the entire sequence based on that snapshot. It does not track the cube in real time during the descent. If someone moves the cube after the arm starts descending, the arm will close on empty air. For a desktop demo, this is fine. For a production system, you would want continuous tracking and replanning, which is beyond the scope of this tutorial.
Can I use this with a depth camera instead of ArUco markers?
In principle, yes. A depth camera (like the RealSense) gives you point clouds, and you can detect objects by their shape instead of by a marker. The perception pipeline would be completely different (point cloud segmentation instead of ArUco detection), but the IK solver, the pick-and-place task, and the linear descent logic would stay the same. The /cup_pose interface decouples perception from manipulation.
14. Series wrap-up
Four parts, one robot.
Part 1: Built it. Mechanics, electronics, ESP32 firmware, web control. A $50 desktop arm that moves when you drag sliders on your phone. Read Part 1
Part 2: Modeled it. URDF/Xacro, RViz visualization, a Gazebo digital twin with physics simulation. The arm existed in software for the first time. Read Part 2
Part 3: Planned with it. MoveIt 2 motion planning, from mock hardware to real servos over a serial bridge. The arm went where you told it, avoiding collisions along the way. Read Part 3
Part 4: Gave it eyes. Camera calibration, ArUco perception, a custom 5-DOF grasp IK, and a pick-and-place pipeline that survived contact with reality. The arm decided where to go on its own.
The arm on your desk now closes the full robotics loop: sense, plan, act. A camera observes the world, software converts pixels into coordinates, an IK solver computes joint angles, a motion planner finds a collision-free path, and a bridge drives physical servos. Every one of those steps has real failure modes, and you have worked through all of them.
Every package, launch file, calibration tool, and parameter file from this series ships in the complete workspace:
OmArm Zero ROS 2 Workspace — complete package
