ANKITMEHTA

Hierarchical STAG: One Factor Graph From Fleet to Wheels

A warehouse robot answers three questions at once. Which job do I take? Which path through the aisles? What do I send to the wheels right now? Most stacks give each question its own module. H-STAG puts them in one hierarchy of factor graphs in GTSAM. Each level sends a sub-goal down and a summary of what it saw back up.

My latest piece is the control architecture at the bottom of that stack. It has three layers. A decision layer picks a path of grid cells that satisfies a task, such as “visit X1, then X2”. A motion layer turns those cells into a smooth plan every 0.5 s. A feedback layer tracks that plan on a unicycle robot every 0.05 s. The decision layer works on a discrete space. The other two work on continuous states and share one GTSAM graph, solved with Levenberg–Marquardt.

The layers are simple on their own. The hard part is passing information between them. A plan adapter turns cells into motion objectives. Decode turns the motion plan into a reference for the feedback layer. Encode sends the measured position back up. The inspector shows which factor pulls on which variable at each of those hand-offs.

To test all of this I also built the warehouse in NVIDIA Isaac Sim. Pallet trucks and counterbalance AMRs drive through palletizing, staging, drop, marshalling, and outbound dock zones. The scene gives ground truth to check the estimates against.

Conductor: A VLM Planner That Watches Its Own Skills

“Go to the living room, pick up a glass, and place it on the kitchen table.” To a person that is one task. To a Stretch 4 it is a long chain of small steps. The skills that move the robot are local specialists. Each one sees only the current camera frame. It has no map, no memory, and no idea of the full task.

So a VLM planner composes the task from a fixed library of eight primitives. It calls a skill only after a grounder confirms that the object is in view and in reach. Skills are objects that can be stopped, not calls that block. The planner watches each skill while it runs and stops it the moment something is wrong. A monitor decides success by looking. It never trusts the planner or the skill.

Each skill call declares a stop predicate, such as “glass in gripper”. The supervisor checks it every tick while the skill runs. The monitor checks it again on a fresh observation after the skill ends. A VLM look at every frame would cost about a second and real money per check. So supervision has three tiers. A free tier runs at 10 Hz and reads the skill's own status: empty grasp, stall, timeout. A grounding tier runs at 1 Hz and asks if the object is still in view. A VLM tier runs at 0.2 Hz, and only when the planner asks for it. A test asserts that the default run makes zero model calls.

Task memory keeps plans that worked. Each coordinate is replaced by the query that found it, such as “the drinking glass on the coffee table”. The trace says what to do. The live RGB-D image says where. Move the glass and the trace still applies. Global memory keeps success rules and named failure modes with their recoveries, such as an empty grasp or a false claim of success.

I also moved every Stretch 4 sensor stream across Zenoh and ROS 2 DDS, which cut 180 ms from image processing.

Safe Bedside Manipulation in Geriatric Care

A simulated two-arm robot helps a caregiver change an incontinence brief on a care bed. The robot assists. It does not replace the caregiver.

Most safety work bounds the force pushed into a surface. For aging skin that is the wrong quantity. Skin tears from sideways drag well below the push limit. Sliding a brief out from under a body is drag. So I estimate shear in real time and check it against a second sensor in the bed. Furthermore I am attempting to design a care-giving bed that works in conjunction with the robot.

The recovery system also has to know which steps it can undo. Stopping is not always the safe move.

Learned Gap Density Estimation for Reactive Navigation

Dynamic Gap steers a robot through the gaps between pedestrians. Its Kalman filter assumes linear obstacle motion. It also puts false velocity on static obstacles. We have explored using a GRU to correct this behavior.

Density alone is a snapshot. In one collision the chosen gap went from 0.12 to 2.79 in six planning cycles. By then the robot had committed. So I added two features. One is the rate of change of density. The other is aspect ratio, 1/θ, which tells a corridor apart from an opening.

For one experiment, I ran three configurations over 50 episodes each. With the planner config held fixed, the future-density model cut collision events by 30%. The goal rate rose from 53% to 67%. No episode got worse.

Predictions tracked ground truth. Navigation still got worse. Walls carry near-zero density, so minimising it pulled the robot into them. The planner also blew its 10 Hz compute budget and fell back to idling. Neither problem was in the network. Retuning three planner weights cut the mean path from 52.7 m to 30.0 m.

I also rewrote two gap cost functions. The old width term had no ceiling and reached 540 at a 1 mm gap. It now rejects gaps the robot cannot fit through, then prices the clearance that is left.

Decentralised Two-Quadruped Search and Rescue

Two quadrupeds search an 11 m arena of 18 trees for a moving target. There is no central controller and no shared memory. Each agent has its own gait control, eight IR rays, and a private belief. They exchange belief over UDP at 10 Hz and fuse it by covariance intersection.

So far, I've found and fixed six defects. Most originated from unexpected places. One check decided when the robot could say it had found the target. It mixed position uncertainty with speed uncertainty, and speed uncertainty is always far larger. The total never fell below the threshold, so the robot kept reporting a search it had already finished.

Frontier navigation also failed and a fixed waypoint approach replaced it. That works in this arena. It cannot work in an unexplored one so fixing that is the next problem.