Project 01 · June to August 2026

5-DOF Autonomous Robotic Arm

A robot built to pick up an object at any location in a given 3D space, with a 100% ground pick rate and an 80% elevated pick rate, running off MATLAB and Python and driving to the serial bus driver.

Ground pick success: 100%
Elevated pick success (8/10): 80%
Picks anywhere it can reach: 3D
Degrees of freedom: 5

SolidWorksSimscape MultibodyMATLABOpenCV / PythonServo control

Results

The three clips below are sample points and not the limit of where the arm can reach. The two pose system solves for a target anywhere inside the arm's reachable volume, at any height, without being told how high the object is sitting. Off-height picks lean on that solved depth instead of a known table plane, and that is where the gap between 100% and 80% comes from. Each trial pairs the physical arm with the OpenCV detection view from the same run.

Ground-level pick

Playback: 2.5× speed

Physical arm (iPhone)

Medium-height pick

Playback: 2.5× speed

Physical arm (iPhone)

OpenCV detection + sim overlay

High pick

Playback: 2.5× speed

Physical arm (iPhone)

OpenCV detection + sim overlay

The descent moves in slow jogs on purpose. It gave us a safety margin against an encoder seam runaway, and the camera needed a close to stationary frame for OpenCV to resolve the brick without motion blur. Clips are sped up 2.5× for viewing.

What fires on every step of the descent

The descent is not one command. It is a loop that re-measures, re-solves and re-commands about every eight millimetres of travel, and this is the chain that runs each time.

The camera finds the brick and we shorten the error vector

OpenCV segments the brick by colour and scores its studs and contour geometry, which gives us a pixel centroid. The loop subtracts that from the aim point to get a 2-D pixel error vector, and then shortens it on purpose. If we commanded the whole correction at once it would overshoot, because the camera is mounted on the wrist, so any move the arm makes changes the same measurement the correction came from. Scaling the vector down to a small step keeps the loop converging instead of oscillating, since every step only moves part of the way and then measures again against a fresh frame.

The pixel step becomes a Cartesian target in the base frame

A probe measures the loop's own gain by jogging a known distance and watching how many pixels the brick moved, which gives us a live mm per pixel scale for whatever posture the arm is in. That turns the shortened pixel vector into millimetres along the arm's real reach and tangential axes, which we found by perturbing a pitch joint instead of reading them off the CAD frame. What comes out is a target point in the base frame, one small step from where the claw already is.

Python hands the target to MATLAB and MATLAB solves the inverse kinematics

The Cartesian target goes over a local TCP socket to a MATLAB server holding the arm's rigid body tree. MATLAB runs inverseKinematics seeded from the arm's current joint angles so it settles on the nearest solution instead of some other legal posture somewhere else in the workspace. A 5-DOF arm has redundant freedom against a 3-DOF position target, so we constrain the solve, holding wrist roll and holding base yaw on radial moves. The angles that come back get checked against measured per joint travel limits and a Cartesian floor guard before anything moves.

Joint angles turn into paced servo commands

The angles convert to raw encoder ticks through each joint's calibrated scale and direction sign, then go out over the serial bus in hops small enough to stay under our travel cap, with a rest in between. We read position back after every hop instead of assuming it arrived, so a stalled or refused joint ends the move instead of getting pushed through, and that pause is what let us stop a wrong move by hand.

The 2 pose system, and why we never have to know the height

One camera looking at one frame cannot get depth. A single image only gives you a ray pointing from the lens toward the brick, and every point along that ray lands on the same pixel. The usual way around it is to assume the object is sitting on a known plane, normally the tabletop, and intersect the ray with that plane. It works on the ground and falls apart the moment the brick is on top of something, because the claw drives to where the table would have been.

The 2 pose system takes that assumption out. The arm captures the brick from one pose, swings the base tangentially to a second pose a known distance away, and captures again. Two rays from two known camera positions intersect at one point in space, and that intersection is the brick's actual 3-D position with the height included. The arm ends up being its own stereo pair, using its own forward kinematics to know how far apart the two viewpoints are.

