Day 5: Scan, find, grab, drop
IT WORKS!! The arm can now scan the grid, find the object, pick it up, and drop it in a bowl on its own. To get there, I had to make the scans repeatable, add a way to measure accuracy, re-tape the sensor after it slipped, rewrite how the arm moves between poses, and make the shutdown safe.
To measure accuracy, every run now saves a truth.yaml file next to its data with the
object's real centre, size, height and rotation in grid mm, and an analysis script compares the scan's
estimate to it. This is possible because, for now, the object is always an Apple USB-C charger (hard plastic, about 4.5 × 4.5 × 2.75
cm), centred in one cell and aligned with the grid.
In the first measured run, the charger was in B3. The height-weighted centroid of all the points the
scan marked as raised was 3.1 mm from the true centre (12 mm unweighted). But the map also showed
raised points in A3 and B4, which were empty. The empty-table points ranged from −11 to
+22 mm (5th to 95th percentile).
The cause was that the arm doesn't land in exactly the same pose twice. I first suspected the grid,
which is drawn slightly rotated (the corner fit already accounts for that), and the tape. To check, I
re-measured the empty table at the same commanded poses five minutes after calibrating. The readings
should have matched, but they were off by ±6.5 mm (IQR), and the error was the same along
each arc: one arc read 10 mm low at every point, and the next read 13 mm high.
- The STS3215 encoders have 4096 steps per turn (about 0.09° each), and my move code counted a joint
as arrived once it was within
10 steps(about 0.9°). - Under gravity, the shoulder and elbow stop slightly short of their targets, by a different amount each time. Sag compensation then applied that offset to the whole arc, which is why the errors were per arc.
- An error of 0.9° at the shoulder or elbow moves the sensor a few mm up or down, and the two add up to about 1 cm, which is a third of the object's height.
- Fix: after every move, the code reads the actual joint angles. If a
joint is short, it re-commands the target plus the shortfall, up to 6 times, until the joint is within
3 steps(about 0.26°).
The scan now decides where the object is while it's scanning, and stops early. The beam is a cone of about 15°, and it returns the closest surface anywhere in a spot about 1–2 cm wide. So near an edge, it detects the object before it's directly above it, and objects look blurred and bigger than they are. Because of that, no single ping decides anything:
- A 5 cm cell counts as the object if at least
35%of its points read above half the object's height, there are at least 3 of those points across 2 or more sweeps, and their median height is consistent with the object's height. - A cell is only judged once the scan has passed all of its points and its neighbouring cells have been scanned too. Then the cell with the highest score (hit fraction × height match) is picked.
I taught the grasp poses by hand at the 4 corner cells (plus a drop pose over the bowl), and the code blends between them for every other cell. To grip, the gripper closes slowly until it stalls, and if it closes all the way, it missed and goes home. I also added a param to disable this for grabbing very soft objects like a Nee-Doh cube.
Day 4: The scan worked!
The first full scan found the object. I scanned the 20 × 20 cm grid from 70 mm up, the 5 × 5 cm object showed up clearly as a raised area, and every reading was valid.
At each of the points, the arm stops, pings, and logs the raw echo time in µs along with the joint angles read back from the servos (not the commanded ones). The height at a point is its empty-table distance from Day 3's calibration pass minus the median of the new pings. Since every point has its own baseline, it doesn't matter that the arm isn't perfectly level across the grid.
height = table_distance[point] − median(distance over the burst)
My first map compared every height to a single table reading from the start point, so the far row read about −20 mm. Using each point's own baseline fixed it.
Next: a real mount (maybe), moving the wires out of the way (I think they cause interference at some angles), and picking the object up.
Day 3: Scan grid, scan path, and height correction
I taped the sensor to the gripper, recorded the arm's joint angles at the corners of the scan area, wrote the scan path, and corrected the arm scanning lower than the target height.
The HC-SR04 is taped flat to the outer face of the fixed jaw, so it faces out the side of the gripper, not out of the tip. To point it straight down, the gripper has to be level with that jaw facing the table. The workspace is a 20 × 20 cm square on the table, split into 16 cells of 5 cm each (rows A–D, with A nearest the base, and columns 1–4).
With torque off, I moved the sensor over each corner of the grid by hand and saved the joint angles at ~6cm scan height.
I tried doing the kinematics with the official SO-101 URDF, but it was off from my taught corners by about 3 cm, and the offsets it needed to fix that were physically impossible (like putting the sensor 335 mm behind the gripper). So the scanning doesn't use a model at all. Instead:
- The base angle comes from the four corners. The base just rotates around a vertical axis, so the corners' pan angles tell me where that axis sits relative to the grid, to within about 5 mm.
- For reach and height, I blend the shoulder, elbow and wrist angles between my near-corner and far-corner poses, depending on how far out the point is.
Before anything moves, the planner checks every point, and the halfway pose of every move, to make sure the arm stays inside the taught area and within its joint limits. The full plan is 195 points over 13 arcs.
During initial scans, the arm scanned lower than I'd taught (about 35 mm instead of 60 mm), for two reasons. At far reach, the shoulder sagged under the arm's weight, so I now command each joint slightly past its target. In the middle of the grid, blending between the corner poses doesn't keep the sensor at a constant height, so I added a calibration pass over the empty table that adjusts the height at each point until the sonar reads 70 mm.
Day 2: Servo ID and tests
I configured the servos, calibrated the arm, tested it very slowly, and tested the ultrasonic sensor.
The servo setup was the standard process. I assigned each servo its ID, calibrated each joint's range (which sets the safety limits for everything after this), and checked the voltage and temperature.
I wired the HC-SR04 to an Elegoo Uno, with trig on pin 11 and echo on pin 10. I kept the firmware
really simple. When the Mac sends a P, the Uno fires one ping and replies with the raw
echo time as an integer. All the bursts, medians and filtering happen in Python, so every raw reading
makes it into the log.
The sensor can't see anything within about 2–3 cm, because the transducer is still ringing from its own burst. I was planning to do the first scans from super low (~7 cm), so I'll need to use very short, flat objects.
Day 1: Assembled!!
I built the arm! It's an SO-101 (six joints: base pan, shoulder lift, elbow flex, wrist flex, wrist
roll, gripper) with Feetech STS3215 servos, the 7.4 V version (wish I went with 12 V). Total cost was
~$150, and assembly took a little over an hour. It would have been faster if I hadn't forgotten a bracket
initially LOL.
I 3D printed the body and got the servos on Amazon (would've been cheaper from other sources, but I wanted a quick lead time).
The ultrasonic sensor will need to be mounted on the wrist, so I'll have to make an extra part and attach that. For early tests, I think I'll just tape it.
Day 0: Napkin sketch + ideas
Inspired by @cdngdev's SO-101 arm drawing the golden gate bridge using GPT-6 Astra, I decided to build my own SO-101 arm. Having not initially ordered a camera, I was very inspired to do some sort of computer-vision-without-vision project - solving the classic "put the cubes in the bowl" problem, but without camera vision. The most obvious answer would've been lidar, but this felt way too simple.
I thought it could be really cool to try this with echolocation. When bats echolocate, the difference in timing and volume between their two ears gives azimuth, and their ear shape carves notches into the returning echo that gives elevation. Bats keep the entire waveform (amplitude, spectrum, the fine timing structure), so one call provides sufficient information for texture, shape, and distinguishing objects, all at 20-120 kHz.
To do this spatial ultrasonic on a robotic arm would be expensive. Getting direction takes several receivers spaced apart, each with its own amplifier and an ADC sampling fast enough to record the echo itself, all running off the same clock. Instead of that, I've opted to use an HC-SR04 from an Arduino kit I bought years ago. It sends a 40 kHz burst and returns a single number representing how long the first echo took to come back. The beam is about 15 degrees wide. From 20 cm above a table's surface, it scans a roughly 5cm x 5cm patch.
The plan: The HC-SR04 mounts near the wrist, pointing down. The arm walks it across a grid over the table, pings at every point, and logs joint angles with each reading. Anything closer than the table is an object. A ping returns one distance and no direction, so the object sits somewhere in a 5cm cone, but overlapping enough adjacent cones makes localization of objects possible.
While waiting for my servos to arrive today (fingers crossed for Tuesday), I wanted to test the HC-SR04 and see how accurate the readings were. I didn't bring a USB-C to USB-A adapter to college, and I only have Macbooks, so I tested via my Rapsberry Pi and ancient Samsung monitor. The sensor is about as accurate as you'd expect it to be for the price point, but I'm fairly certain that it'll work nicely from a scanning distance of ~20cm. The sensor transmits and listens with the same element, so for the ~200 µs the burst takes to go out and stop ringing it can't hear anything, which leads to a blind zone if an object is within a few cm of the sensor. That doesn't matter scanning from 20 cm up, but it does when the wrist comes down to grab something, which is why the last few centimeters will have to run on servo current (I think?) instead of sound.
Next: assembling the arm when the servos get here and figuring out the best way to mount the HC-SR04!