Failing more gracefully

My outdoor filming for the project update video was cut short when the machine cut power to the motors, fell down, and one of the legs snapped off.  Fortunately, I already had plenty of footage when that happened, so it didn’t really impact the video.

Robot down
Nice infill shot

First, this demonstrates the not too surprising fact that this particular part of the leg design could use to be improved.  Second, and the topic of this post, is improving what the machine does when the inevitable failure does occur.

What happened — just the facts ma’am

In this particular instance, I had been running the machine outside continuously outside for an hour or so on a relatively warm day.  This iteration of the servos has basically no heatsinking whatsoever on the servo control board.  With the gearboxes, it isn’t necessary for short duration or low power testing.  However in this case, one of the servos eventually reached its temperature limit and entered a fault state.

As implemented now, any individual servo that hits a hard fault immediately cuts all power.  Also, at that time, the overall gait control, upon sensing any servo failure, immediately cuts power to all the other servos too.  As you can imagine, when a machine that weights 10kg loses all power to all joints with a few milliseconds, it falls down pretty hard.

Areas to improve

There are of course many areas of improvement which this event demonstrates.  One, the servos need better thermal properties.  This was known, and I hope to address in the second revision where I can heatsink the controller properly.  Second, the leg should be strong enough to handle falling down.  And third, it would be nice, if it could, if the machine would do something more graceful when continuing on in a controlled manner isn’t possible.

I tackled that third step right now in two phases.  First, I set up an optional communications watchdog in the servo.  If control commands aren’t received in a timely manner, the servo enters a new mode where it merely commands a zero velocity with no position control at all.  This means that if the control software segfaults, or the primary computer goes out to lunch, the machine will gently lower to the ground rather than dropping like a rock.

Second, I modified the control software’s reaction to an individual servo fault.  Now, it commands this new zero-velocity state of all unfaulted servos so as to minimize the amount of damage done to the overall machine.  If only one or two servos have faulted, this will still result in a relatively gradual let down.

Here’s a quick video demonstrating the two failure types:



Bringing up FD-CAN on the STM32G4

To verify that I could make FD-CAN work in the next revision of the moteus controller, I made a simple desk setup between two NUCLEO-G474RE boards.  I started by soldering up some breakout boards for the TCAN334G CAN transceiver I’m planning on using:



And then wired those up with a lot of jumper wires:


After a fair amount of fiddling, bisecting against the ST CUBE example project, and fixing some problems with my STM32G4 support in rules_mbed, I ended up with some 16 byte CAN frames being sent and received with a data rate of ~4Mbit.

A whole frame
A single bit within the data part of the frame

STM32G4 for mbed

While working on the next revision of the moteus controller, I started by bringing up a software toolchain on a NUCLEO-G474RE board.  Unfortunately, even the most recent mbed 5.14 doesn’t yet support that processor.  Thus, I have a half-baked solution which pulls in the ST CUBE sources for that controller into the mbed tree as a patch.

The patch only implements wrappers for the things I care about, so it isn’t a complete solution. Since I am not really using any mbed libraries anymore in any project, that isn’t a whole lot.  Right now I’m just using it for the one function that sets up the board, a linker script, and the pin mappings.  I will probably eventually just make a rules_stm32 and ditch mbed entirely but for now that is more work than it is worth.


OpenOCD for STM32G4

While bringing up an STM32G4 for the new revision of the moteus controller, I wanted to be able to flash and debug the system, and thus needed a working OpenOCD installation.  The NUCLEO-G474RE board has an ST-LINK-V3 debug interface, which no released version of OpenOCD supports, although thankfully that is working just fine at HEAD in git.  However, to make the STM32G4 work I had to pull some patches from the sysprogs/openocd tree.

My resulting work can be found at:

Hopefully the dependent OpenOCD gerrit review will eventually land: which will clear the way for getting G4 support into master there.

For now, I’ll just live with a custom compiled OpenOCD.


mjbots quad A0: October 2019 Roadmap

My last video gave an overview of what I’ve accomplished over the past year.  Now, let me talk about what I’m planning to work on going forward:

I intend to divide my efforts into two parallel tracks.  The first is to demonstrate increased capabilities and continue learning with the existing quad A0, and second is to design and manufacture the next revision of all its major components.

New capabilities and learning

The first, and most important capability I want to develop is an improved gait and locomotion system.  While the moteus servos in the quad A0 are capable of high rate compliant control, the gait engine that I’m using now is still basically the same one that I made for the HerkuleX servos 5 years ago.  It just commands open loop positions to each of the servos and uses no feedback from the platform at all.  This severely limits what the robot can do.  For instance, if the terrain is not level, legs will drag on the ground or it will not walk at all.  The maximum speed is relatively slow and achieving it requires careful tuning of servo-level gains.  While it is more robust than nearly any other open loop 4 legged walker while standing up, even small disturbances can cause it to fall over.

At a minimum, I’d like to switch to controlling the position and force of the endpoint of each leg and switch to a gait engine that takes into account the actual position of each joint during the gait cycle.  That should enable it to maintain good ground contact with all feet that are intended to be in a stance position.  Now, often feet will skip around, as the servo gains are all relatively high to account for the lack of feedback.

The next level would be to take into account an IMU to ensure balanced feet lift offs.  While I have an IMU in this configuration, it currently isn’t usable due to my improvised attempts to improve the overall system update rate.  I’ll need to update some of the hardware to get an IMU again before I work on this part.

New hardware revisions

I have learned a lot with the moteus controller, servo, and the quad A0 over the last year.  I’d like to take that learning and update the components to achieve:

  • Manufacturability: Many of the sub components now are very labor intensive to produce or assemble.
  • Cost: I have in many cases made things much more expensive than they needed to be in order to shave a bit of time off the prototyping process.  Also, I have enough confidence now to get larger volumes of parts, which further reduces cost.
  • Capability: There are many easy areas where the system performance can be greatly improved.

My overall goals are to make a system that is capable and affordable — to provide an ecosystem of parts and systems for researchers and hobbyists to use.  That means keeping everything open source, and providing access to all subcomponents and designs while still being able to provide a complete system that is capable out of the box.

Here’s a quick rundown of the various areas I’m tackling:

Moteus controller

My next revision of this controller will be a slightly bigger change, in that the form factor, the microcontroller, and the connectors used will all be altered.  I’m planning on switching to an STM32G4 for the controller, using daisy-chained connectors for power in addition to communications, switching to FD-CAN for communications, and reworking the form factor to enable all the primary connectors to exit from one edge of the servo.

Also, I’ll take this opportunity to design for automated end of line test.  That means I will leave enough probe points such that a test fixture can validate the correct operation of a board without intervention.

I don’t anticipate the performance to change a whole lot, although I may increase the allowed input voltage.  Even so this will still be a very capable board, with 3 phase current sensing, >=50A peak phase current, >= 400W peak power and a switching and internal control loop rate of >=40kHz.

Moteus servo

The first round of gearbox servos went through many revisions to achieve something that was functional.  That said, while functional, they are fragile, and require about a full man day of assembly time per servo.  I’m working to get pre-production volumes of around 100 units for all the components such that no post machining will be required and the servos can just be assembled together.

While I’m at it, I intend on switching everything to metal from 3d printed plastic, and reducing the overall dimensions.  It is likely the mass will increase a little from the current 410g, but I’m hoping that it won’t be that much compared to the increased strength and rigidity.

Junction board / raspberry pi 3 hat / pre-charge board

I intend to rework the architecture behind the power and data distribution internal to the quad A0.  Currently there is a raspberry pi3 hat, which merely powers the raspberry pi and provides an RS485 interface.  Then a “junction board” splits out the RS485 bus into four separate connectors as well as splitting out power to all four legs and housing the IMU.  Finally, a separate “pre-charge” board ensured that the servos can have power applied safely.