Two details had to be right for this to actually work. The move has to be tangential, because parallax needs translation across the line of sight, and on this arm the base yaw is the only joint that gives us that. The three parallel pitch joints mostly move the camera along its own view, so they are no help here. The baseline also has to be verified instead of assumed, because near parallel rays turn one pixel of noise into tens of millimetres of depth error. The run reads the joint and the FK tip before and after each leg, and refuses to triangulate if the arm did not actually travel far enough.

What that buys us: the pick target gets solved instead of assumed, so the arm can grab an object sitting anywhere inside its reachable volume, on the table, on a book, or on a raised block, without being told ahead of time how high it is.

Pipeline

Camera

OpenCV brick detection, 2 pose vectors

MATLAB / Simscape

Inverse kinematics over the surveyed chain

Serial bus

Paced, safety-capped servo commands

Physical arm

Closed-loop visual servo descent

How it came together

The inspiration for this project stemmed from when I had seen someone run a robot via machine learning and manual training in tandem. As someone who was curious about MATLAB as well, I was interested to see if I could capture the same results, running purely off of model data and feedback loop simulation.

Starting with SolidWorks, I was able to mock up a basic model of a 5-DOF robot arm with stretched out lever points to include a greater reach, while still utilizing a set of 6 STS3215 motors from Amazon. While I hadn't bought the motors at this step, I was able to find a rough model online where I was then able to dimension out and reverse engineer a model part to use in my assembly.

After learning how to set up an imported SolidWorks assembly into MATLAB, and wiring the power for each joint through trial and error in a bug-filled Simulink environment, I was able to run a simple demo. From here I took myself through researching MATLAB's built in inverse kinematics functions, and how to utilize them for my model. The first framework I put together gave me the bones, but it eventually led to a total of 7 different bugs: scrambled joint mapping with “phantom” joints driven from misinterpreted mates in SolidWorks, incorrect home positions, double-counted joint offsets, incorrect joint weighting, a misplaced end effector, incorrect limit setup, and a From Workspace compile issue. After spending a month learning MATLAB and researching how people handled simulation for my kind of scenario, I was able to eventually work through the issue, and get a basic inverse kinematics model running through Cartesian coordinates.

At this point, with my university starting back up in just over a month, I made the decision to invite my friend Krish Vatsa, who was already comfortable with robotics. He then handled the OpenCV work we had outlined, while I began cleaning up the MATLAB.

Once parts had arrived, knowing that when printing PLA more tolerances were needed, I printed a test part with just the fixture points, and adjusted my model to get ready to print again. After spending a day constructing, we hooked up a live server from MATLAB straight into Python, and set up our serial bus driver.

From here, Sim and Reality began to diverge. The model's numbers were inverted, so the solver was interpreting the incorrect moment arm. One jog moved the claw 30 mm where the model had predicted 13.8, off by a factor of 2.17. We instead decided to opt for a physical test at different points, measuring joint offsets with a ruler.

Hand-eye calibration never came together either. Every solve we ran put the camera 80 to 100 mm from the wrist, against the 24 mm we measured by hand. We stopped trying to trust an absolute camera-to-wrist transform and built the detection framework around relative corrections instead. That included a self updating and correcting vector-shortening model, at the reference of some literature Krish was able to find.

After weeks of trial and error with systems breaking down, motors jumping 180 degrees past limits, torque disabling, and adrenaline filled power cuts, we brought our held-out error down from the CAD model's 73.1 mm to 6 mm, which at the ground pick level left us with still a 100% success rate. Working with a 2 pose system, we were able to achieve an 80% success rate off an elevated surface, working with just 1 camera mounted to dictate height.

What the rebuild bought

This was the biggest single change we made, measured against touch points the model was never fitted to in the first place.

Forward kinematics accuracy, checked on held-out touch points

Imported CAD model73.1 mm

held-out error

Ruler-survey rebuild6.0 mm

held-out error, zero fitting

That is a 12× reduction in positioning error, and it came out of ten ruler measurements with no curve fitting and no reliance on the CAD geometry.

Simulation validation

Before any hardware was involved, I used the Simscape Multibody model imported from the SolidWorks assembly to check the inverse kinematics against target poses in simulation. Everything was solved in MATLAB and checked against a mesh view of the arm before a single part got printed.

Simscape Multibody, early IK validation

Fabrication & build

