In a effort to keep track of change in Robbies Code a summary of changes and mods we will post a summary of activity
March 2026: Swerve Drive Gets Smarter
The swerve drive looked right on paper but had an annoying judder in practice: wheels would oscillate back and forth when trying to hold a near-zero velocity. The root cause was a missing hysteresis band around the wheel flip decision. When a wheel needs to go, say, 91° from its current position, it can either rotate 91° or flip 180° and go 89° the other way — both are mechanically equivalent. Without a proper dead-band, the controller would keep flip-flopping between the two options near the boundary. Adding a 45°/90° hysteresis band eliminated the oscillation.
Two other quality-of-life improvements went in at the same time: a hardware steering
limit (MAX_STEERING_RAD) that makes the wheel flip rather than clamp when
it hits the physical range, and first-order velocity smoothing
(MOTOR_RAMP_TAU) to take the edge off acceleration. The robot moves
noticeably more smoothly as a result.
Tunable Firmware Over Serial
One thing worth highlighting: we added a maxrate parameter to the ESP32
steering firmware. Previously, adjusting the maximum stepper speed meant changing a
constant, recompiling, and reflashing — a five-minute interruption that breaks your
testing flow. Now you send a single serial command:
echo -e "S 800 200 1000\n" > /dev/espThat sets max rate to 800 steps/s, stall detection timeout to 200ms, and watchdog to 1000ms. The setting is saved to NVS flash and survives a power cycle. To query the current config:
echo -e "q\n" > /dev/espThis sounds like a small thing but makes a real difference when tuning — try a value, drive the robot, adjust, repeat without stopping anything.
Splitting the Arm's Hardware Interface
The SCARA arm had a different kind of conflict. The arm's vertical axis (joint 0) is a
stepper motor driven by a separate ESP32-based controller, while all the other joints
use Feetech/ST serial servos. Originally both were lumped into a single
ros2_control hardware block, which isn't how ros2_control is designed to
work — different hardware interfaces need separate blocks. Splitting them into
_j0 (stepper, via stepper_ros2_driver) and
_servos (Feetech, via feetech_ros2_driver) cleaned this up
properly.
We also added a use_mock_hardware:=true xacro argument so the arm can be
launched in simulation mode without any physical hardware attached — useful for testing
MoveIt2 motion planning on the desk before trusting the real arm.
March 2026: Teaching the Robot to See Low Obstacles
This is the big one from this week, and the one that required the most debugging.
The Problem: The Lidar Is Blind Below 390mm
Robbie's RPLidar is mounted at 390mm above the floor. Its beam is horizontal, so it simply cannot detect anything shorter than that — a 305mm drinks can sitting on the floor is completely invisible to it from a navigation standpoint. This isn't a configuration issue; it's physics.
The RealSense D435 at the front of the robot is mounted at 300mm and has a 57° vertical field of view, so it does see low obstacles clearly. The question was getting that data into the Nav2 costmap properly.
Ghost Obstacles: A 3D Raycasting Problem
We switched the local costmap from a 2D ObstacleLayer to a
VoxelLayer, which can ingest 3D pointcloud data and mark obstacles at the
correct height. The obstacle marking worked fine — slide a box in front of the camera
and it appeared in the costmap immediately. The problem was clearing: when you removed
the box, the mark stayed. Move someone's foot through the space and the footprint
remained permanently. The robot would start navigating around nothing.
The cause was subtle. The VoxelLayer stores a 3D grid of voxels (cubes). We had it configured with 16 height slices of 5cm each, covering 0–80cm. The lidar clears free space by raycasting — sweeping a beam horizontally at 390mm. That beam only intersects voxel slice index 7 (0.35–0.40m). Obstacle points from the RealSense were marking slices 2 through 6 (0.10–0.30m, the height of a typical low obstacle). The lidar's clearing rays passed right over those slices and never touched them. Once marked, they stayed forever.
The fix is almost embarrassingly simple in hindsight: collapse all 16 slices into one single 80cm-tall voxel.
z_voxels: 1
z_resolution: 0.8 # one voxel covering 0–0.8mNow the lidar ray at any height within 0–80cm intersects the single voxel and clears the whole column. The 3D height discrimination is gone, but for a local costmap whose only job is to answer "is there something in the way", that trade-off is fine.
The Wrong Topic Name
While digging through the config we found that the pointcloud topic in
nav2_params.yaml was set to
/front_depth_camera/depth/color/points but the RealSense launch file sets
camera_name: front_camera, making the actual published topic
/front_camera/depth/color/points. The costmap had been silently failing to
subscribe to any pointcloud data at all — which explains why obstacles weren't appearing
in the first place. A frustrating but instructive bug: always verify what topics are
actually being published before assuming your configuration is working.
Fixing the Crab-Walking
With the costmap now working, a different problem became obvious: the robot was navigating like a crab.
On a route that required turning — go forward, turn left, continue — the robot was cutting diagonally across the corner rather than following the path and rotating to face the next leg. Because the RealSense only covers the forward 86°, a robot strafing sideways is effectively navigation-blind in its actual direction of travel. The voxel layer it had just been given was immediately undermined by the robot pointing the camera the wrong way.
This came down to two settings in the MPPI (Model Predictive Path Integral) controller:
-
PathAngleCritic.max_angle_to_furthest: 1.0— this critic only fires when the robot's heading is more than 1.0 radian (57°) off the path direction. So on a 45° diagonal approach the critic was completely silent, and the robot was free to strafe without penalty. Dropping this to 0.4 radians (23°) means the critic activates much earlier and pushes the robot to align with the path. -
PathAlignCritic.use_path_orientations: false— the path planner knows what direction the robot should be facing at every point along the path. With this set to false, MPPI was ignoring that information entirely — just trying to stay near the path line without caring about heading. Flipping it to true gives the controller the directional information it needs.
We also raised PathAngleCritic.cost_weight from 5.0 to 12.0 to make it
the dominant force when the robot is heading the wrong way.