I intend on moving all data distribution and the IMU onto the board that mounts on the host computer.  Thus it will have power in, an IMU, and 4 (or maybe more) CAN bus outputs.  Then, a separate power distribution board will include the pre-charge circuit as well as connectors for power for all four legs.  Simultaneously, I plan on adding an actual power switch, so that I don’t have to keep pulling the battery in and out to cut power.


The quad A0 chassis is a single monolithic 3D printed block.  Actually attaching legs to this is nigh impossible.  It takes a long time, and not all of the joining screws can actually be used because of interference with other pieces.  Also, using the Ryobi style batteries results in a lot of dead space due to the mounting stud.  I am planning on switching this chassis to be printed in multiple components that are assembled together with screws and threaded inserts and switch to a battery form factor that is more compact.

That should also allow for mounting the primary computer inside the chassis, and still leave enough room for a bigger computer, like an NVIDIA Jetson nano.


I’ve already had one failure in the current iteration of the leg that indicates some redesign is necessary.  Also, the two primary brackets that connect the shoulder to the upper leg, and upper leg to lower leg should be made from aluminum instead of being 3d printed.  Those brackets had to be really big and printed in odd orientations in order to be strong enough, and they could be a lot lighter, more convenient, and more cost effective in metal.

Looking forward

Thanks for all of your positive feedback so far, and I look forward to more exciting times executing these plans!

Quadruped robots: One year in!

While I’ve been working to some degree on quadrupedal robots for the last 5 years, it has been just about 1 year since I kicked up my effort a notch, with my post about improved actuators for SMMB.  I figured it was a good time now to produce a video summarizing what I’ve gotten done over the last year:

Concurrently, I’ve posted a “state of the project” text update on, just to get a wider readership.  If you’ve been reading here all along, there won’t be anything terribly new there, but it is a decent summary of where I stand.

Oh, and I’ve decided to give the robot a more memorable name for now.  The “mjbots quad A0”!  Yeah, I’m no marketing whiz, but it will suffice.

Coming up next, is a brief roadmap of where I plan on focusing my efforts in the next 3 to 6 months.


Improved startup shutdown kinematics

Back when I was getting Super Mega Microbot “Junior” ready for Maker Fair Bay Area 2019, I made it minimally self sufficient through a quick hack of adding some PVC pipe that allowed it to manipulate its feet into a known good position while the robot was safely up in the air.

This worked, but had a number of obvious disadvantages.  For one, it looked ugly!  Second, the machine couldn’t squat down very far before getting stranded on the “resting legs”.  I’ve finally gotten around to doing at least a first attempt at something better!

The basic problem is that normal gaits result in having the feet positioned under the robot.  Then, if you want to sit down, you can’t do it because your feet are in the way.  What I’ve done for now is have the robot take some steps to widen its stance so that the feet are out of the way, then sit down.  Standing up is the inverse.  First the feet are positioned in the widened stance, lowered until the machine is at the correct height, then some steps are taken to get into the normal walking gait stance.

At this point, even this is somewhat of a hack, because the gait generation code is only minimally modified from what I had back in 2014.  When it gets modernized, this standing up procedure will need to be re-implemented, but for now this will let me both lose the ugliness, and experiment with things like bigger jumps.

Enjoy seeing it do its thing in the video below:

Improving the moteus update rate, part 4

In part 1, part 2, and part 3, I looked at what was limiting the update rate of the moteus controller when built into a quadruped configuration and how to improve that.  Now, it is time for the final demonstration!

That video was shot with a 150Hz overall update rate.  The plot shows the commanded and actual position of the three joints in the front right leg, although not all to the same vertical scale.  Updating the servos themselves only used about 3.5ms per cycle, but the gait logic used another 1-1.5ms, which made hitting 200Hz not super reliable, thus running at 150Hz.

Future work