I modelled the arm in SolidWorks around the servos themselves. I found the motor dimensions online and reverse engineered them into a reference to build the housings and connectors against. Once the kinematics checked out, I sliced and printed the segments on a Creality Ender, then we assembled and wired everything against that reference on the bench.

Under the hood

Safety

A software freeze is the first thing we reach for instead of the power cut, since it holds torque and stops motion without dropping the arm. Travel limits are measured per joint instead of assumed, and the paced stepping leaves a rest between hops so we can catch a bad move by hand.

Closed-loop control

Wrist roll, and base yaw on radial moves, get locked out of the freedom the solver would otherwise spend on them. A pan budget lets tangential base yaw moves use the angle their geometry actually needs, while still capping the radial ones that were only wasting it.

Frame handling

The Simscape import gives you a base frame that is upside down compared to the physical robot. We handle that once at the MATLAB client seam so the rest of the code only ever speaks the physical frame.

View repository

Literature referenced

We did not improvise the hand-eye calibration or the kinematic identification work. Each diagnostic came out of an established result, so here is what we read and what it changed in the code.

01
H. H. Chen, “A screw motion approach to uniqueness analysis of head-eye geometry,” CVPR, 1991.
Screw congruence. AX = XB makes A and B conjugate, so every pose pair has to agree on rotation angle and on screw pitch, but our tooling was only ever testing the angle. Adding the pitch test caught four pose pairs that the angle test had passed cleanly, and one of them had 0.06° of angle error hiding 115 mm of pitch error.
02
Y. Sun and J. M. Hollerbach, “Observability index selection for robot calibration,” IEEE ICRA, 2008.
Observability indices O1 to O4. We used these instead of condition number to gate capture quality, because condition number gives a capture of uniformly tiny rotations a perfect 1.0 and cannot tell a good capture from one where the arm barely moved. Captures are gated on O3 now.
03
F. C. Park and B. J. Martin, “Robot sensor calibration: solving AX = XB on the Euclidean group,” IEEE Transactions on Robotics and Automation, 10(5), 1994.
A closed form AX = XB solution, using an orthogonal Procrustes reduction for rotation and then least squares for translation. This let me solve from explicit motion pairs instead of going through OpenCV's absolute pose interface, which is what made pooling across boards possible and took the solve from 11 constraints up to the roughly 390 the data actually had in it.
04
V. V. Fedorov, Theory of Optimal Experiments, Academic Press, 1972 (Fedorov exchange algorithm).
D-optimal subset selection. We pick pose subsets by greedy seeding plus Fedorov exchange on log determinant instead of on O3, since the determinant criteria are the ones reported as correct under greedy selection. O3 went from 2.03 to 3.55 and the conditioning went from failing to passing.
05
H. Zhuang, S. H. Motaghedi, and Z. S. Roth, “Robot calibration with planar constraints,” IEEE ICRA, 1999.
Planar constraint identifiability. One plane leaves the identification matrix rank deficient, and you need three planes that are not parallel to each other before you can identify anything. That is why we scoped the table plane tool to refute a hypothesis on purpose, instead of claiming it could identify parameters off one flat surface.
06
R. Y. Tsai and R. K. Lenz, “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration,” IEEE Transactions on Robotics and Automation, 5(3), 1989.
The other hand-eye method OpenCV gives you. Solving with both TSAI and PARK and cross checking them is what exposed that our original agreement gate was only comparing translation, which meant both methods could agree on the same badly wrong orientation and still come back reporting ok.

What I took away

This project taught me a lot about the importance of not only having the end to end vision early on, but to also be flexible to work within that vision. Making decisions to scrap certain features, and rewrite frameworks, especially in an era of AI integration is becoming increasingly more important. I used Claude Code to scaffold both the MATLAB framework and the detection framework, and this project really taught us how to work effectively with AI where it was needed, and work things from scratch without the reliance of AI when we had to.

I'm looking forward to continue my research in the applications of simulation at my sophomore year at Georgia Tech, and excited to see where my career will take me.

Built with Krish Vatsa, who handled the OpenCV work we had outlined and found the literature behind the vector-shortening model, while I cleaned up the MATLAB. Calibration, fabrication, and closed-loop control were done together, and integrating the three systems was the hardest and longest part of the project.