I would still like to be able to perform 1kHz full system updates.  These experiments have let me come up with a plan that I think will achieve that with plenty of margin from the servo side in the next revision.

  • Switch the controllers to use FD-CAN:  I had initially not used CAN as the communication mechanism because I didn’t want to be limited to 1Mbps and 8 byte frames.  However, recent STM32 controllers come with FD-CAN support, which allows up to 64 byte frames and 8Mbps.  The hardware FD-CAN receiver implements CRCs for free, should be more reliable, and manages some amount of pre-filtering and processing, which should further reduce the turnaround time of querying a device.
  • Integrate with the host computer over SPI: While I was able to make serial work by busy loop polling on a dedicated CPU, the SPI bus has an even higher possible bitrate and even if its kernel driver is just as problematic, it can still be polled in the same way.
  • Operate 4 separate busses:  This will be done by having probably 2 STM32’s on the host computer daughterboard, each managing two busses.  This way each leg will have its own CAN bus.


Improving the moteus update rate, part 3

Back in part 1 and part 2, I looked at problems that limited the rate at which the host computer could command the full quadruped and some of the solutions.  Now, in part 3, I’ll cover more of the solution.

More solution steps

Previously, I switched to using PREEMPT_RT, switched bridging strategies, and optimized the turnaround of the individual servo.  Now, I’ll move on to optimizing the host software.

4. Host C++ software micro-optimizations

The primary contributor in the host software to the overall update rate is the time it takes to turn around from receiving a reply from one servo, to sending the next command.  I first did some easy micro-optimizations which came up in profiling.

  • error_code: My implementation of error_code with strings attached was doing lots of string manipulation even when no one asked for it.  Fixing that saved a fair amount of time throughout.
  • Memory allocation: There were a few sites in the code that generated packets where a persistent buffer could be used each time, instead of having to allocate a fresh one.

5. boost::asio

The host software was using boost::asio to interact with the serial port.  It is high performance for what it does, allowing multiple external operations to happen in the same thread, but necessarily relies on an epoll loop and non-blocking write operations.  These aren’t particularly fast in the linux kernel, and the best turnaround time on the rpi I could achieve with asio was around 80us.

I implemented a standalone proof of concept which just uses a single thread to read and write to the serial port in a blocking manner.  Doing that allowed me to get the turnaround down to around 30us.

6. Protocol design

The register protocol that is used for high rate control had one opcode for setting a single register, and another for setting multiple consecutive ones.  The multiple consecutive one requires an additional byte to identify how many registers are set.  The same thing is true for queries.

I shaved a byte off of the common case of both by allowing writes and reads of up to 3 registers to encode their length in the primary opcode.

With that change, the full query packet to each servo was 23 bytes, and the full reply packet was 28 bytes.

7. Linux serial driver latencies

With the blocking thread approach from step #5 and that thread set to real-time priority, the average turnaround was indeed 30us.  However, when run in the full control software (which does other processing as well), occasionally latencies would be in the several millisecond range.  Also, since the PL011 on the raspberry pi only has a 16 byte FIFO, reading any frames larger than that was unreliable, as the kernel didn’t always get around to servicing things fast enough.  Even with sub-16 byte frames, and a blocking reader, the kernel would still delay reads by a millisecond or so sometimes.  I believe this is because the PL011 only provides interrupt notification on even 4 byte boundaries of its FIFO, and otherwise relies on the kernel to poll it.

Well, I can poll too, so I fixed this by just disabling the kernel’s serial driver, opening up “/dev/mem” and polling the IO memory of the controller manually in a busy loop from a RT thread.  This let me get turnarounds down to 4us, and also let me receive packets of arbitrary length without loss.  See and

8. Linux scheduling latencies

With the above changes, the serial port was doing fine, but linux still had problems scheduling the primary process to run with less than 1ms precision, even when it was RT priority.  To solve that, and make the serial thread a bit better performing too, I used the “isolcpus” feature of linux to exclude 2 of the 4 raspberry pi processors from normal scheduling.  Then the main thread of the application got processor 3, and the serial thread got processor 4.  With those changes, the time required to poll the full set of 12 servos is rock solid.  Here’s a plot showing the cycle time required to poll a full set of telemetry data  (but not command them, which adds a bit more time per query but doesn’t affect the variability).


Next steps

And in the next and final post of this series, I’ll demonstrate the final result, showing all of these changes are integrated into the primary control software.