Engineering a Wheeled Biped
From first principles to real-time control on the SG2000.
Edition prepared 2026-10-02, revised 2026-11-07. Synthetic examples are not hardware calibration.
Contents
- 0. How to use this book
- 1. The robot as a complete feedback system
- 2. Getting started on the development computer
- 3. Electronics and wiring
- 4. Mechanical design and fabrication
- 5. Kinematics, center of mass and changing inertia
- 6. Sampling and state estimation
- 7. Dynamics and the linear quadratic regulator
- 8. Reading the balance controller as an executable specification
- 9. Real-time firmware and deterministic execution
- 10. Shared memory, ownership and hardware mailboxes
- 11. Porting the library to the SG2000
- 12. Linux, ROS2 and the supervisory layer
- 13. Simulation, regression tests and evidence
- 14. Commissioning and tuning through measured stages
- 15. Advanced control and research directions
- 16. Troubleshooting, maintenance and reproducibility
- Appendix A. Glossary, units and notation
- Appendix B. Configuration and laboratory records
- Appendix C. Primary references and further reading
0. How to use this book
This book teaches the engineering of a small wheeled robot that balances upright while changing its leg geometry. The intended computer is a Milk-V Duo S with an SG2000, whose C906 little core is the robot's only microcontroller. The selected IMU is a BMI323. The wheels are iPower GM4108H-120T direct-drive gimbal motors driven by DRV8313 bridges on the carrier, each with the AS5048A rotor angle sensor it ships with, on SPI. Each knee is a DS3240 servo with the same AS5048A board as its joint sensor, on the same SPI bus. A carrier board is proposed in the hardware directory. The mechanical design, the built board and the board software integration are still unproven.
You can study and run the first software experiments without buying or powering any robot hardware. Later chapters deliberately require measurements before proceeding: a controller cannot infer motor torque constants, link geometry or current capacity from a photograph.
What you will be able to do
The learning path begins with coordinate systems, units and feedback. It then develops the robot's power and mechanical systems, derives a wheeled inverted-pendulum model, and explains an implementation that evaluates a small control law at a fixed frequency. From there it moves into communication across unlike processors, Linux/ROS2 integration, experimental validation, and advanced control methods.
The central skill is connecting an abstraction to evidence. A variable named height might denote a leg's end-to-end distance rather than the center of mass above the axle. A PWM value might approximate voltage rather than torque. A test that passes on a desktop might verify arithmetic while telling you nothing about interrupt latency. Learning to recognize these distinctions prevents errors that otherwise look like mysterious tuning problems.
Four kinds of statements
Throughout the book, interpret technical material using these categories:
| Category | Meaning in this project |
|---|---|
| Implemented | The behavior can be traced to a source file in this repository |
| Demonstrated on the host | A compiler or software test has exercised a particular property on the development computer |
| Illustrative | A worked example uses invented parameters to teach a calculation |
| Proposed integration | The chapter specifies work needed to connect the portable code to actual hardware or ROS2 |
An implemented feature can still lack target validation. A proposed method can be technically reasonable without being available in the repository. Numerical examples and test gains are not a robot calibration file.
The current deliverables are a portable C library, host regression tests, an offline gain-design tool, a concept image and a simple OpenSCAD packaging model. There is no linked SG2000 firmware image, native I2C/PWM board adapter, complete Linux IPC driver, ROS2 package or verified physical balance result. The book explains how to build those layers without presenting them as completed.
Reading paths
A beginner should read Chapters 1–8 in order, doing the unit conversions and host exercises. Read Chapters 9–12 next to understand where the control code will run. Chapters 13–14 explain how to gather evidence before making the robot support itself.
An embedded developer can start with Chapters 2, 8–11 and 13, then return to the model derivation. A controls researcher should read the coordinate conventions in Chapter 1, the geometry in Chapter 5, the model and implementation in Chapters 6–8, and the limits discussed in Chapter 15. Someone concentrating on fabrication should work through Chapters 3–5 and 14 before turning the concept into motorized hardware.
Do not skip the implementation walkthrough because you already know LQR. This implementation includes position-reference clipping, saturation, arming conditions, a geometry gate and timeout behavior that are not represented by the simple equation u = -K e.
Mathematical notation
We use SI units in calculations and software: metres, seconds, kilograms, radians, newtons and newton-metres. CAD uses millimetres, so lengths from CAD must be multiplied by 0.001 before entering controller parameters. Angles displayed in degrees must be multiplied by π/180 before entering a field that expects radians.
A dot denotes a time derivative. For example, θ̇ is pitch rate in rad/s and ẍ is forward acceleration in m/s². A subscript k denotes a sample; x[k+1] is the state one control interval after x[k]. Boldness is not required to distinguish vectors: a chapter defines their elements explicitly before using them.
The symbol l denotes hip-to-wheel leg length; h denotes the modeled body center-of-mass distance above the wheel axle. They are not interchangeable. I denotes rotational inertia about the body center of mass unless explicitly qualified. u is the sum of the two wheel torques. A per-wheel limit is therefore not a limit on u unless multiplied by two.
Every numerical example should be read with its stated assumptions. If a calculation ignores friction or treats a mechanism as rigid, it is answering a limited question. The next engineering step is to assess the size and consequences of that approximation.
How to work with the repository
Keep the chapter next to the relevant source file in your editor. Trace one value all the way through the system: start with a sensor register, convert it to SI units, rotate it into the body frame, pass it through the estimator, put it into the controller state, and follow the resulting torque through the motor conversion. This is more effective than reading every source file without a question in mind.
Maintain a laboratory notebook with configuration, firmware revision, mechanical revision, observations and raw data for each experiment. An unexplained improvement is less useful than a small, repeatable result whose conditions are recorded. Change one experimentally important factor at a time until your measurement system is dependable.
The exercises include short answers so you can check your interpretation immediately. Many engineering exercises intentionally do not have one numerical answer: selecting a deadline budget, for example, requires measurements of the actual target. In those cases the answer identifies the evidence needed to justify a decision.
First exercise
A drawing gives a wheel diameter of 80 mm. A software field is named wheel_radius and expects metres. What number belongs there?
Answer: 0.040 m. Entering 80 introduces both a diameter/radius error and a millimetre/metre error. Such mistakes can leave a program syntactically valid while making every physical prediction wrong.
1. The robot as a complete feedback system
Prerequisites: basic arithmetic and Chapter 0.
Learning objectives: explain balance as feedback, separate the responsibilities of the processors, interpret the robot's frames and identify which components remain to be designed.
1.1 Why two wheels need active balance
A stationary two-wheel robot has a narrow support region in its forward direction. If its body leans forward, gravity tends to increase that lean. Moving the wheels forward can bring their contact line beneath the body again. The controller repeatedly estimates the lean and commands wheel torque so that the combined motion returns toward an upright operating point.
This is an unstable equilibrium: an exactly upright mathematical state can remain still, but small disturbances grow without correction. A controller needs to respond before the available wheel torque, speed or floor traction becomes insufficient. A faster computer cannot compensate for a motor that cannot generate the required acceleration.
The robot has several related objectives. It must regulate pitch, follow a requested forward speed, turn through differential wheel motion, and change its body geometry with the knees. These objectives compete for actuator capability. Turning consumes some wheel torque that might otherwise arrest a fall. Raising or lowering the body changes inertia and can excite motion that a fixed-geometry model does not describe.
For this reason the implementation prioritizes balance torque over yaw torque. It also limits knee motion rates and schedules the feedback gains by verified leg length. Those are design choices, not universal solutions for every mechanism.
1.2 The information and power paths
commands / mode / requested leg length
operator -> Linux + ROS2 -> command ring -> C906 controller
mailbox |
v
IMU + wheel encoders + joint geometry -> estimator -> feedback
|
v
wheel torque targets + knee position targets
|
motor/servo board adapter
|
v
robot
|
state ring + mailbox -> Linux -> odometry / display
This diagram shows information. Electrical power follows a different path: a battery or regulated supply feeds the motors, servos and computer through suitable power distribution. GPIO pins communicate logic levels; they do not supply wheel-motor power. Chapter 3 keeps the power and signal diagrams separate so that this distinction remains visible.
The controller closes the fast loop locally. A delayed Wi-Fi packet should not delay the next IMU update. Linux provides high-level intent and processes state, while the real-time side checks whether that intent is fresh and compatible with its operating conditions.
1.3 What AMP means here
Asymmetric multiprocessing means the processors have different roles and may run different operating systems. The desired arrangement is ARM64 Linux on the selected large processor and a C906 running bare-metal or FreeRTOS control. The large-core ARM/RISC-V boot choice is a board/platform configuration, not a request to run both alternatives as two Linux CPUs. Verify the selected boot image and the small-core firmware together. Milk-V describes the selectable architecture and board interfaces in its Duo S documentation.
Shared memory stores command and telemetry packets. Hardware mailboxes act as doorbells telling the other processor that useful work may be available. The packet is not carried over an external UART, and the mailbox is not assumed to hold the full sensor record. Sharing a physical address is insufficient by itself: memory attributes, barriers, ownership and initialization must agree on both sides. Chapters 10–11 explain that contract.
Linux is not literally executing ROS2 alone. It still runs the kernel, device drivers and supporting services. The intended division is that application-level balancing and sensor fusion do not depend on Linux scheduling. Keep the Linux workload small until the board's available CPU and memory have been measured under realistic operation.
1.4 Coordinate frames and signs
The control body frame is right-handed: x points forward, y left and z up. A positive yaw about z turns the robot left. A positive pitch about y tips the top of the robot forward. A positive roll about x raises its left side. Accelerometer specific force reads approximately +g on body z when the stationary robot is upright.
Both wheel positions are defined positive for forward vehicle travel. Mirrored motor wiring and mirrored encoders can make their raw electrical directions opposite; the board adapter must normalize them. With track width b,
forward speed v = (v_left + v_right) / 2
yaw rate ω = (v_right - v_left) / b
When the right wheel moves faster, the vehicle curves left, giving positive yaw. Test that sign physically before interpreting odometry or yaw feedback.
The CAD drawing uses X lateral, Y forward and Z up. Assigning positive CAD X to robot-right gives the mapping x_body=Y_CAD, y_body=-X_CAD, z_body=Z_CAD. A model does not acquire ROS frame conventions merely because it resembles a robot.
1.5 State, measurement, reference and output
A measurement is a quantity provided by a sensor or validated mechanism assumption. A state estimate combines measurements and models, such as a filtered pitch. A reference describes desired behavior, such as 0.2 m/s forward motion. An output is the instruction sent toward an actuator.
| Quantity | Example | Where it enters this project |
|---|---|---|
| Raw sensor reading | BMI323 signed integer gyro sample | Decoder and board adapter |
| Calibrated measurement | Body gyro rate in rad/s | bal_sample |
| Estimated state | Pitch and roll | bal_controller |
| Reference | Requested speed and leg length | bal_command |
| Control result | Per-wheel torque in Nm | bal_output |
| Electrical command | Signed PWM duty | Actuator adapter |
A hobby servo command is a reference to its internal position loop. Unless position feedback is exposed, that command is not a measurement of knee angle. Setting a validity flag does not make it one. The supplied controller therefore requires geometry_verified before using the geometry for balance.
1.6 Inventory of what exists
| Layer | Present in the repository | Remaining integration |
|---|---|---|
| Geometry | Two-link distance/IK and CAD layout | Actual constrained leg and measured joints |
| Estimation | Complementary pitch/roll filter | Calibration, mounting rotation, physical validation |
| Balance | Scheduled four-state feedback | Identified plant, stable gains, actuator validation |
| Runtime | Bounded callback orchestration | C906 timer, interrupt priorities, watchdog wiring |
| IPC | Three SPSC shared-memory rings: commands, state, diagnostics (ABI version 2) | Reserved memory, barriers, hardware mailbox adapter |
| Devices and drive | BMI323/AS5048A/ADS1115 decoding, servo pulse map, voltage-mode FOC with voltage-limited torque, 10 kHz drive loop with per-wheel dropout coasting | SPI3 frames to the rotor and knee sensors, native I2C transfers to the IMU and ADC, PWM registers, fault inputs (port skeleton) |
| Linux | Integration specification | Kernel-facing transport and ROS2 node |
| Mechanics | Render and printable layout blanks | Motor/servo mounts, bearings, load paths and hip guidance |
The control tests use 150 mm upper and lower links. The CAD mockup uses 80 mm links. A straight 80+80 mm leg reaches only 160 mm end to end, so the tests' 160–280 mm operating range cannot describe the CAD mechanism. Treat both as teaching artifacts until a single physical design is finalized.
1.7 Modes describe intent, not completed behaviors
BAL_DISABLED and BAL_RECOVERY both inhibit the outputs in the current implementation. BAL_STAND and BAL_CROUCH request zero forward/yaw speed; STAND follows the supplied height target, CROUCH lowers to min_height whatever is commanded. BAL_DRIVE permits speed and yaw references. “Stand” does not implement a floor-to-upright maneuver; “recovery” does not right a fallen robot. The Linux supervisor and future mechanical design must define those additional behaviors if needed.
This nomenclature matters because software state names can create false expectations. A state machine is useful only when its entry conditions, allowed outputs and exit conditions are explicit.
Exercises
- At
v_left=0.10 m/s,v_right=0.14 m/sandb=0.20 m, find speed and yaw rate. Answer: 0.12 m/s and +0.20 rad/s. - Why cannot knee command angle replace knee feedback? Answer: load, lag, backlash, calibration error or a stalled servo can make actual position differ from the requested angle.
- Does sharing RAM make a mailbox unnecessary? Answer: periodic polling can discover data, but mailbox notification reduces discovery latency; neither mechanism replaces the ownership and visibility protocol.
- What is the first system-level experiment? Answer: reproduce the host build and identify each quantity's units and sign before attempting hardware control.
2. Getting started on the development computer
Prerequisites: system overview, a terminal and basic familiarity with files.
Learning objectives: build the library, run meaningful host checks, navigate the source, execute an offline control-design example and understand what these results establish.
2.1 Begin without the robot
The portable library is ordinary C11 linked with the math library. It does not require ROS2 or the SG2000 SDK for host testing. This separation makes it possible to inspect the control equations and error paths before debugging a cross-compiler, peripheral bus or boot image.
You need a C compiler compatible with the project's GCC/Clang-style warning options and CMake 3.16 or newer. Python with NumPy and SciPy is needed for offline LQR design. OpenSCAD is optional for editing the mechanical layout. These are separate tools for separate artifacts; installing one does not install the others.
From the repository root, run:
cmake -S . -B build/book -DCMAKE_BUILD_TYPE=Debug
cmake --build build/book
ctest --test-dir build/book --output-on-failure
CMake generates build rules; the build command compiles the library and two test executables; CTest runs both. balance_tests contains many test functions even though CTest reports it as one test. balance_closed_loop runs the C runtime, drive and FOC against a simulated wheel-pendulum (see simulation and testing). A successful result verifies that the current host compiler accepts the code and that the assertions exercised by those executables pass.
The source deliberately rejects warnings through -Werror. Treat a new warning as information to investigate rather than immediately weakening the compiler options. A cross-compiler may expose a portability assumption that the host compiler did not.
2.2 Read the repository by responsibility
| File | Question it answers |
|---|---|
| control.h | What are the units, inputs, configuration and outputs? |
| control.c | How are states estimated and control outputs computed? |
| runtime.h | Which hardware services must the board provide? |
| runtime.c | What happens once per periodic control invocation? |
| ipc.h | What is the cross-processor wire layout? |
| ipc.c | How is slot ownership transferred? |
| devices.c | How do raw sensor data and motor commands change representation? |
| test_balance.c | Which example behaviors are currently checked? |
| design_lqr.py | How are feedback gains generated offline? |
| robot_starter.scad | Which provisional shapes make up the packaging mockup? |
Read headers before implementations. In embedded work, an interface's timing and unit requirements are as important as its function name. For example, sample_try must provide a consistent snapshot without waiting. A function with the correct C signature but a blocking I2C transaction inside it violates that interface.
2.3 Use tests as executable questions
test_kinematics asks whether inverse geometry followed by forward geometry returns the requested distance. test_control exercises torque direction, invalid samples, reset and command timeout. Other functions exercise index wrap, full rings, unavailable mailbox notifications, device conversions and sensor-gap recovery.
The synthetic gains in these tests were chosen to exercise algebra and output limits. Their existence does not imply that the corresponding physical robot has been modeled or that a closed-loop simulation passes. Do not copy the test config() into firmware and set commissioned=true as a substitute for identification.
To study a test, write down its starting state, stimulus and assertion. For instance, after setting a small positive pitch and disabling accelerometer correction through the gate, the test expects positive corrective wheel torque. That establishes a sign convention inside the program. A separate motor/encoder bench experiment must establish the same convention outside the program.
2.4 Check memory and undefined behavior
On a host compiler that supports the sanitizers, build a separate directory:
cmake -S . -B build/book-sanitize -DCMAKE_BUILD_TYPE=Debug \
-DCMAKE_C_FLAGS='-fsanitize=address,undefined -fno-omit-frame-pointer'
cmake --build build/book-sanitize
ctest --test-dir build/book-sanitize --output-on-failure
Address and undefined-behavior instrumentation can expose memory errors and certain invalid language operations on executed paths. It does not model SG2000 cache coherency, verify motor limits, or establish race freedom between real processor cores. The IPC host test is sequential and uses host fences; its result is not a hardware coherence test.
Use separate build directories so instrumentation cannot accidentally become part of a target build. Record the compiler and command alongside reported results. A claim such as “sanitizers passed” is incomplete without knowing which program and scenarios ran.
2.5 Run the offline design example
The book includes a deliberately synthetic input file:
python3 tools/control/design_lqr.py docs/book/examples/illustrative_plant.json
It specifies a 2.5 ms control period, wheel radius, effective base mass, body mass, positive state/effort weights, and three geometry knots containing leg length, CoM height and body pitch inertia. The output is three C initializer rows for a gain table. They are learning output, not production gains.
If Python reports a missing package, create a project-specific environment and install the book's optional dependencies there:
python3 -m venv .venv-book
.venv-book/bin/python -m pip install -r docs/book/requirements.txt
.venv-book/bin/python tools/control/design_lqr.py docs/book/examples/illustrative_plant.json
The dependency file declares the libraries needed by the examples and book generator; it is not a frozen, fully reproducible scientific environment. Record installed versions with your experiment. For a released robot configuration, preserve a tested lockfile or environment manifest as well as the plant input.
Change one hypothetical parameter, such as CoM height, and examine how the gain rows change. This reveals that LQR is a model-dependent calculation. It does not discover unknown mechanics from the IMU automatically. Chapters 5 and 7 explain how measurements become the input file.
2.6 Three different builds
A host build produces a library and executable for your development machine. A target firmware build uses the SDK's RISC-V compiler, linker script, startup code, RTOS and peripheral support to produce a small-core image. A Linux/ROS2 build produces ARM64 user-space software and possibly a kernel module matched to the selected kernel.
These are different output artifacts. Copying build/book/balance_tests onto the board does not transform it into C906 firmware. Likewise, a generic Ubuntu ARM64 root filesystem does not supply the correct bootloader, device tree, reserved memory or small-core launch procedure for the Duo S.
For a newcomer, the practical milestone order is: host tests; known-good board boot; observation-only sensor acquisition; measured actuator behavior with balance inhibited; cross-core transport; validated model; supervised balancing. Each milestone reduces the number of unknowns in the next.
2.7 Inspect the mechanical concept
Open the SCAD file and select part="assembly" to inspect the layout, then a specific component such as upper_link to view a flat printable blank. The render image is an illustration; it is not a rendering of exact production CAD. The SCAD assembly contains reference envelopes for purchased parts and mathematically positions the legs without supplying a complete constrained hip mechanism.
Treat the first print as a fit experiment. Measure holes, thickness, warping and clearances rather than immediately adding motors. Read Chapter 4 and the existing printing notes before assuming a display model can carry dynamic loads.
Exercises
- Why keep host and sanitizer build directories separate? Answer: build flags and artifacts remain explicit, and instrumented binaries cannot be mistaken for normal or target outputs.
- What does one passing CTest test mean here? Answer: one executable ran its internal assertions; it does not mean every required system behavior has one independently reported test.
- Why does a gain table require three different inertia measurements? Answer: body geometry changes with leg configuration; each knot must describe its operating point rather than reuse an unjustified constant.
- If a function allocates no memory but waits forever for I2C, is it real-time suitable? Answer: no. Bounded completion time is the essential requirement.
3. Electronics and wiring
Previous: Getting started · Book contents · Next: Mechanical design
Prerequisites and learning objectives
Read the system overview and build the host project first. You need basic familiarity with voltage, current, resistance, and a multimeter; this chapter develops the connections to motor control. By the end, you should be able to draw the power tree, explain why voltage-mode FOC is not measured torque, convert encoder counts into SI units, and specify a nonblocking IMU acquisition path.
The selected parts are a Milk-V Duo S, a BMI323 IMU, two iPower GM4108H-120T direct-drive gimbal motors driven by two DRV8313 bridges on the carrier, the AS5048A magnetic angle sensor board that ships on each motor (SPI), the same AS5048A board on each knee axis, and two DS3240 servos that set the knee angle. In the printed body (design/body_v1) each servo drives its hip, and the leg linkage makes the knee follow at twice the hip angle. The proposed carrier board connects them; its schematic, pin map and power budget are in the hardware proposal. The board is unbuilt, so treat every assignment as a design to verify. The implemented functions are in devices.c, foc.c and drive.c; the board callbacks are contracts in runtime.h.
3.1 Separate energy paths from information paths
The v0.8 carrier layout is complete: four layers, 100 × 66 mm, 219 footprints and checked fabrication outputs. It remains unbuilt and bench qualification is pending. v0.6 added two accessory power outputs to v0.5: J24 gives the protected battery voltage through a 2.5 A eFuse that follows the E-stop by default, and J25 gives the 5 V logic rail through a 0.5 A switch for an accessory microcontroller. v0.7 only corrects a silkscreen label and adds 3D models. v0.8 changes parts so JLCPCB can assemble the board from LCSC stock: 16 V output capacitors with one more on each buck, a NANO2 battery fuse and low-ESR bulk electrolytics; the The BMI323 is on the carrier BOM (LCSC C5368700) and is assembled with the board. J16/J18 form the upper SPI bank and J15/J17 the lower bank. J23 adds a general 3.3 V display connection on the IMU/ADC I²C bus; optional display work must not block IMU acquisition. Use the display guide and bring-up procedure. The active improvement plan distinguishes proposed hardware changes from the completed routing and measurements still needed.
A microprocessor GPIO conveys a logic state. It cannot supply a wheel motor's mechanical power. A motor driver uses that small signal to switch a much larger current from its motor supply. Likewise, a servo signal carries a position request while separate conductors supply the servo's motor and electronics.
Begin with a power tree:
Signal paths are separate: the RTOS commands wheel PWM/enable and servo position, and receives BMI323 I2C data/data-ready signals. The figure shows power distribution only.
This is functional architecture; a selected breakout may contain its own regulator or level conversion. Inspect its schematic before connecting it. Distinguish battery voltage when fully charged from its nominal label, and regulator output current from input current. Check all branches at maximum expected load, including simultaneous motor acceleration and servo movement.
Shared logic needs a common voltage reference. Route motor current back to the power distribution point through appropriately sized conductors, so it does not flow through the IMU's ground connection. A shared ground does not mean arbitrary daisy chaining. A short resistance of 0.1 Ω produces a 0.2 V shift at 2 A: enough to corrupt a marginal digital interface or alter an analog measurement.
Place local decoupling at devices and bulk energy storage near the motor supply input according to the selected boards' requirements. A capacitor can support a transient only within its stored charge:
For a hypothetical 1 A step lasting 100 µs, 470 µF changes by roughly 0.21 V before including equivalent series resistance. This calculation shows why capacitance helps short transients and cannot replace a regulator capable of sustained load. Deceleration can return energy to the supply; validate that the battery/regulator arrangement can accommodate this operating condition.
3.2 Make a signal ownership table before assigning pins
The board exposes multiplexed peripherals. A connector label is not proof that a peripheral is free, at the expected logic voltage, or configured for the C906. Milk-V documents different I2C voltage domains on its camera connectors, so do not infer one voltage for every accessible signal. Use the actual board revision's pin table and schematic. Milk-V Duo S hardware documentation
Create a project-specific table like this. The carrier proposal fills the chosen-pin column; mark the last column only after verifying the SDK and device tree:
| Function | Direction at C906 | Required behavior | Proposed pin/peripheral | Linux ownership disabled? |
|---|---|---|---|---|
| IMU SDA/SCL | Bidirectional | Native I2C, correct pull-ups | I2C4 on J4 B1/B2 (1.8 V) via PCA9306 | Unverified |
| IMU data ready | Input | Timestamped interrupt | A20, J3-16 | Unverified |
| Rotor angles | SPI | Two AS5048A, one frame each per fast tick | SPI3 (B13–B15), chip selects B16 and B18 | Unverified |
| Knee angles | SPI | Two AS5048A on the same bus, one frame each every 10th fast tick | SPI3, chip selects B22 (J3-15) and B21 (J3-5) | Unverified |
| Right wheel phases | Output | Three PWM on one controller | PWM1–3 (B11, B12, B20) | Unverified |
| Left wheel phases | Output | Three PWM on one controller, ≥2.2 V high | PWM8–10 (E0–E2, 1.8 V) via SN74LVC8T245 | Unverified |
| Driver enable/fault | Output/input | Default inhibited; fault monitored | MOT_EN_REQ B19, nFAULT A28, nSLEEP/nRESET via translator | Unverified |
| Two servo commands | Output | Hardware pulse generation | PWM6/7 (A18/A19) | Unverified |
| Hardware watchdog kick | Output | Edge every balance tick | B3 via translator → 74LVC1G123 | Unverified |
| E-stop re-arm | Output | One rising edge sets the E-stop latch | C20, J4-36 (1.8 V) via SN74LVC8T245 → 74AUP1G74 clock | Unverified |
Linux and the RTOS must not both reconfigure a control peripheral. Ownership includes its pin mux, clocks, reset, DMA channel, and interrupt routing. These details are completed in platform integration.
Power-up is itself an operating mode. Pull resistors and enable logic should keep the wheels inhibited while pins are inputs, Linux boots, or the RTOS resets. Software cannot correct an unintended motor pulse that occurs before software starts. On the carrier, both DRV8313 EN pins come from MOT_EN_REQ AND WDT_OK AND ESTOP_RUN. Every input has a pull-down or a fail-open loop. WDT_OK comes from an edge-triggered one-shot, so a hung core drops the motors within about 10 ms whether its pin is stuck high or low. ESTOP_RUN is a D flip-flop that the E-stop loop clears directly. Only a deliberate firmware edge sets it again, so a bouncing switch cannot restart the motors. The same signal switches the drivers' supply (VMOT) through a high-side MOSFET, which makes the E-stop a power stop for the wheels and not only a logic stop. A yellow LED shows MOT_EN, and a red one shows a DRV8313 pulling its open-drain nFAULT low.
3.3 BMI323: from bytes to physical vectors
The BMI323 uses VDD of 1.71–3.63 V and VDDIO of 1.08–3.63 V. With SDO tied to GND its 7-bit I2C address is 0x68. Register 0x00 is a 16-bit word whose low byte, the chip ID, is 0x43. Every I2C read inserts two dummy bytes before that payload. The configured ±4 g accelerometer scale is 8192 counts/g. The ±2000 °/s gyro scale is 16.384 counts/(°/s), which is 32768/2000; the register description rounds it to 16.4. bal_imu_decode uses 16.384, and the tests lock it. Registers 0x03–0x08 hold accelerometer xyz then gyro xyz, each a little-endian word. bal_bmi323_pack swaps those two halves into the gyro-then-accelerometer frame the decoder converts. The gyro needs about 30 ms after it is enabled before the samples are valid. Verify the ranges by readback. Bosch BMI323 datasheet
I2C pull-ups connect the bus to a valid logic supply. Multiple breakout pull-ups act in parallel. Too weak a pull-up creates slow rising edges; too strong a pull-up demands excessive low-state sink current. Short, routed wiring and an oscilloscope measurement of the actual bus are more useful than choosing a resistor by habit. A level shifter must support the bus's bidirectional open-drain behavior.
Each axis is a signed, little-endian 16-bit integer. The decoder combines the low byte and high byte, sign extends the result, and applies:
For example, 8192 accelerometer counts represent approximately 9.80665 m/s². A gyro reading of 1640 counts represents 100 °/s, or about 1.745 rad/s. These are decoder examples, not calibration measurements. The implementation does not detect whether a different range was accidentally configured.
The IMU's printed axes need not match the robot. The body convention throughout this book is x forward, y left, z up, with forward pitch a positive rotation about y. The port must rotate sensor vectors into this frame and subtract calibrated gyro bias. At rest on a level support, az should be approximately +g, because the accelerometer measures specific force rather than a gravity vector pointing down. Test every axis by hand before considering balance gains.
Nonblocking initialization and acquisition
Initialization can take milliseconds without blocking a control tick. Represent it as a state machine whose calls perform one bounded action and return:
INHIBITED -> CHECK_ID -> CONFIGURE -> WAIT_POWER_MODE
-> VERIFY_REGISTERS -> COLLECT_STATIONARY_BIAS -> READY
any error -> SENSOR_FAULT, outputs inhibited
WAIT_POWER_MODE stores a deadline; it does not execute a delay loop. Populate deadlines, valid configuration values, and interrupt routing from the selected sensor configuration and datasheet. Configuration traffic occurs before arming. Bias collection accepts only stationary, consistent samples and has a timeout rather than waiting forever.
At runtime, a data-ready interrupt records acquisition time and starts an asynchronous native I2C burst. Completion validates the transaction and publishes a complete snapshot. The controller's sample_try() attempts to obtain that snapshot and returns immediately if none is available. Timestamp the sample's acquisition event, accounting for sensor filtering delay as required, rather than pretending the completion time is the physical measurement time.
A rough bus budget illustrates the distinction between execution time and elapsed transfer time. A register-address write followed by a 12-byte read takes approximately 15 nine-clock byte transfers, or 135 SCL clocks excluding additional timing and stretching. At a hypothetical 400 kHz this is approximately 338 µs; a 400 Hz control period is 2500 µs. The processor should do useful work while those clocks occur. Scope the real transfer and bound timeout/recovery behavior. No transfer may wait indefinitely on a stuck bus.
The existing decoder implements none of this I2C machinery. See sampling and estimation and real-time firmware for the snapshot and timing contracts.
3.4 DRV8313 and voltage-mode FOC: torque without a current sensor
Each DRV8313 on the carrier has three independent half-bridges, one per motor phase. Each INx input sets its phase high or low while EN is high, so three PWM duties synthesize any phase-voltage vector. The logic-high threshold is 2.2 V, which is why the left wheel's 1.8 V PWM pins pass through a translator. Overcurrent protection trips at 3 A minimum and latches until nRESET is pulsed; toggling EN does not clear it. TI DRV8313 datasheet, electrical characteristics and protection The carrier ties the low-side PGND1–3 pins straight to ground, so there is no phase-current measurement.
The GM4108H-120T is a direct-drive gimbal motor with 24 slots and 22 poles, so 11 pole pairs. Its 11.1 Ω winding, taken here as line to line, is a 5.55 Ω star-equivalent phase resistance. The vendor rates it at 0.12–0.18 N·m at 1.5 A, about 0.1 N·m/A. iPower GM4108H-120T Its no-load speed and torque figures do not agree with each other under this model, so measure , and R. There is no gearbox: the rotor angle is the wheel angle relative to the motor's stator.
In the rotor-aligned d–q frame, with electrical speed , star-equivalent phase resistance R and inductance L, the motor obeys
Holding and neglecting the derivative terms gives the voltages bal_foc_torque() applies:
The inverse Park transform rotates by the electrical angle . bal_svpwm() then subtracts the midpoint of the three phase voltages (min/max injection), which keeps the modulation linear up to . Above that, both components scale down together and the output reports saturated; the delivered current is then below the estimate.
The q-axis current is clamped at the motor's rated 1.5 A. With the tests' illustrative N·m/A (for a sinusoidal motor , and the tests use V·s/rad), bal_foc_max_wheel_torque() is 0.45 N·m; the vendor's torque figure suggests nearer 0.15 N·m. The balance controller's per-wheel max_torque must not exceed the measured value.
Voltage is the tighter limit with this winding. At standstill 1.5 A needs V, while the linear limit is 6.9 V on an empty 12 V pack, 8.5 V at a nominal 14.8 V and 9.7 V at 16.8 V; back EMF takes more as the wheel turns. Each fast tick, bal_foc_torque_limits() solves
for the range of q-axis current, clamps it to the current limit and converts it to the wheel torque available forward and backward. Back EMF reduces the torque that accelerates the wheel and adds to the torque that brakes it. The drive publishes both limits, and the balance controller clamps its output and freezes its moving position reference against them, so it never plans with torque the battery cannot deliver.
This is estimated torque. The actual current is , so any error in R, or speed becomes a torque error. Copper resistance rises about 0.39 %/°C, so a 60 °C winding rise makes the estimate roughly 23 % high. Angle error rotates the current away from the q-axis. At 400 rpm the rotor turns about 461 electrical rad/s, so one 100 µs fast tick is about 0.046 electrical rad (2.6°). angle_lead compensates the AS5048A's one-frame pipeline and half a held tick, about 0.15 ms in total. The test values in the repository are illustrative; measure R, and for each motor.
Calibration with the wheel lifted and a low current limit:
- Wire the phases so that a positive increases encoder counts; swap two phases if it does not.
- Apply a small fixed at , let the rotor settle, and record .
- Measure R with a four-wire meter or a DC step, and from the line voltage while spinning the motor by hand.
- Raise the limit gradually while watching DRV8313 temperature and nFAULT. Pulse nRESET only while disarmed.
A hypothetical check: N·m with N·m/A gives A and, at standstill, V. With a high-resistance winding most of the voltage goes into , which is why battery voltage and wheel speed limit the torque so directly.
3.5 Magnetic angle sensors make wheels and knees observable
Each wheel's rotor angle comes from the AS5048A board that ships on the GM4108H with its magnet already aligned. It is a 14-bit sensor, 16384 counts per turn, on SPI. Both sensors share SCK, MOSI and MISO on SPI3, and each has its own chip select. Every 16-bit frame carries an even-parity bit. A read command's reply comes in the next frame, so the fast loop sends READ_ANGLE (0xFFFF) to both sensors every tick and decodes the reply to the previous one. bal_as5048a_decode() accepts a reply only with correct parity and a clear error flag; after an error-flag reply the port sends one CLEAR_ERROR. At start-up, the DIAG register confirms the magnet's field is in range (bal_as5048a_diag_ok()).
SPI makes the rotor read cheap: two 16-bit frames at 2–4 MHz take a few microseconds, so the fast loop runs at 10 kHz. A rejected frame does not stop the wheel. bal_foc_coast() extrapolates its electrical angle from the speed estimate for up to max_encoder_errors frames, and the other wheel is unaffected. The cables run beside motor phase leads switching at about 20 kHz. The carrier terminates SCK and MOSI with 33 Ω at the SoC and puts an ESD array on every cable line at the connectors. Keep the cables short and twisted, and route them away from the phase wires.
The two knee sensors are the same AS5048A boards, with a diametric magnet on the shin, on the same SPI3 bus with chip selects of their own (B22 for the left knee, B21 for the right). A knee moves far more slowly than a rotor, so the port reads them only every 10th fast tick (1 kHz), with the same protocol and the same bal_as5048a_decode(). A rejected reply simply leaves the knee measurement unchanged; if no good reply arrives within knee_timeout_us, the geometry becomes invalid rather than a wrong leg length. At start-up, check DIAG across the whole knee travel, because the magnet's distance can vary as the joint turns. I2C4 now carries only the BMI323 and the rail ADC.
On a wheel rotor the sensor gives the commutation angle and the odometry. With wheel radius r, unwrapped counts, and no gearbox,
With the design wheel's r = 0.04 m that is about 15 µm per count. bal_foc_update_angle() unwraps modulo 16384 and rejects steps larger than max_step_counts. After k coasted ticks it accepts up to times that step, because the wheel kept turning meanwhile. At 10 kHz one count per tick is about 0.15 m/s of wheel speed, so the speed estimate still needs its low-pass filter.
The count measures the rotor relative to its stator, and the stator is fixed to the shin. The shin pitches with the body and, through the leg, with the knee. Ground travel is therefore
with body pitch θ, knee flexion q and shin_pitch_per_knee , which is +0.5 for the forward-folding leg of body v1. It is exact with the 2:1 belt, and holds within 1° with option B's four-bar. bal_step() makes this conversion, and adds to the speed; the port passes the encoder values through unchanged. Without it, a pure body rotation would read as wheel travel and change the effective pitch gains (see the balance controller).
The knee sensors measure joint angle directly, which an ordinary PWM servo does not report. Calibrate knee_zero with the leg straight against a jig, and set knee_sign so flexion is positive. The drive marks knee feedback valid only when calibrated and fresh (knee_timeout_us). The controller compares the measurement with its own target and latches BAL_ACTUATOR if the servo stays more than max_knee_error away for knee_error_ticks ticks. A requested servo angle is still never passed off as a measured one.
Wheel slip still invalidates the no-slip odometry assumption, even when every encoder count is correct.
3.6 Bench diagnostics and exercises
Before powering wheels, inspect continuity, polarity, supply levels, enable defaults, and signal levels. Record IMU stationary data with motors disabled, then compare it while unloaded motors spin. A new noise component suggests wiring, vibration, or power coupling that should be understood before changing filter gains.
| Symptom | First discriminating measurement |
|---|---|
| IMU does not acknowledge | Supply and SCL/SDA voltage; correct 7-bit address |
| Angle jumps during motor startup | Logic rail and ground movement at IMU |
| Correct command but weak torque | Battery voltage, actual current, bridge temperature |
| Robot turns under equal commands | Encoder sign, wheel scale, motor asymmetry |
| Servo movement resets Linux | Servo branch transient and shared return path |
| Sensor fault appears under load | Transaction duration, dropped samples, interrupt latency |
| Encoder faults only while the motors run | AS5048A cable routing; SCK/MISO edges and the parity error count, on a scope next to a phase lead |
- A decoder reports 4096 counts at the configured accelerometer range. What acceleration does that represent? Answer: approximately 4.9033 m/s² on that sensor axis.
- An AS5048A on a direct-drive wheel of 40 mm radius: how far does the wheel roll per count? Answer: 2π × 0.04 / 16384 ≈ 15 µm, assuming no slip.
- Why can half the commanded q-axis current fail to produce half the maximum torque? Answer: without a current sensor, current is inferred from R, k_e, speed and angle; winding temperature, back-EMF error, voltage saturation and angle lag all change the real current.
- An I2C transaction never completes. Should
sample_try()keep retrying until it succeeds? Answer: no; return promptly, preserve bounded timing, and let stale-sensor/fault handling inhibit output.
The deliverable from this chapter is a measured power and signal plan, with every control peripheral assigned to one owner and every physical quantity traceable from hardware to SI units.
4. Mechanical design and fabrication
Previous: Electronics and wiring · Book contents · Next: Kinematics and mass properties
Prerequisites and learning objectives
Read the system overview and examine the printable concept alongside robot_starter.scad. You should know what a force, torque, and pivot are. This chapter teaches how to count the mechanism's independent motions, establish a load path, distinguish packaging from structural design, and turn a parametric model into a measured prototype.
The current CAD is a packaging mockup with printable blanks. It is not a complete knee mechanism, a servo mounting standard, or a strength-qualified assembly. Its generic motor and servo envelopes represent volume reservations. No selected commercial motor, servo, bearing, or shaft has been shown to fit them. The concept image is an illustration separate from the CAD.
4.1 Begin with the physical question
A two-wheel balancing robot has a narrow contact region in the forward direction. If its body leans, the wheels must accelerate to reposition that support beneath the moving mass. Articulated legs add useful capabilities: the body can crouch, adjust clearance, and potentially level across small left/right disturbances. They also add moving mass, compliance, backlash, actuator delay, and configurations with different dynamics.
A productive first prototype freezes those extra motions. Fixed, measured legs let you identify the wheel/body plant and validate estimation, torque sign, and timing. Once fixed-geometry balance is demonstrated, introduce one controlled geometry change at a time. This is an experimental sequence: it gives each failure a smaller set of possible causes.
Decide what motion the knee is meant to create before selecting its servo. Does it change only hip-to-wheel distance? Does it also shift the wheel forward? Does the body maintain orientation while crouching? Those questions determine the linkage and number of actuators. An attractive posed drawing does not answer them.
4.2 Coordinate systems and the existing model
This book uses body x forward, y left, z up. Positive pitch tips the body forward about +y. The OpenSCAD assembly instead uses X lateral, Y forward, Z up. Assigning positive CAD X to the robot's right gives the proper rotation
Thus x_b = Y_c, y_b = −X_c, and z_b = Z_c. Merely swapping X and Y without the sign change creates a reflection, which is not a valid rigid-body rotation. Define the mounting origin separately: axis conversion does not place the IMU or body frame at the axle by itself.
The current default dimensions are:
| Item | CAD value | Engineering interpretation |
|---|---|---|
| Wheel diameter | 80 mm | Reference envelope; effective rolling radius needs measurement |
| Wheel-centre track | 200 mm | Left/right contact geometry for yaw model |
| Overall wheel width | 222 mm | Packaging envelope |
| Upper/lower pivot spacing | 80/80 mm | Kinematic link lengths |
| Body tray | 110 × 75 × 90 mm | Electronics volume |
| Link plate thickness | 6 mm | Provisional printed geometry |
| Tray wall/base | 3 mm | Packaging surfaces, not sized bearing supports |
| Knee pose parameter | 15° upper-link lean | A display parameter, not measured servo angle |
For the equal-length default pose, the knee flexion is 30°, because each link leans 15° away from the hip-to-wheel line on opposite sides. Its hip-to-wheel distance is about 154.55 mm, while total outside height is about 281.55 mm. Neither number is the body's centre-of-mass height.
The CAD computes knee and hip positions so each wheel appears below its hip. This is a geometric placement rule inside a renderer. It supplies no physical constraint to force a real assembly into that pose.
4.3 Count independent motions before buying actuators
Consider one planar leg with a freely rotating hip and freely rotating knee. With the body fixed, two joint angles are independent: the upper-link angle α and knee flexion q. Their two values determine the wheel-centre location in the sagittal x-z plane. A single knee servo supplies only one controlled joint coordinate. It cannot independently determine both the wheel's forward position and its vertical position.
If q is fixed while the hip remains free, the entire bent leg can swing. The distance between hip and wheel may be known, but its direction is not. In particular, a known distance L does not imply a vertical height L. This is why the current scalar inverse kinematics requires a mechanism that guides the hip-to-wheel direction.
There are several possible architectures, each requiring new detailed design:
- A guided linkage can enforce a relation between hip and knee motion, reducing the leg to one independent extension coordinate.
- A guided carriage can constrain the relative motion to a prescribed path, with added friction, guide loading, and packaging cost.
- A hip actuator plus knee actuator can control both planar coordinates, with more sensors, wiring, torque requirements, and a richer model.
- Fixed brackets can remove joint motion for initial balancing experiments.
A generic four-bar linkage is not automatically a vertical guide. Its link lengths and attachment points define a trajectory that must be derived and checked. Similarly, attaching a servo horn to a plate does not create a second independent bearing support.
“Biarticular” has a specific mechanical meaning: an actuator or transmission influences two joints. Two articulated links alone do not make a biarticular drive. The current repository implements a two-link leg-length relationship; it does not define a tendon, differential, or coupled actuator spanning hip and knee.
4.4 Draw the load path
A load path describes how forces travel through the assembly. For a static upright pose, begin at the wheel-ground contact, pass through the wheel hub and axle support, lower link, knee joint, upper link, and hip support into the body. Then include the battery, electronics, and covers as applied loads. Each interface needs a means to carry force, bending moment, and any relevant torsion.
The wheel shaft sees more than motor torque. A wheel displaced sideways from its bearing produces bending. For a hypothetical 10 N wheel reaction with a 30 mm overhang,
This moment loads the shaft and its supports even when the motor is unpowered. A small gearmotor's output bearing may not be intended to carry the entire robot's cantilevered load. Depending on the selected hardware, a separate hub bearing and supported axle can route structural loads away from the gearbox.
At a knee, support the pivot with bearings or bushings and a retained shaft. A servo output spline supplies actuation torque; whether it can also carry radial and axial loads is a property of that servo and mounting arrangement. A bracket supported on both sides can reduce cantilever loading, but it still needs alignment and dimensional control.
A simple double-shear calculation illustrates one part of joint sizing. If a pin of diameter d carries force F through two shear planes,
For hypothetical F = 20 N and d = 4 mm, the average shear stress is about 0.80 MPa. This low-looking number does not validate the joint: pin bending, hole bearing stress, plastic creep, fatigue, and looseness may dominate. In a plate of thickness t, nominal bearing stress is approximately F/(td); clearance and uneven load sharing make actual contact nonuniform.
Provide spacers so tightening a fastener clamps the intended inner race or sleeve rather than squeezing moving plates together. Use a retention method that remains secure through reversals. A nut that backs off changes backlash and can invalidate the assumed geometry before it visibly detaches.
4.5 Estimate actuator demand across the whole workspace
A servo chosen from a single upright pose can fail at a deeper crouch. Load torque depends on the derivative of the linkage geometry. For the equal-link mechanism developed next, L = 2a cos(q/2), so
If a leg supports axial load F, the magnitude of generalized holding torque is approximately
With hypothetical a = 0.08 m, q = 60°, and F = 10 N, this gives 0.40 N·m. This is the torque at the defined knee coordinate of the ideal constrained mechanism. Horn linkage ratios, friction, moving link weight, impacts, and transmission efficiency change actual actuator demand. Static holding demand is not the whole motion requirement.
Manufacturers may report stall torque in kgf·cm. The conversion is 1 kgf·cm = 0.0980665 N·m. Stall torque occurs at zero speed under stated electrical conditions; it is not a demonstrated continuous operating torque. The required torque-speed-duty-cycle region should fit inside measured actuator capability with thermal margin.
Backlash is just as important as peak torque. A joint that can move freely through a small angle before engaging the gears changes the body's height and attitude without corresponding servo motion. Increasing a controller gain cannot remove this lost motion; it can instead produce repeated impacts at the backlash boundaries.
4.6 Printing is a manufacturing process, not a material certificate
The model contains four printable selectors: body_tray, lid, upper_link, and lower_link. Export the individual parts rather than slicing the assembly with its reference motors and wheels. The supplied printing notes contain the exact OpenSCAD commands and quantities. At the time of this documentation, STL export and slicer validation have not been performed.
First print a link as a dimensional coupon. Measure hole diameter, pivot centre distance, slot width, plate thickness, and flatness. A nominal 4.2 mm CAD hole is a placeholder clearance, not a guaranteed bearing fit or servo spline. Printer extrusion, shrinkage, orientation, and postprocessing all affect its real size. Record the measured deviations before modifying every part.
Fused-filament parts are direction dependent. A load carried by continuous strands within a layer differs from a load trying to peel adjacent layers apart. Choose orientation from the load path, then inspect whether holes, thin walls, or sudden section changes concentrate stress. The default links lie with broad faces on the bed; this is convenient and may favor some in-plane loads, but it does not qualify out-of-plane bending strength.
Perimeters often carry substantial bending load near a part's surface. Infill can support walls and reduce local deformation, yet a high infill percentage does not make a poor joint geometry sound. Add adequate edge distance around holes, generous transitions, and a support architecture that avoids concentrating the full hip reaction in the tray's thin wall.
Material choice depends on temperature, creep, impact, and manufacturing quality. A fit model can establish packaging even when its material is unsuitable for prolonged loaded operation. Electronics and motors add heat; sustained fastener preload can creep plastic and loosen a joint. Select a documented material/process combination, then validate a representative printed joint under its intended load and temperature.
Do not assign a universal allowable stress to “3D printed plastic.” Use properties appropriate to the actual material, print orientation, process, and conditioning, or measure them. For an initial load test, a restrained fixture with incremental loading and displacement measurement can reveal weak interfaces before powered balancing.
4.7 Clearances, cable routing, and serviceability
Sweep the mechanism through its entire allowed workspace. Check both symmetric crouch and differential left/right extension, because roll correction can bring one knee much closer to the body. Also include expected body pitch, motor wires, connectors, screw heads, servo horn rotation, and access for tools. A clearance that exists at the display pose can disappear elsewhere.
Cables need controlled slack and strain relief. Across a joint, route the bend away from a sharp edge and prevent the cable from becoming a hidden mechanical stop. Motor leads should not tug on the IMU mount. Place the sensor on a rigid body reference with a known orientation; an excessively flexible mount can introduce an additional moving frame into the estimator.
Make the battery replaceable without disturbing sensor orientation or joint calibration. Its retention must hold position during acceleration and a caught fall. Moving a battery changes mass properties, so mark its mounting position and include it in the configuration record.
Accessible fasteners, replaceable bushings, and separate electronics carriers make iteration much faster. A structural side plate should not require disconnecting every signal just to inspect a knee bearing. Design the support tether attachment as a load path too; attaching it to the decorative lid may defeat its purpose.
4.8 From mockup to a reproducible mechanism
Create a revision record containing the CAD commit, exported part files, print material and orientation, measured link lengths, wheel radius and track, joint limits, actuator model, gear ratios, and assembled masses. Photograph cable routing and mark left/right signs. This record connects a real robot to the gains and geometry loaded in software.
A useful acceptance sequence is: inspect the unpowered assembly; measure dimensions; verify free motion and mechanical stops; measure backlash; perform restrained static loading; calibrate joint angles; then test actuation without balance enabled. Evidence from each step narrows uncertainty for the next.
In fixed-leg commissioning, the software port must suppress knee actuation and report the measured fixed geometry. geometry_verified=true is a statement about actual physical knowledge; it should not be set merely to bypass an inhibit. For moving legs, add actual joint feedback and verify that the intended guide/linkage constrains the motion assumed by the controller.
Exercises and short answers
- The default upper-link lean is 15° with equal link lengths and the wheel below the hip. Is knee flexion 15° or 30°? Answer: 30° under the book's q = 0 straight convention.
- A wheel reaction of 15 N acts 20 mm from a bearing. What bending moment results? Answer: 0.30 N·m.
- A two-joint leg has only a knee servo. What remains uncontrolled if the hip is freely pivoted? Answer: the leg's orientation about the hip; scalar leg length does not determine wheel position.
- Why is a high infill percentage insufficient to approve a hip mount? Answer: load-path geometry, layer orientation, local bearing stress, fasteners, fatigue, and creep remain unverified.
- Which configuration record changes when a battery moves upward? Answer: component positions, centre of mass, pitch inertia, and potentially the identified plant and gain table.
The next chapter turns these physical relationships into equations. Keep the mechanism constraints explicit: they are what makes a mathematical coordinate correspond to a measurable joint or distance.
5. Kinematics, center of mass and changing inertia
Previous: Mechanical design · Book contents · Next: Sampling and state estimation
Prerequisites and learning objectives
Read Chapter 4 and review sine, cosine, derivatives, vectors, and the meaning of a moment. Work through the examples with a calculator before using a symbolic algebra package. By the end, you should distinguish joint coordinates from Cartesian position, derive the repository's length relationship, recognize singularities, and build a mass-property table suitable for gain design.
The scalar functions bal_leg_length() and bal_leg_inverse() are implemented in control.c. The full planar kinematics, Jacobian, mass aggregation, and dynamic extensions below are teaching models and proposed design work; they are not additional implemented controller capabilities. The actual knee mechanism remains unspecified.
5.1 Define every coordinate before writing an equation
Body axes are x forward, y left, z up. Pitch θ is positive for a forward lean, a positive rotation about y. Distances use metres; angles and angular rates use radians and radians/second. CAD values expressed in millimetres must be divided by 1000 before entering controller configuration.
For one planar leg, let H be the hip and W the wheel centre. Upper and lower pivot-centre lengths are a and b. Knee flexion q is zero for a straight leg and increases as it folds. Restrict the teaching model to 0 < q < π and one selected folding branch.
Define α as the upper link's geometric angle forward from downward vertical, with the body upright. This is a joint-geometry convention, not the sign convention for body pitch: a forward rotation of a downward-pointing vector has the opposite y-axis rotation sign to a forward rotation of the upward body axis. Keeping the two symbols distinct avoids silently reversing a joint Jacobian.
Three heights must be kept separate:
| Symbol | Meaning |
|---|---|
| L | Hip-to-wheel distance, |
| z_H − z_W | Vertical separation between hip and axle |
| h | Effective body centre-of-mass height above axle at the upright equilibrium |
Only a suitable mechanism constraint makes the first two equal. The third also depends on the distribution of mass above and below the hip. The controller's fields named height currently represent L, averaged over both legs for gain scheduling; they are not measurements of h.
5.2 Forward kinematics of a free planar leg
Set the hip origin to (0, 0) in the x-z plane. The knee and wheel positions are
At q = 0 the links align, as required. For a = b and α = q/2, the horizontal terms cancel and the wheel lies directly below the hip. A different α rotates the same bent shape around the hip without changing its end-to-end length.
Squaring and adding the coordinates eliminates α:
This is the law of cosines under the q = 0 straight convention. Some textbooks define the interior triangle angle instead; substituting that angle without conversion changes the sign in the cosine term.
With hypothetical a = b = 0.08 m and q = 60° = π/3 rad,
For α = 30°, the knee lies 40 mm forward of the hip, and each link contributes about 69.28 mm of vertical drop. These dimensions deliberately resemble the starter CAD, but they are an example pose, not a validated mechanism or chosen joint limit.
5.3 Inverse kinematics: a distance request versus a position request
Solving the length equation gives
A physical triangle satisfies |a−b| ≤ L ≤ a+b. The repository deliberately requires a strict interior value, rejecting both endpoints. A configured operating range should leave additional mechanical and numerical margin inside that interval.
Floating-point roundoff can place an otherwise valid cosine argument slightly outside [−1,1]. Clamping that argument after checking physical reachability prevents a numerical domain error. Clamping arbitrary impossible positions into the workspace would instead conceal a bad request. bal_leg_inverse() first rejects unreachable/boundary lengths and then clamps the computed cosine argument.
For a free two-joint leg, a full position request contains both x_W and z_W. First compute
For the selected q > 0 branch,
The second formula rotates the bent leg into the requested direction. A different folding branch produces another solution, subject to mechanical constraints. These equations also show what the current scalar controller omits: it computes q from L but does not command or measure α. Mechanical guidance or a future controlled hip must supply the missing relation.
If the actual mechanism uses a four-bar, tendon, or nonconcentric servo linkage, derive its actuator-to-joint mapping. A servo shaft angle is not automatically q. Its zero offset, sign, transmission ratio, and potentially nonlinear relationship belong in a calibrated mechanism model.
5.4 The Jacobian connects position, velocity, and force
Differentiate forward kinematics:
Each entry has units of metres per radian. Its determinant is
At a straight or fully folded configuration, this determinant is zero: the two joint motions cannot generate two independent Cartesian velocity directions. Near either singularity, solving for joint rates can demand very large motion from small Cartesian requests. A matrix inverse that exists numerically can still be mechanically unusable.
For a force F = [F_x, F_z]ᵀ applied at W, virtual work gives the associated generalized external joint torque
The holding actuator torque is its negative, plus terms for gravity, friction, and other loads. This distinction matters when testing signs. The transpose relationship is about consistent force/coordinate definitions, not a rule that every servo torque equals the magnitude of JᵀF regardless of how the mechanism is constrained.
For the guided scalar coordinate,
At a = b = 0.08 m and q = 60°, dL/dq = −0.04 m/rad. An extension request of +0.01 m/s requires q̇ = −0.25 rad/s: the knee straightens. At q = 10°, the same extension requires approximately −1.43 rad/s. A fixed height-rate limit alone therefore does not bound joint speed near straightening; the code separately limits knee target rate.
At the exactly folded equal-link case, L also vanishes, so the derivative expression's 0/0 form needs a limiting analysis. The full Cartesian Jacobian remains singular there, and the folded configuration is excluded. Do not use an algebraic cancellation as an argument for operating at a collapsed mechanism.
5.5 Measurement uncertainty becomes geometry uncertainty
A small joint error δq changes length by approximately
At the 60° example, a 1° error gives about 0.70 mm length error. Near straightness, length is less sensitive to angle error, yet inverse angle demand is more sensitive to a requested length change. These are compatible statements about opposite mappings.
Backlash is more difficult than zero-mean sensor noise because its error depends on motion direction and load. A fitted calibration curve measured while extending may not describe retracting motion. Record both directions and loaded behavior. Elastic deflection can make the true wheel-to-hip distance differ from the rigid-link equation even with an accurate angle sensor.
A simple first-order uncertainty calculation is
where independent errors are assumed and
Correlated errors require covariance terms. This calculation helps decide whether a finer angle sensor or better measurement of pivot spacing will improve the model more.
5.6 Centre of mass is a weighted position
For components with mass m_i and centres r_i in one frame,
Include battery, wires, fasteners, servos, mounts, and moving links. Separate the components assigned to the balancing body's effective mass from those assigned to the axle/base mass. Otherwise the same motor or wheel mass can accidentally enter the dynamic model twice.
The following hypothetical frozen-geometry example groups all body components into three categories. Positions are relative to the wheel axle; all x positions are zero solely to simplify arithmetic. The link group's inertia must already include the spread of the individual links about that group's centre.
| Group | Mass, kg | Standing z, m | Crouched z, m | Group pitch inertia about its own CoM, kg·m² |
|---|---|---|---|---|
| Torso/electronics | 0.80 | 0.20 | 0.16 | 0.0020 |
| Battery | 0.30 | 0.12 | 0.08 | 0.0003 |
| Links and joint hardware | 0.20 | 0.08 | 0.06 | 0.0004 |
At the standing configuration,
At the crouched configuration, h ≈ 0.12615 m. The torso moved down by 40 mm, but the aggregate CoM moved by about 36.92 mm because the link group moved differently. Neither h equals an assumed hip-to-wheel length.
Real forward-folding knees may move component centres in x as well. A nonzero x_C means that geometrically upright body surfaces do not necessarily correspond to zero gravitational moment about the axle. Identify a consistent equilibrium reference instead of hiding a mass offset inside arbitrary gain changes. The current linear design assumes an equilibrium with the effective body CoM above the axle.
5.7 Inertia describes where the mass is
Pitch inertia about the aggregate CoM is
provided each I_{y,i} is expressed about an axis parallel to body y. The second term is the parallel-axis theorem. Distances along y do not enter a point mass's moment about the y axis; they do affect moments about other axes.
Using the standing example,
Keeping the illustrative group inertias constant in the crouched example gives I_y ≈ 0.0051308 kg·m². A real mechanism generally also changes the groups' orientation and internal spread, so recompute those terms from geometry rather than copying this simplifying assumption.
The inertia about the axle is I_axle = I_y + mh² when the CoM is directly above it. The examples yield 0.04030 and 0.02582 kg·m². The design tool expects I_y about the body CoM, not I_axle. Its dynamic matrix adds mh² internally; supplying axle inertia would count that offset twice.
For full three-dimensional assemblies, rotate each component's inertia tensor into the common body frame and then translate it:
This is especially relevant when the left and right legs differ in extension. A single scalar scheduled on average length cannot capture every roll/pitch coupling from an asymmetric pose.
5.8 From geometry measurements to a gain schedule
The offline LQR design tool accepts three increasing leg_length knots, each with separately measured/modelled com_height and pitch_inertia. It also requires body mass, effective base mass, wheel radius, sample period, and cost weights. A useful source table therefore contains L, both measured joint angles, h, I_y, and the mechanism revision at every knot.
Wheel rotational inertia can contribute an equivalent translational term I_w/r² to the base model under rolling constraints. The selected simplified model and identification procedure must consistently account for such terms. Do not treat the CAD's total mass as both effective base mass and body mass.
For each geometry: weigh parts, locate component centres, compute or estimate inertia, check the assembled CoM experimentally, and compare predicted motion with restrained measurements. CAD mass properties require correct density, purchased-part masses, and origins. Printed infill and omitted fasteners can make default solid-model estimates misleading.
A rigid body suspended as a small-angle physical pendulum gives an optional inertia check. For total mass m, CoM distance d below a test pivot, and period T,
Lock all internal joints, measure several periods, and account for fixture inertia. An uncertain small d can make this subtraction poorly conditioned. This is an identification experiment, not part of the real-time controller.
5.9 Why changing height changes more than a lookup index
A gain table treats each geometry as a frozen rigid body. During motion, link velocities and accelerations introduce additional terms. For a point mass at a variable distance h from a moving axle,
Differentiation gives velocities containing both ḣ and θ̇. Even the angular momentum term mh²θ̇ has derivative
The second term is absent from a frozen-height model. A real multibody mechanism adds joint-dependent inertia, gravity, and actuator reactions. Height and knee-rate limits reduce these effects but do not prove that they are negligible.
The present controller interpolates gains using measured average leg length and limits requested height and knee motion. It does not solve full multibody inverse dynamics. Stable poles at every stationary knot are necessary evidence for those models, but they do not establish stability during arbitrary transitions, saturation, backlash, or fast differential leg motion. These extensions are developed in dynamics and LQR and advanced control.
Exercises and short answers
- For a = b = 80 mm and L = 120 mm, compute q. Answer: q = 2 arccos(0.12/0.16) ≈ 82.82°, or 1.4455 rad.
- Why does q alone fail to determine a free leg's wheel position? Answer: α can rotate the bent leg while preserving its length.
- At q = 60° in the equal-link example, what joint rate produces L̇ = −5 mm/s? Answer: q̇ = +0.125 rad/s; positive flexion crouches the leg.
- Which inertia should be passed as
pitch_inertia? Answer: the effective body's pitch inertia about its own CoM, in kg·m². - A 0.2 kg component moves from 0.10 m to 0.20 m above a fixed axle. How does its point-mass axle inertia change? Answer: by 0.2(0.20²−0.10²) = 0.006 kg·m²; an aggregate CoM calculation still needs to be redone.
- Does a stable stationary gain table prove safe fast crouching? Answer: no; time-varying dynamics, actuator limits, measurement error, and transition rates remain to be validated.
6. Sampling and state estimation
Prerequisites: units and frames from Chapter 1, sensor interfaces from Chapter 3.
Learning objectives: distinguish sensor and control rates, derive the complementary filter, account for coordinate transformations and recognize what the available measurements cannot determine.
6.1 A sensor sample is a measurement at a time
An IMU sample is not simply six numbers. It is six numbers associated with a configuration, coordinate frame, calibration and acquisition time. If the gyro was configured for one range while the decoder assumes another, all subsequent control calculations use the wrong angular velocity. If data remain in a buffer for 15 ms but receive a new timestamp when copied, a freshness check can accept stale information.
The current sample structure has one timestamp_us for the whole snapshot. The adapter must therefore bound the ages and relative skew of IMU, wheel and joint data before setting valid. A single common timestamp cannot independently express one old encoder reading among fresh IMU readings. An eventual richer interface may provide acquisition timestamps per sensor group.
bal_imu_decode expects a 12-byte frame of gyro xyz followed by accelerometer xyz, at ±4 g (8192 counts/g) and ±2000 °/s (16.384 counts per °/s). The BMI323 returns the opposite order in registers 0x03–0x08, and every I2C read starts with two dummy bytes; bal_bmi323_pack builds the frame the decoder uses. The decoder itself neither configures the device nor confirms a successful bus transfer. Register behavior, output data rates, data-ready signaling and the gyro's 30 ms startup come from the Bosch BMI323 datasheet.
6.2 Rate, interval, latency and jitter
At 400 Hz, the nominal interval is T=1/400=0.0025 s. This is the distance between control invocations, not an allowance to spend the entire interval waiting for a new sensor sample. Sensor acquisition can run asynchronously before the control invocation, leaving the controller a completed snapshot.
Latency is the elapsed time from a physical event or acquisition to the corresponding actuator update. Jitter is variation in timing. A loop can have an average rate of 400 Hz while occasionally pausing for 20 ms; the average hides the missed opportunities to correct an unstable plant.
For a sinusoidal component at frequency f, pure delay L contributes phase lag 360 f L degrees. At 10 Hz, 5 ms contributes 18 degrees. This simple calculation does not prove a stability margin, but it explains why adding a smoothing filter or a deep queue can worsen behavior even when the signal looks cleaner.
Sampling also limits what can be distinguished. Frequency content above half the sampling frequency can appear as lower-frequency content unless attenuated before sampling. A digital filter after sampling cannot reconstruct information already aliased by sampling. Mechanical vibration, IMU internal bandwidth, output data rate and control rate must be considered together.
6.3 Calibrate and rotate before filtering
Use a fixed mounting rotation R to transform sensor-frame vectors into the body frame:
ω_body = R * (ω_sensor - gyro_bias)
a_body = R * corrected_accelerometer
A valid rigid rotation preserves vector lengths and handedness. Arbitrary axis swaps can create a reflection if their signs are not handled correctly. Establish the transformation by physically rotating the board about each defined positive body axis and inspecting the response.
At rest, estimate gyro bias by averaging a stationary interval after startup settling. The averaging interval must actually be stationary; including hand motion produces a biased calibration. Record the temperature and repeatability. The current controller does not estimate bias online.
Accelerometer correction may involve offset and scale. Multiple static orientations help distinguish a constant offset from axis scaling. A resting magnitude close to g is useful evidence, but it does not verify the sign of every axis or the gyro mounting transformation.
6.4 Derive inclination from acceleration
For a body with roll φ and pitch θ, an upright stationary accelerometer senses specific force approximately [0,0,g]. Under quasi-static conditions, the body components of that vector give:
pitch_acc = atan2(-a_x, sqrt(a_y² + a_z²))
roll_acc = atan2( a_y, a_z)
For a pure forward pitch of 5 degrees, the expected accelerometer components are approximately a_x=-g sin(5°)=-0.855 m/s² and a_z=g cos(5°)=9.769 m/s². The negative x component is consistent with positive pitch in the chosen FLU convention.
These equations rely on the measured vector being dominated by gravity-related specific force. When the wheels accelerate, the accelerometer also responds to translational motion. Interpreting that acceleration as tilt can feed a false attitude correction into the controller. Placing the IMU away from the body rotation center adds tangential and centripetal acceleration terms as well.
The code computes the acceleration magnitude and accepts the inclination correction only when abs(norm(a)-g) <= accel_gate. This rejects some disturbances, but an accelerated vector can still have a magnitude close to g. The gate is a heuristic, not a separation of gravity from motion.
Part of that motion is known. The wheels report the axle's speed, and its rate of change is the axle's forward acceleration . Before using the accelerometer, bal_step low-pass filters that acceleration (50 ms, one tick old) and removes it from the measured vector: and . The gate then judges only what is left. The IMU's own tangential and centripetal terms, and for an IMU a height h above the axle, are not removed; they would need that height and an angular-acceleration estimate. The closed-loop simulation in chapter 13 shows why the compensation matters. Without it, the 0.2 m/s² of a slow back-and-forth recovery reads as 0.02 rad of tilt, the filter passes it at that frequency, and the loop settles into a lightly damped oscillation with a period of about 3 s.
6.5 Body gyro rates are not always Euler rates
Near upright and small roll, body gyro y approximates pitch rate. More generally, for the Euler convention used in the implementation:
θ̇ = ω_y cos φ - ω_z sin φ
φ̇ = ω_x + sin φ tan θ * ω_y + cos φ tan θ * ω_z
The tan θ terms become problematic near ±90 degrees. This controller operates inside a much smaller tilt envelope and faults on excessive tilt. It is not a global attitude estimator for arbitrary tumbling or recovery motion.
On the first valid sample the code initializes pitch and roll from the accelerometer. Pitch and roll rates are initially zero rather than immediately calculated from that sample's gyro. After a sufficiently long sample gap it reacquires inclination when the gravity gate permits, resets rates and retains a fault. These details matter when studying startup and recovery logs.
6.6 The complementary filter
Gyro integration responds quickly but drifts because a small bias accumulates. Accelerometer inclination provides a long-term reference when its quasi-static assumption is reasonable, but it is vulnerable to translational acceleration. A complementary filter combines their strengths.
Let θ⁻ = θ_previous + Δt * θ̇ be the gyro prediction. The implementation computes:
β = Δt / (τ + Δt)
θ = wrap(θ⁻ + β * wrap(θ_acc - θ⁻))
θ_acc is the inclination of the accelerometer vector after the axle's acceleration has been removed (6.4). τ is the configured filter time constant. The function wrap(a)=atan2(sin a,cos a) selects a principal angular difference so that crossing an angle boundary does not produce an artificial correction of nearly a full revolution. Roll is updated similarly. When the acceleration gate fails, β becomes zero and the filter uses gyro prediction alone.
For Δt=0.0025 s and τ=0.3 s, β is about 0.008264. If the prediction is 0.020 rad and the accelerometer reports 0.030 rad, the corrected estimate is approximately 0.0200826 rad. The accelerometer changes the estimate gradually; it is not an average with equal weights.
Increasing τ reduces short-term accelerometer influence but leaves gyro bias less corrected. Decreasing τ makes the estimate follow accelerometer inclination more closely, including acceleration-induced errors. Select the value from motion logs and the controller's tolerance to delay and bias, not solely from how smooth a stationary plot appears.
6.7 Timestamp arithmetic and repeated samples
The implementation uses unsigned 32-bit microsecond timestamps. The full counter wraps in approximately 4294.967296 seconds, or 71.58 minutes. Modular subtraction correctly handles ordinary wrap when intervals remain within the intended range. Half-range sequence comparisons assume gaps below about 35.79 minutes when distinguishing ordering.
Fusion uses the difference between successive sample timestamps. A repeated timestamp skips the filter update. By contrast, the current yaw integration uses nominal control dt and wheel speed each control invocation. Thus a snapshot containing repeated wheel data may continue contributing yaw under its bounded-age assumption. This behavior should be understood before extending the estimator to irregular sensor rates.
A software timestamp attached on delivery cannot reveal time spent queued in a device or DMA pipeline. Use data-ready capture or an appropriately mapped sensor acquisition clock if accurate age is important.
6.8 Wheel state and odometry
The encoder travels s_L,s_R measure each wheel relative to its stator, which is fixed to the shin and turns with the body and the knee (chapter 3). The ground position adds that rotation, p = (s_L+s_R)/2 + r(θ + k_s (q_L+q_R)/2) with k_s = shin_pitch_per_knee, and the ground speed adds r θ̇ to the mean encoder speed. The code computes forward distance increment from changes in p and estimates yaw rate from the wheel speeds. It integrates planar odometry with the heading at the middle of the nominal interval:
Δs = p - p_previous
x_new = x + Δs cos(ψ + ω T/2)
y_new = y + Δs sin(ψ + ω T/2)
ψ_new = wrap(ψ + ω T)
The first sample sets p_previous to the first ground position, so an arbitrary encoder origin does not immediately jump x or y. The initial pose is still zero; there is no external initial-heading input. Slip, wheel-radius error, unequal tires and changes in effective track width produce accumulated odometry error.
A six-axis IMU provides no absolute compass heading. Integrating gyro yaw can improve some short-term estimates but also drifts. Global yaw and position require additional information or assumptions, such as visual tracking, landmarks or an external localization system.
6.9 Practical estimator lab
With motors inhibited, log raw readings, calibrated readings, acquisition timestamps, filtered angles and gate state. Hold several known static inclinations, then rotate slowly about one axis at a time. Check angle signs and steady errors. Next translate the board without intentionally rotating it and observe how acceleration can affect inferred tilt. This exposes a limitation that a stationary calibration cannot reveal.
Current wire telemetry includes several IMU and angle fields but not the original sample timestamp or gate state. Extend observation-only logging deliberately if the experiment needs those quantities; do not pretend they are already in the packet. Keep any extra logging outside the deterministic control path.
Exercises
- A gyro bias is 0.01 rad/s. How much open-loop angular drift accumulates in 20 s? Answer: 0.2 rad, about 11.46 degrees.
- What is β at τ=0.3 s and dt=5 ms? Answer: approximately 0.016393.
- Why can the norm gate accept bad tilt information? Answer: translational acceleration can change vector direction without moving its magnitude outside the accepted band.
- What additional field most directly helps a Linux consumer assess sensor age? Answer: acquisition time, together with a valid mapping from the RTOS timebase to the consumer's clock.
7. Dynamics and the linear quadratic regulator
Prerequisites: kinematics and mass properties, basic derivatives, vectors and matrix multiplication.
Learning objectives: derive the model actually used by the gain tool, calculate discrete feedback gains, interpret their units and understand the limits of geometry scheduling.
7.1 Start with a deliberately limited plant
The offline tool treats the robot as a rigid inverted body above a driven wheel axle at each fixed leg configuration. It assumes planar motion, no wheel slip, a level surface, small deviations about upright, and a total wheel-torque input. The model does not include knee acceleration, flexible printed parts, motor inductance, gearbox backlash or a servo's internal dynamics.
Define x as axle travel, θ as forward pitch, r as wheel radius, m as body mass, h as body CoM distance from the axle and I as body pitch inertia about its CoM. M is the effective base mass including appropriate wheel rotational-inertia contributions. Do not add a wheel's translational mass twice or blindly reflect motor inertia without checking the chosen coordinate model.
The body CoM position is (x+h sin θ, h cos θ). Differentiating and collecting kinetic energy gives:
T = 1/2 (M+m) ẋ² + m h cos θ * ẋ θ̇ + 1/2 (I+m h²) θ̇²
V = m g h cos θ
The potential energy has a maximum at upright, which explains the unstable equilibrium. These expressions are the starting assumptions of the model, rather than a claim that the current unbuilt mechanism exactly obeys them.
7.2 Why wheel torque is different from cart force
Let u be the sum of left and right wheel torques that drive the wheels forward. Under rolling assumptions, the generalized horizontal drive is u/r. The motor also exerts an opposite reaction on the body, giving a pitch generalized torque of -u in this reduced model.
Applying the Euler–Lagrange equations yields:
(M+m) ẍ + m h cos θ * θ̈ - m h sin θ * θ̇² = u/r
m h cos θ * ẍ + (I+m h²) θ̈ - m g h sin θ = -u
A force-actuated cart-pole model omits that body reaction term. It can be a useful educational comparison, but inserting its B matrix into a wheel-torque controller silently changes the physical meaning of the input. The repository's model function explicitly includes both 1/r and -1 in the generalized input vector.
7.3 Upright linearization
For small θ, use sin θ≈θ, cos θ≈1, and discard the product involving θ θ̇². Define J=I+m h² and the mass matrix H:
H = [[M+m, m h],
[m h, J ]]
[ẍ, θ̈]^T = H⁻¹ * ([0, m g h θ]^T + [1/r, -1]^T u)
The determinant is D=(M+m)J-(m h)². With positive physical parameters it is positive. It is useful to derive the inverse once on paper even though the Python tool uses a numerical linear solve:
ẍ = -(m² g h² / D) θ + (J/r + m h)/D * u
θ̈ = ((M+m)m g h / D) θ - (m h/r + M+m)/D * u
A positive pitch accelerates further positive without control, while a positive wheel input initially accelerates pitch in the correcting direction. Forward wheel motion and backward angular correction are not contradictory: the axle moves beneath the leaning body.
For state z=[x, ẋ, θ, θ̇]^T, the continuous system is ż=A z+B u, with the two kinematic rows ẋ=z₂ and θ̇=z₄, and the dynamic coefficients above in the acceleration rows. The actual feedback uses position and velocity errors rather than absolute travel.
7.4 A worked synthetic model
Take M=0.5 kg, m=2 kg, h=0.27 m, I=0.04 kg m², and r=0.04 m. These values teach the calculation and are not measurements of this project.
A ≈ [[0, 1, 0, 0],
[0, 0, -16.539151, 0],
[0, 0, 0, 1],
[0, 0, 76.570142, 0]]
B ≈ [0, 29.988433, 0, -92.539040]^T
At θ=0.02 rad with u=0, the model predicts pitch acceleration about 1.5314 rad/s². A constant positive total torque of 0.02 Nm contributes approximately -1.8508 rad/s², before considering the other state evolution. This calculation helps check input sign; it is not a complete control response.
Pay attention to units: the second B entry maps Nm to m/s² and the fourth maps Nm to rad/s². Feeding normalized duty directly into this B matrix would be a unit error unless the plant input model were changed accordingly.
7.5 Discretize for the actual control period
The controller holds an output between updates. For an ideal zero-order hold and period T,
A_d = exp(A T)
B_d = integral from 0 to T of exp(A s) B ds
z[k+1] = A_d z[k] + B_d u[k]
The tool uses scipy.signal.cont2discrete with its zero-order-hold behavior; the SciPy documentation specifies this interface. Simply replacing A_d with I+A T and B_d with B T gives a first-order approximation, which should not be confused with exact hold discretization.
The discretized model still omits sensor and actuator latency. If the actual output takes effect one period late, the plant sees a delayed control law. Include that delay in a later model rather than assuming a gain derived at the right nominal rate automatically handles it.
7.6 The optimization problem
LQR selects a linear feedback law minimizing a quadratic sum:
cost = sum over k of (e[k]^T Q e[k] + u[k]^T R u[k])
u[k] = -K e[k]
Q weights state errors, and positive R weights actuator effort. For the discrete infinite-horizon problem, the algebraic Riccati equation is:
P = A_d^T P A_d
- A_d^T P B_d (R+B_d^T P B_d)⁻¹ B_d^T P A_d + Q
K = (R+B_d^T P B_d)⁻¹ B_d^T P A_d
The tool obtains P with solve_discrete_are; see its official numerical contract. Existence of a useful stabilizing solution depends on the plant and weighting assumptions, including stabilizability and appropriate detectability conditions. A successful matrix solve is not a proof that the real motors meet the model.
An accessible interpretation is that the controller trades state deviation against torque over the future modeled motion. For broader derivation and local nonlinear-control context, consult the original MIT Underactuated Robotics LQR notes. The equations above are used here to explain this repository's discrete implementation.
7.7 Choose weights with scales in mind
Adding square metres and square radians has no direct physical meaning until the weights define the tradeoff. One starting interpretation is normalized error: choose meaningful error scales s_i and use Q_ii≈1/s_i², then select R from a meaningful torque scale. This is a starting design heuristic, not a tuning guarantee.
For the synthetic model with T=0.0025 s, Q=diag(10,1,100,1) and R=1, numerical evaluation gives approximately:
K = [-2.757508, -3.008145, -13.263020, -2.025915]
closed-loop poles:
0.784019, 0.975897, 0.996897 ± 0.002493 j
All pole magnitudes are below one for this frozen discrete linear model. The signs follow from the model and the u=-K e convention; do not force every gain positive because a PID tutorial used positive constants under a different error definition.
The four gain units are respectively Nm/m, Nm/(m/s), Nm/rad and Nm/(rad/s). If a gain table is moved between degree-based and radian-based software, the angular coefficients must be transformed; merely copying the numbers changes the law.
7.8 Schedule by measured geometry
The code stores three gain knots sorted by leg length. Between adjacent lengths l_i and l_(i+1), it computes:
α = clamp((l-l_i)/(l_(i+1)-l_i), 0, 1)
K(l) = K_i + α (K_(i+1)-K_i)
The scheduling variable comes from the average forward-kinematic lengths of the measured or mechanically verified left and right legs. The gain tool separately receives h and I at each knot. It does not assume h=l, nor does it derive inertia from a requested crouch factor.
The Python tool checks each knot and 101 interpolation samples in each of the two intervals, interpolating h, I and K. This is a frozen-time eigenvalue screen at 202 sampled points, including repeated interval endpoints. It is not a proof over all configurations or over trajectories where geometry changes with time. It also does not enforce torque saturation, servo limits or ground contact.
7.9 What the model leaves for later
Different left and right lengths may introduce roll and geometry asymmetry not represented by an average-length planar model. A moving knee changes angular momentum and gravitational geometry. Fast height changes can therefore destabilize a controller whose every fixed-height model is stable. The implemented motion-rate limits help restrict operation, but their safe numerical values require experiments and a fuller model.
Closed-loop validation should progressively add motor voltage limits, back EMF, sample delay, estimator dynamics, sensor bias, knee lag and friction. The goal is to expose the domain where the reduced law remains useful. Chapter 15 explains methods for extending that domain rather than treating one set of poles as universal evidence.
Exercises
- Why is
B_thetanegative in this convention? Answer: positive wheel torque moves the axle forward and applies a backward reaction to the body. - For a gain of -20 Nm/rad and a pitch error +0.01 rad, what pitch contribution appears in
u=-K e? Answer: +0.20 Nm total wheel torque. - Can three stable knots guarantee stable interpolated gains? Answer: no; even frozen interpolation requires examination, and time-varying stability is a further question.
- What must change if control period changes? Answer: discretize again, redesign/check gains and revisit timing, estimation and delay assumptions.
8. Reading the balance controller as an executable specification
Prerequisites: Chapters 6–7 and basic C.
Learning objectives: follow the actual controller path, distinguish fault and mode behavior, explain output allocation and identify interface assumptions before integrating hardware.
8.1 Configuration is part of the algorithm
control.h defines the public contract. bal_config contains the sample period, geometry, speed/torque limits, tilt limits, filter parameters, knee/height rates, command and sensor timeouts, a three-knot gain table and commissioned.
bal_init clears controller state and checks positivity, finiteness, allowed control period, interior link reach, tilt relationships, timeout range and gain ordering/coverage. It accepts periods between 2 and 5 ms. The maximum leg length must be strictly below the sum of link lengths, and the minimum strictly above their difference magnitude. These conditions avoid the exact geometric endpoints.
Validation establishes structural plausibility. It does not prove a chosen motor can supply max_torque, or a chosen servo can follow max_knee_rate. commissioned is a software declaration made by the application, not a sensor or a certificate. It should only be set after the calibration and operating envelope are recorded.
8.2 Command ingestion
bal_set_command rejects nonfinite values, out-of-range mode values, speed/yaw commands above configured bounds and height outside the allowed interval. Invalid commands latch BAL_COMMAND. Accepted commands are stored with RTOS-local receipt time.
The function is intended to run in the same context as bal_step; simultaneous writes from a mailbox ISR would violate that ownership unless a separate safe handoff were added. The runtime already performs the handoff by consuming the shared-memory ring in the control context.
Receipt time is not source-generation time. If Linux stores an old command in a queue and delivers it later, the current RTOS interface treats its recent arrival as fresh. The producer and transport must prevent replaying historical intent. A future protocol can carry age information with explicit clock mapping.
8.3 Sample and timing checks
Each bal_step begins by zeroing the output, so a return before successful control computation leaves enable=false. It rejects invalid snapshots, unverifiable geometry, nonfinite sensor values, out-of-range knee angles, stale acquisition time and backward timestamp jumps under modular ordering assumptions.
After initialization, the elapsed control time must be between 0.75 and 1.25 times configured dt. This catches gross scheduling deviations; it is not a statement that ±25% jitter is acceptable for the final robot. The runtime additionally measures a computation budget. Both checks supplement rather than replace target timing characterization.
Leg lengths are then computed from knee angles and compared with the configured geometry range. The state field named height is their average. The name should be read according to this implementation, not as a guarantee of world-frame body height.
8.4 Estimate, integrate and check tilt
The estimator initializes from accelerometer inclination, updates with new timestamped samples and applies the complementary filter, after removing the axle's own acceleration from the accelerometer (6.4). The encoder travel is then converted to ground travel with the current pitch and knee angles (3.5). Its increments update x/y, while differential wheel speed supplies yaw integration. These operations continue while disarmed if valid input is available, which helps produce useful observations before enabling actuation.
Nonfinite derived state produces BAL_SENSOR. Excessive pitch or roll produces BAL_FALL. The code permits a fault reset only with a fresh reset command, arm false and inclination inside the tighter arming window, after the earlier sample/geometry checks have succeeded on that invocation.
A reset clears the software fault bitmask. It does not repair an electrical fault or a broken mechanism. The board adapter must continue reporting such conditions, and an eventual supervisor should explain persistent physical causes rather than repeatedly clearing them.
8.5 Arming and rearming precisely
The initialized state has arm_low_seen=true, so the first accepted fresh arm command can enable the controller if all other conditions are met. After bal_fault, that flag becomes false. A disarmed command establishes a new low state; a later valid high command can arm after faults have been cleared.
This differs from an implementation that requires a mandatory low packet before the first ever arm. A Linux supervisor can impose a stricter startup sequence, but documentation must not attribute that stricter rule to the current C function.
Arming also requires commissioned, an allowed active mode, no fault, fresh command and pitch/roll inside the arming window. BAL_DISABLED and BAL_RECOVERY inhibit actuation. An already armed robot with an expired command latches a command fault; current timeout behavior removes drive rather than executing a graceful stopping trajectory.
8.6 Construct the reference and error
For DRIVE, requested forward speed and yaw rate are used; other active modes use zero speed/yaw. The candidate position target advances by requested speed times dt:
p_candidate = clamp(p_target + v_target*T,
p_measured - position_error_limit,
p_measured + position_error_limit)
e = [p_measured-p_candidate,
v_measured-v_target,
pitch,
pitch_rate]
Here p_measured and v_measured are ground values, as in the LQR model. Feeding the encoder values directly would amount to e = [p - rθ, v - rθ̇, θ, θ̇], which turns the pitch gains into K₂ − rK₀ and K₃ − rK₁: not the controller that was designed.
The position-reference limit prevents the reference from drifting arbitrarily far away from the robot. This is reference management, not integral control of velocity error. There is no LQI error-integrator state in the current controller.
If the total torque demand exceeds the combined wheel limit (twice the per-wheel limit in that direction, 8.7), the persistent position target is not advanced to the candidate. This limits accumulation under saturation. The torque calculation on that tick still used the candidate target. Understanding that order is useful when comparing traces with a simplified mathematical description.
8.7 Balance and steering share a finite budget
After gain interpolation, total=-K e. Each wheel's limit is the smaller of the configured max_torque and the torque the drive reports it can deliver now in that direction, which falls with battery voltage and, in the accelerating direction, with wheel speed (3.4). The common per-wheel torque is clipped to those limits. Remaining headroom limits the differential steering correction:
fwd = min(max_torque, left_torque_forward, right_torque_forward)
bwd = min(max_torque, left_torque_backward, right_torque_backward)
common = clamp(total/2, -bwd, fwd)
headroom = max(0, min(fwd - common, common + bwd))
diff = clamp(yaw_kp*(yaw_target-yaw_measured), -headroom, headroom)
left_torque = common - diff
right_torque = common + diff
Suppose the per-wheel limit is 0.4 Nm in both directions, total balance request is 0.6 Nm, and the yaw loop requests 0.2 Nm differential. Common torque is 0.3 Nm, leaving 0.1 Nm headroom. The resulting left/right requests are 0.2 and 0.4 Nm. If total balance request reaches 0.8 Nm, there is no steering headroom.
This allocation preserves requested common torque within its limit while reducing yaw. Independently clipping left and right after adding yaw would distort the common balance contribution. The chosen strategy expresses an explicit priority.
8.8 Height, roll leveling and knee targets
The mean requested leg length is slew-limited by height_rate*T. A roll correction produces a bounded left/right length difference:
Δl = clamp(roll_kp*roll + roll_kd*roll_rate,
-max_leg_difference, max_leg_difference)
l_left_target = clamp(l_mean_target-Δl/2, l_min, l_max)
l_right_target = clamp(l_mean_target+Δl/2, l_min, l_max)
Each length is converted to a knee angle by inverse kinematics, then angle change is separately bounded by max_knee_rate*T. Therefore the eventual commanded mean knee geometry may lag the slew-limited length target. Actual geometry may lag again because of servo dynamics, load and backlash.
The roll correction assumes the mechanical relationship represented by the model. In the chosen sign convention a positive roll raises the left side, so shortening the left leg is intended to level it. A different linkage or axis convention may reverse this physical effect. Confirm the sign with supported, low-energy tests.
8.9 What output means
bal_output carries per-wheel torque requests in Nm, knee position requests in radians and an enable flag. It does not carry an electrical PWM frequency, servo pulse width, current command, motor polarity or driver pin mapping. Those belong to actuate_try and its configured hardware layer.
bal_step does not drive the motors itself. The board adapter's actuate_try hands the wheel torques to bal_drive_command, and the 10 kHz fast loop converts them into FOC phase voltages, accounting for wheel speed and supply and latching driver faults. Because the torque limits come from the same motor model, a returned torque is attainable at the present battery voltage and wheel speed according to that model; errors in R, or the rotor angle still make the delivered torque differ.
8.10 Inspection limitations to preserve
The telemetry packet lacks requested/actual mode, actual applied torque, covariance, sample acquisition time and individual sensor-valid flags. Faulted telemetry can contain old controller state and new or zeroed snapshot fields. A Linux consumer should not treat every packet as a uniformly fresh fused observation.
The controller does not detect contact loss, implement footed walking, provide global localization, or command a floor recovery maneuver. It assumes the configured mechanical model and fresh valid feedback. Treat these as boundaries of the current implementation, then design explicit extensions where the project requires them.
Exercises
- Is position-reference integration the same as integrating velocity error? Answer: no; the reference advances from requested velocity, while an error integrator accumulates measured tracking error.
- Why retain the fault latch after sensor reacquisition? Answer: returning data should not silently restart a previously disabled machine.
- Can a robot be numerically within its torque limit but physically saturated? Answer: yes, available current/voltage/traction depend on operating conditions.
- What happens to yaw authority during maximum common torque? Answer: it drops to zero under the implemented priority allocation.
9. Real-time firmware and deterministic execution
Prerequisites: the controller walkthrough and basic interrupts.
Learning objectives: distinguish deadlines from average speed, implement the runtime's callback contract, design asynchronous sensor acquisition and measure target timing honestly.
9.1 A deadline is an engineering requirement
A real-time control result is useful only if produced before the plant has moved too far for that result to be relevant. A loop that usually computes in 100 μs but sometimes stalls for 50 ms can be less useful than one that consistently computes in 500 μs. Report latency distribution and worst observed cases in addition to averages.
At 400 Hz, releases are 2500 μs apart. A hypothetical execution budget of 1500 μs leaves 1000 μs for scheduling margin and other activity, but that arithmetic alone does not justify the budget. Sensor age, interrupt latency, preemption, actuator latching and unmeasured hardware delays also contribute to the age of applied control.
sensor acquisition snapshot ready timer release output latched
|-------------------|---------------|----------------|
<---------------- total measurement-to-output age ---->
<-- execution -->
The runtime measures elapsed time from its own entry. It therefore cannot directly measure time spent waiting to enter after the timer event. Capture the timer event and output latch with hardware timestamps or logic-analyzer signals to observe the complete path.
9.2 The actual runtime sequence
bal_runtime_tick executes this bounded sequence:
- Read a local clock and validate the immutable shared-memory header/epoch.
- Attempt to consume no more than four command packets.
- Reject old/duplicate sequence numbers and invalid protocol fields.
- Obtain one completed sensor snapshot with
sample_try. - Run
bal_step, or latch a sensor fault if the snapshot is unavailable. - Check elapsed computation budget before applying output.
- Disable outputs if not enabled, otherwise attempt actuation once.
- Form and attempt to publish one telemetry packet.
- Attempt one pending mailbox notification.
- Recheck elapsed budget and feed the watchdog only on a fault-free path.
There is no unbounded retry loop in this orchestration. Its boundedness still depends on every callback returning in bounded time. A callback that enters a driver spin loop destroys the guarantee even though the runtime function itself contains no waits.
The watchdog callback occurs after the final software time check; it must itself have a small validated bound. Budget checks observe that an overrun happened. They cannot preempt a callback that never returns.
9.3 Callback contracts
| Callback | Required behavior | Common integration error |
|---|---|---|
clock_us |
Fast monotonic local time, with defined wrap | Using wall clock or blocking service |
sample_try |
One consistent completed snapshot or failure | Starting and waiting for I2C here |
actuate_try |
Bounded update or explicit failure | Waiting for servo motion to finish |
disable_now |
Immediate wheel disable plus designed servo policy | Queuing disable behind ordinary commands |
mailbox_try_notify |
One bounded doorbell attempt | Blocking for Linux acknowledgement |
watchdog_feed |
Bounded hardware watchdog service | Feeding elsewhere despite control failure |
| barriers | Correct ordering for chosen mappings | Empty functions because host tests passed |
bal_runtime_init checks configuration and callbacks before its successful-path call to disable_now. An early initialization failure does not guarantee that this function disabled hardware. The board must begin in a safe output state before attempting initialization; successful library setup should not be the first action that makes a powered driver safe.
The snapshot callback may return the latest completed snapshot again while its acquisition time remains within the allowed age. It must not mean “consume only a newly arrived sample”: returning false immediately latches a sensor fault. Asynchronous phase jitter alone should not cause a failure when a still-valid snapshot is available. The controller handles repeated timestamps explicitly.
9.4 Asynchronous I2C as a state machine
A native I2C peripheral supports a transaction that progresses under interrupts or DMA while the processor does other work. The driver typically has states such as IDLE, STARTING, TRANSFERRING, COMPLETE and ERROR. A data-ready interrupt can capture time and request a burst only when the driver has capacity. Completion validates the transfer and publishes a snapshot.
Timeouts are measured by inspecting a deadline, not by waiting until it expires. If a transaction has exceeded its bound, the state machine records an error and schedules recovery outside the critical control path. Device reset and required power-mode settling likewise occur through a startup state machine while motors remain inhibited.
IMU data-ready -> capture time -> begin burst -> return from ISR
I2C completion -> validate bytes -> publish completed buffer -> return
control timer -> try snapshot -> compute -> latch output -> return
The code in this repository only supplies the decoders and the callback contract, plus a port skeleton written against a HAL interface. There is no native SG2000 I2C state machine yet. The distinction is important when estimating remaining engineering effort.
9.5 Consistent snapshots without waiting
A producer interrupt and a control consumer must not access a partially rewritten snapshot. Options include a carefully designed ownership handoff, a brief bounded interrupt-masked copy, or a sequence-checked copy that makes a bounded number of attempts and fails if inconsistent.
Double buffering by itself is not a proof: if the producer switches buffers again while the consumer is still copying the old one, the producer can overwrite data in use. Either bound the production/consumption relationship or carry explicit ownership. A seqlock-style reader that retries forever is also unsuitable for a hard bounded callback.
Consider a snapshot with pitch-related IMU data from one instant and wheel position from another. Even if no bytes are torn, the estimator sees inconsistent physical time. Race freedom and temporal consistency are separate requirements.
9.6 Interrupt versus task execution
A bare-metal timer ISR can invoke a bounded control function if interrupt latency, FPU use and interrupt nesting are designed accordingly. A FreeRTOS design may instead release a highest-priority control task from a hardware timer. The task waits between releases, while the calculation and hardware callbacks remain free of waits. The distinction is between the scheduling mechanism and blocking inside the control path.
Neither pattern is supplied as a finished SDK adapter here. Verify FreeRTOS port rules for ISR-safe APIs, interrupt priorities, floating-point context saving and nesting in the exact SDK. Do not call ordinary task APIs from an interrupt merely because they worked in a host mock.
Avoid performing lengthy math in a low-priority task whose execution is delayed by logging or multimedia work. Conversely, placing everything inside the highest-priority ISR can starve acquisition interrupts and increase sensor age. The priority design should follow dependency timing, not simply label every robot interrupt “highest.”
9.7 Establish a timing budget
An illustrative 2500 μs period might allocate 100 μs to snapshot/command handling, 300 μs to estimation and control, 100 μs to actuator and telemetry work, plus explicit margins for preemption and output latching. These are accounting placeholders, not benchmark results.
Measure each stage on the C906. Pulse dedicated diagnostic GPIOs at timer release, runtime entry and output update; capture their differences under high Linux load, full command/state rings, busy I2C, error recovery and long-duration operation. Ensure measurement writes themselves have bounded and understood overhead.
Use fixed upper bounds for loops and avoid dynamic allocation, filesystem operations, network communication and formatted logging in the control path. Transcendental functions such as atan2f, sinf, cosf and acosf have library-specific execution costs. Their presence is not inherently disqualifying, but target WCET must include them. Do not replace them with unchecked approximations before profiling establishes a need and accuracy bounds.
Compile with the correct RISC-V ISA/ABI and FPU configuration. Avoid -ffast-math because assumptions that ignore NaNs/infinities can undermine the finite-value checks used by this implementation.
9.8 Failure containment
A software fault sets controller state and requests disable. An independent hardware watchdog covers a different failure class: the processor or software path stops progressing altogether. Design what happens electrically when the watchdog expires, and verify that it actually removes hazardous wheel drive. Resetting a processor while a motor driver retains its last PWM state may be insufficient.
A knee may support static load even when wheel motion should stop. Therefore disable_now needs a documented servo-support policy rather than blindly treating every actuator identically. The appropriate action depends on the mechanism and test fixture; the portable library cannot determine it.
Exercises
- A 300 μs runtime starts 2 ms late. Did a 1.5 ms execution-budget check prove correct timing? Answer: no; it measured execution after entry, not release latency.
- Why is a bounded failed snapshot preferable to waiting indefinitely? Answer: the controller can enter a defined disabled state instead of applying arbitrarily late feedback.
- What must be included when evaluating worst-case timing? Answer: interrupts, acquisition age, scheduling delay, every callback, memory/cache behavior, math routines and actuator latching under adverse workloads.
10. Shared memory, ownership and hardware mailboxes
Prerequisites: Chapter 9, unsigned integers and basic memory layout.
Learning objectives: read the packet ABI, reason about a single-producer/single-consumer ring, separate ordering from cache visibility and design restart behavior.
10.1 Data storage and notification solve different problems
Shared memory stores the data. A mailbox notifies another processor that it should inspect that data. A mailbox interrupt can be coalesced, delayed or unavailable while another notification is pending. Correctness should therefore follow from the ring's ownership protocol; a notification is an acceleration of discovery, not the only evidence that a packet exists.
The supplied transport is RPMsg-like only in the broad architectural sense of shared buffers plus notifications. It is a custom ABI, not a Linux RPMsg/virtio endpoint. Standard RPMsg userspace tools cannot consume it merely because the design resembles RPMsg conceptually.
10.2 Exact layout
The ABI uses fixed-width integers and IEEE754 binary32 values in little-endian layout. There are no pointers, native enums or C bitfields on the wire. The code asserts several structure sizes at compile time. sizeof(float)==4 alone does not prove every floating-point representation property; the target contract also requires the expected representation and endian ordering.
| Shared-region byte offset | Content | Size |
|---|---|---|
| 0 | magic, version, total bytes, epoch | 16 |
| 16 | reserved header bytes | 48 |
| 64 | command ring | 512 |
| 576 | state ring | 512 |
| 1088 | diagnostics ring (ABI version 2) | 512 |
| 1600 | end of defined structure | — |
Within each ring, write index is at offset 0, read index at 64 and four 96-byte slots begin at 128. The separated index blocks reduce false sharing if the design is extended, but do not establish cache coherence. The present contract still requires uncached mappings on both processors.
The command record is 24 bytes: sequence, mode, flags, speed, yaw rate and leg-length target. It is copied into the beginning of a 96-byte packet. Producers should zero the unused bytes rather than exporting uninitialized process memory. Flag bit 0 means arm; bit 1 means reset fault. Other flag bits are rejected.
The state record fills all 96 bytes: four 32-bit metadata fields followed by twenty floats. Its first fields are sequence, runtime timestamp, fault mask and enabled status. The floating values carry odometry, estimated attitude, average length, converted IMU data, wheel positions and knee angles. These IMU fields are SI measurements, not the original raw register bytes. The yaw-rate field is the gyro's rate about the vertical with its estimated bias removed, not the wheel differential.
The diagnostics record, added in ABI version 2, also fills 96 bytes. It goes out every BAL_DIAG_PERIOD (8) balance ticks, 50 Hz at 400 Hz, for commissioning and tuning.
- Eight 32-bit fields: sequence, timestamp, controller and drive fault masks, a flag word (enabled, left/right FOC saturation, geometry valid, sample delivered), the outstanding timing misses, the dropped state packets and the knee-tracking error count.
- Sixteen floats:
- commanded wheel torques and knee targets;
- the drive's four available-torque limits;
- VBUS, VMOT (the motor supply after the E-stop switch), VSERVO and 5 V;
- the pitch trim in use, the gyro yaw-bias estimate, the filtered axle acceleration and the height target.
A version 1 consumer must not read version 2 memory: bal_ipc_valid() checks the version and the total size.
10.3 The ownership invariant
Each ring has exactly one producer and one consumer. The producer alone changes the write index; the consumer alone changes the read index. A slot may be reused only after the consumer returns ownership by advancing read.
Let unsigned counters be w and r and capacity be N=4. Occupancy is w-r using modular unsigned subtraction. Empty means w=r; full means occupancy≥4. Slots are addressed with counter % 4. The indices keep advancing; they are not restricted to 0–3.
Because occupancy is bounded, ordinary counter wrap does not lose the distinction between empty and full. For example, producer counter 0xffffffff advancing to zero is compatible with a consumer counter near the same wrap boundary. An arbitrary reset of only one counter is not compatible with this invariant.
The general use of producer/consumer ownership and ordering is explained in the Linux circular-buffer documentation. This repository uses its own fixed packet layout and bounded-count implementation; it does not directly use the kernel macros shown there.
10.4 A packet publication
The producer reads the remote consumer index and acquires before deciding whether the target slot is free. It writes the 24 packet words, executes a release barrier, advances the write index and releases again before notification.
The consumer reads the producer index, acquires, copies the owned packet and releases before advancing read. The ordering prevents a peer from observing an index announcing a packet before the associated data is available, or reusing a slot before the prior reads have completed.
producer consumer
write payload words
release
publish write index ------------------------> observe write index
acquire
copy payload
release
observe read index <------------------------- publish read index
acquire
reuse slot when free
This diagram assumes the memory mapping actually exposes the writes to the other core. Barriers order accesses; they do not automatically turn a private dirty cache line into shared physical data.
10.5 Volatile, barriers and caches
volatile tells the compiler to perform the volatile accesses according to its language rules. It is not a replacement for interprocessor ordering or cache maintenance. A compiler barrier alone constrains compiler motion; a processor barrier constrains hardware ordering according to its instruction semantics. Cache clean/invalidate operations address a further visibility problem when caches are not coherent for the region.
The simplest supported contract here is a correctly reserved region mapped uncached by both processors, plus appropriate acquire/release callbacks that order normal shared-memory and mailbox MMIO accesses. The exact instructions and mappings belong to the board port. A RISC-V fence in one function does not cure an incorrect ARM mapping, and a Linux volatile userspace pointer does not configure memory attributes.
The kernel memory-barrier guide explains why access ordering and device/cache behavior must be treated carefully. This book's recommendation is specific to the current uncached transport contract; a cached variant requires a separate audited ownership/cache protocol.
The C implementation also should not be advertised as a portable lock-free multi-threaded host queue. Concurrent ordinary C threads accessing these volatile objects need language-level race and synchronization analysis. The sequential host tests establish ring arithmetic, not ISO C thread safety or AMP correctness.
10.6 Backpressure and notification
When a state ring is full, the runtime drops the new state packet and increments dropped_states locally. It does not overwrite unread slots. This preserves ownership but means an old packet can remain queued while newer ones are discarded. Linux must drain promptly and evaluate telemetry age.
At 400 telemetry packets per second, four slots fill in about 10 ms if initially empty and the receiver stops consuming. A nominal 50 Hz receiver that reads only one packet per timer tick cannot keep up. It should drain all available packets, often retaining the newest valid state for publication, while preserving sequence/drop diagnostics as needed.
A successful push sets notify_pending. If the mailbox is busy, one retry occurs on a later control tick. Linux should also periodically inspect/drain the ring to recover from lost/coalesced notifications. Commands are polled by the RTOS each control tick independent of mailbox arrival.
The command ring is likewise bounded, but command backlog is not useful history. A delayed arm or velocity request can be dangerous semantically even when its bytes are valid. The Linux producer should send current intent, avoid large external queues, and follow a restart handshake rather than replay old commands.
10.7 Sequence numbers, time and epoch
Packet sequence numbers let the runtime ignore duplicates and backward commands under a half-range modular comparison. They do not authenticate the sender or prove the age of a newly delivered command. The current command packet contains no source acquisition timestamp.
The shared header epoch distinguishes a coordinated session. It is immutable while endpoints run. If the runtime sees a changed epoch, it faults and disables. That is detection of an invalid live transition, not permission to reset shared data underneath the peer.
A valid restart procedure must stop outputs, quiesce both endpoints, reset the rings under one coordinator, choose a new epoch and restart from disabled state. A Linux process crash must not result in its replacement casually zeroing memory while the RTOS continues reading. If reliable hot restart is required, design a negotiated protocol and version it explicitly.
10.8 Validate the actual ABI
Compile both sides against an intentional ABI specification, inspect structure offsets and exercise recognizable packet patterns across the processors. Use values that reveal byte swapping, float interpretation and word tearing. Check that both endpoints reject an unexpected magic/version/size before processing payloads.
Do not blindly include the C11 header in a C++ ROS2 translation unit: _Alignas and _Static_assert spelling is C-oriented. A future Linux bridge can expose a C wrapper or define a compatible C/C++ wire header while retaining exact layout assertions. Such a header adaptation is integration work, not something the current ROS2 package already does.
Tests should cover full rings, prolonged receiver stalls, lost notifications, counter wrap, producer restart, malformed flags and mixed firmware versions. Perform target tests under Linux memory and I/O load to observe ordering and IRQ behavior that host tests cannot reproduce.
Exercises
- How long does an empty four-slot state ring last at 500 Hz without reads? Answer: roughly 8 ms to accumulate four packets.
- Why not overwrite the oldest state slot when full? Answer: the consumer may be reading it; latest-value semantics need a different ownership design.
- Does a sequence number prove a command was recently generated? Answer: no; an old queued command can have a newer sequence than the last consumed command.
- What makes a restart safe? Answer: coordinated quiescence and initialization, not just setting a new epoch field.
11. Porting the library to the SG2000
Prerequisites: Chapters 9–10 and familiarity with an embedded build.
Learning objectives: separate a portable library from bootable firmware, create a board resource-ownership plan and identify the evidence required for a working AMP deployment.
11.1 Preserve a known-good platform baseline
Record the board revision, boot mode, boot media, SDK repository revision, toolchain version, Linux kernel and device tree, and small-core firmware. These form one platform configuration. Changing the root filesystem while unintentionally replacing the kernel or boot firmware can change peripheral support and the AMP launch sequence.
Milk-V documents a large-core ARM/RISC-V selection on Duo S and lists the V2 SDK image family as supporting ARM variants for Duo S. Its official image family is Buildroot, which is distinct from the Ubuntu root filesystem desired for ROS2. Verify image choices against the vendor download page rather than treating an arbitrary ARM64 Ubuntu image as board firmware.
The project's library has no SDK checkout and no cross-compilation target. There is consequently no verified command in this repository that produces a complete bootable SG2000 firmware image. The steps below define the work required once a specific SDK revision is selected.
11.2 Divide hardware ownership explicitly
Linux and the RTOS must not independently configure the same I2C, PWM or interrupt resources. Create a reviewed ownership table before writing drivers:
| Resource | Intended owner | Integration evidence |
|---|---|---|
| Selected IMU I2C controller and data-ready IRQ | RTOS | Linux node disabled/reassigned; clocks and pins retained |
| Wheel and servo PWM channels | RTOS | Verified timer clock, pinmux and output behavior |
| SPI3 for the rotor and knee sensors (B13–B16, chip selects B18, B22, B21) | RTOS | Mode 1, 16-bit frames at 2–4 MHz; parity and error-flag counts at speed |
| I2C4 for the IMU and rail ADC | RTOS | Interrupt/DMA task; Linux node disabled |
| Driver fault and enable signals, E-stop re-arm (C20, J4-36) | RTOS/hardware interlock | Defined boot, watchdog and E-stop latch behavior (latch cleared at boot) |
| Shared-memory page | Both under protocol | Matching reserved map and attributes |
| Mailbox channel/commands | Both under one allocation | No conflict with vendor services |
| Network and ROS-facing interfaces | Linux | Userspace transport and process lifecycle |
Disabling a Linux device-tree node may prevent Linux from claiming a peripheral, but it does not automatically configure that peripheral for the RTOS. Clock/reset/pinmux initialization and interrupt routing still need an owner. Conversely, Linux power management must not turn off a clock the RTOS depends on.
The repository's port skeleton shows these callbacks, the 10 kHz fast ISR and the E-stop arm sequence written against a small HAL interface. It compiles for the C906 in CI (cmake/riscv64-unknown-elf.cmake) but has not run on hardware; the HAL is the SDK glue still to be written.
Use the actual board connector voltage domain and schematic. Do not assign pin numbers from another Duo model or from a visually similar connector. The hardware chapter explains why sensor logic voltage matters independently of motor supply voltage.
11.3 Reserve memory through all boot stages
Select an aligned physical region based on the actual memory map. Exclude it from Linux allocation and from RTOS heaps/stacks, firmware images, bootloader work areas and other reserved users such as multimedia buffers. The 1088-byte structure can fit in a page, but the chosen reservation granularity and mapping must follow the platform.
A device-tree fragment is meaningful only when its reg address/size cells match the real platform map. Likewise, a linker symbol must point to that same physical region and the small-core mapping must make it accessible. This book deliberately supplies no numerical physical address to paste into an unknown image.
A reserved-memory declaration describes ownership exclusion; it does not by itself guarantee that every mapping on every core is uncached. Establish the mapping attributes explicitly. Avoid overlapping cached and uncached aliases to the same region. Document who initializes the header, when the peer is allowed to read it, and how memory visibility is established before release from reset.
11.4 Bind the vendor mailbox without copying demo timing
Milk-V's RTOS mailbox example documents /dev/cvi-rtos-cmdqu, Linux rtos_cmdqu driver sources and FreeRTOS communication code. It distinguishes ordinary submission from a submission that waits for a reply. Those documented paths are starting points for locating the interfaces in the chosen SDK, not a promise that every revision has identical internals.
The real-time adapter must attempt notification without waiting for Linux. It must respect the SDK's mailbox locking and channel ownership; direct register writes that collide with vendor services are not a valid optimization. A nominally non-waiting API still needs inspection for internal locks, retries or interrupt masking before it can satisfy the runtime's bounded callback contract.
The mailbox ISR should acknowledge the relevant interrupt and signal data availability. It should not run a ROS callback, parse arbitrary pointers or execute the entire sensor/control stack. Validate message IDs and keep the robot's notification format distinct from other firmware services.
11.5 Integrate the portable sources
Add control.c, ipc.c, runtime.c and devices.c plus public include paths to the small-core build. Supply the selected math library and C11 support. Match the SDK's RISC-V ISA, ABI, alignment, startup and interrupt conventions rather than inventing flags from the processor name alone.
Provide one static bal_runtime, static configuration and the shared-region pointer. Initialize board outputs disabled, initialize drivers and clocks, complete sensor calibration, establish the shared session, then call bal_runtime_init. Do not start the periodic timer if initialization fails.
The configuration should come from a reviewed hardware-specific file with units and calibration provenance. The host test configuration is not that file. Compile-time constants make memory use predictable, but their physical meaning still needs validation.
Float operations require the correct floating-point execution and context behavior. If FreeRTOS tasks can use the FPU, ensure the port saves/restores required state across context switches and interrupt nesting. A controller that runs correctly alone can fail when another task begins floating-point work if context handling is wrong.
11.6 Bring up one boundary at a time
Begin with timestamp output or a diagnostic GPIO to establish clock rate and timer scheduling. Next run the IMU acquisition path while motors remain inhibited, then encoder capture, then output generation into an instrument or disconnected load. Introduce IPC only after the local observation path is understandable.
For the Linux side, add a transport layer that owns or safely cooperates with the mailbox driver, exposes a controlled packet interface, validates ABI/session state and maps reserved memory with the intended attributes. A raw /dev/mem prototype is not an adequate substitute for an explicit long-term driver contract, especially where untrusted addresses or concurrent access can enter the system.
The ROS2 process should not be the authority that determines device memory attributes or competes for interrupt ownership. It should consume a defined Linux transport API. That API remains to be implemented for this project.
11.7 A target acceptance record
Before declaring the port operational, preserve evidence for each boundary: the exact firmware revisions that boot, a map showing the shared region is excluded everywhere, validated cache attributes, consistent packet transfer under stress, acquisition-time measurements, output polarity and scaling, and watchdog-disable behavior.
A sample acceptance record should state the test condition and measured result rather than “works.” For example: “No torn payload patterns across N transfers under workload W” is a bounded observation; it does not prove all possible schedules. Record counterexamples and fault injection too, including Linux disconnect, mailbox busy, I2C error, stale sensor and target reset.
Exercises
- Why must the device tree and RTOS linker agree? Answer: otherwise one side may allocate or overwrite memory the other treats as shared protocol storage.
- Does selecting ARM boot install Ubuntu? Answer: no; boot architecture and Linux distribution are separate platform choices.
- Why inspect a non-waiting mailbox API implementation? Answer: it may still contain internal operations whose execution bound is unsuitable for the control deadline.
- What is a good first target milestone? Answer: a repeatable, measured timer/observation path with outputs inhibited, before closed-loop motor control.
12. Linux, ROS2 and the supervisory layer
Prerequisites: the system architecture and IPC chapters.
Learning objectives: design the ROS bridge, maintain command freshness, publish meaningful odometry and transforms, and separate supervisory intent from the RTOS's actual state.
12.1 Select a compatible software stack
The intended ROS pairing is Ubuntu 24.04 ARM64 with Jazzy, with Ubuntu 22.04 ARM64 and Humble as an alternative if a proven board integration uses that release. ROS support applies to an operating-system/architecture combination, not automatically to the Duo S boot chain or peripherals. The authoritative platform combinations are maintained in REP 2000.
The project has not verified a ready-to-flash Ubuntu image that preserves the required Duo S kernel, mailbox and RTOS integration. Begin from a known-good platform and validate root-filesystem compatibility deliberately. Keep a minimal ROS installation on the board; visualization, simulation and computationally expensive planning can run on a development computer.
There is currently no ROS2 package in this repository. The following interfaces are a proposed design contract. Commands such as ros2 topic list become meaningful after that package and the Linux transport have been implemented and launched; they are not evidence that a node already exists.
12.2 Responsibilities of the bridge
A Linux bridge translates between ROS messages and the fixed IPC records. It should not recompute the fast pitch controller or access IMU registers concurrently with the RTOS. Its responsibilities are command validation, bounded transport, telemetry interpretation, clock conversion, ROS publication and diagnostics.
A practical process may contain a ROS executor and a transport worker, with an explicit ownership handoff between them. The command-ring producer must remain single-owner even if several ROS callbacks update desired state. Either serialize production or stage the latest intent in an internal synchronized object that one transport writer reads.
Likewise, one state-ring consumer should own its read index. Multiple subscribers do not each consume the hardware ring; the bridge consumes once and publishes messages to ROS. This distinction preserves the SPSC assumptions described in Chapter 10.
12.3 Command interface
A proposed interface is:
| ROS input | Meaning | Mapping |
|---|---|---|
/cmd_vel |
Planar velocity request | linear.x → speed, angular.z → yaw rate |
| Explicit height/length request | Desired leg geometry | Metres → command height field |
| Mode request | Disabled, stand, crouch, drive, recovery | Validated mode enum |
| Arm request | Permission to enter active balancing | Arm flag, separate from speed |
| Reset request | Clear a recoverable latch while disarmed | Reset flag under stated conditions |
Choose one clear height interface instead of silently interpreting an arbitrary joint trajectory as a body-length request. A crouch factor in [0,1] requires an explicit conversion, such as l_target=l_max-c*(l_max-l_min), followed by validation. It remains a leg-length request under the current model, not an independently measured world-height target.
The bridge must reject nonfinite values and unsupported components. It should normalize and check units before sending a record, while retaining the RTOS validation as a second boundary. Parameter changes that affect physical gains or geometry should be applied only through a deliberate disabled reconfiguration procedure.
12.4 Freshness has two layers
A transport timer can continue running after teleoperation has stopped publishing. If it repeatedly sends the last nonzero velocity, the RTOS receipt-time watchdog remains satisfied even though the operator command is stale. Maintain a separate monotonic timestamp for the last accepted ROS command.
At each transmission, compare command age with the supervisor's policy. If stale, generate the explicitly designed stop/disable intent rather than republishing old motion. The present RTOS timeout removes output; a graceful balancing stop would be additional behavior that needs implementation and validation.
A conceptual producer looks like this:
on ROS command:
validate units, range, mode and source authority
store latest intent and local monotonic reception time
on transport opportunity:
inspect telemetry session/fault information
if latest operator intent is stale:
select the defined stale-command policy
serialize one current command with a new sequence
try enqueue once; do not accumulate historical commands
Receiving a high-rate stream does not automatically establish authority. If a navigation node and teleoperation node can both command motion, arbitrate them at one supervisory point and expose which source is active.
12.5 QoS is not the motor watchdog
ROS2 quality-of-service policies control communication behavior such as history, reliability, durability and deadline events. A sensor-data profile commonly favors fresh data over retransmission history, while command channels require deliberate choices about compatibility and stale samples. Deadline or liveliness events can help diagnostics but do not themselves invoke the robot's stop policy. See the official ROS2 QoS reference.
For this project, use small bounded histories and explicit freshness checks regardless of middleware reliability. Reliable delivery of an obsolete motion command is not a useful guarantee. Test the actual publisher/subscriber QoS pairing; incompatible policies can make a topic appear present without passing data.
12.6 Interpret odometry correctly
The RTOS exports x, y and yaw derived from wheel motion. These are local dead-reckoning estimates, not a global map pose. Publish pose in an odometry frame and twist in the appropriate child frame, following the nav_msgs/Odometry definition.
An illustrative 2D publication uses:
pose position = (x, y, 0)
orientation quaternion = (0, 0, sin(yaw/2), cos(yaw/2))
forward twist = measured average wheel speed
yaw twist = measured differential wheel speed / track
The child frame needs a carefully chosen origin. Wheel-axle velocity is not automatically the velocity of a moving torso origin when knees or body pitch change. A common design is an axle/planar base frame for wheel odometry and a separate articulated body transform. Define the geometry explicitly instead of assigning the same twist to every link.
Do not publish zero covariance merely because the current packet has no covariance fields. Zero entries are often interpreted as excessive certainty. Estimate uncertainty from repeatability and a stated model; if a downstream estimator requires a covariance the system cannot justify, extend the measurement model first.
12.7 Build a transform tree without duplicate authorities
A proposed frame relationship is:
map -- external localization --> odom
odom -- wheel odometry --------> base_footprint or axle_base
base frame -- measured mechanism/attitude --> body/base_link
body -- calibrated rigid mount --> imu_link
body/legs -- joint geometry ----> wheel and knee frames
The odom frame should remain locally continuous while global localization may correct map-to-odom. The body frame belongs to the robot. These roles follow REP 105; selecting exact additional frame names is a project design decision.
Only one component should publish a given transform edge. Publishing the same edge from both the odometry bridge and a second estimator creates contradictory authorities. Also avoid counting the same wheel information twice as if it were independent evidence when feeding multiple derived measurements into a fusion node.
The existing telemetry lacks complete body pose and mechanism details for an exact articulated transform tree. Pitch/roll estimates plus mean leg length do not by themselves describe an unconstrained two-link hip. A finalized mechanism and measured joint state are prerequisites.
12.8 Time conversion and restarts
The wire timestamp is a 32-bit RTOS runtime-start timestamp, not ROS wall time and not necessarily sensor acquisition time. Extend wrap within a session and estimate an offset, and potentially clock drift, between RTOS time and Linux monotonic time. Map to ROS time only under a clear policy for system time changes and simulated time.
If telemetry arrives after a variable queue delay, reception time is a poor substitute for production time. A clock handshake with bounded round-trip delay can estimate offset uncertainty. Record that uncertainty rather than assuming sub-millisecond agreement from a single exchange.
On epoch change, discard the previous timestamp extension, sequence interpretation and any assumption of odometry continuity. Notify consumers that the source session changed. Do not reconnect and continue broadcasting an apparent jump as if it were ordinary measured motion.
12.9 Supervisory state and diagnostics
The RTOS wire record includes enabled and fault bits but no actual mode field. The bridge can report requested mode and observed enabled/fault state separately. It cannot honestly claim a confirmed actual mode solely by echoing its last request. Add explicit acknowledgements or a versioned status field if mode confirmation is required.
Useful diagnostics include command age, state age, sequence gaps, ring fullness, mailbox retry counts, epoch changes, driver faults, supply voltage and configuration identity. Some are not present in the current packet and require extension or local counters. Expose the measurement source for each diagnostic so operators can distinguish an observation from an inference.
Exercises
- Why can repeated transmission defeat an operator timeout? Answer: the RTOS sees fresh arrivals even when the underlying ROS command is old.
- Is
/odomglobal localization? Answer: no; wheel dead reckoning accumulates error and needs an external reference for global corrections. - Can enabled=true prove DRIVE mode? Answer: no; STAND and CROUCH can also be enabled, and the current wire record lacks mode confirmation.
- Why distinguish axle and body twist? Answer: body rotation and joint motion can produce different linear velocities at different origins.
13. Simulation, regression tests and evidence
Prerequisites: the LQR, runtime and IPC chapters.
Learning objectives: choose tests that answer specific questions, run the included numerical lab, interpret its limits and plan target fault injection.
13.1 There is no single test called “the robot works”
Different tests establish different properties. A kinematic round trip checks algebra. A closed-loop simulation checks a model and controller combination. A hardware timing measurement checks an implementation under observed workloads. A supported physical experiment checks behavior within its mechanical and environmental conditions.
Keep those claims separate in reports. A convincing engineering case is a chain of bounded results, not a large list of passing commands whose relation to the robot is unclear.
13.2 Existing host coverage
The unit-test executable, balance_tests, covers FK/IK consistency, ground-referenced wheel travel, the drive's voltage-limited torque and its use as the controller's limit, some configuration rejection, arming/reset behavior, invalid samples, command expiry, timing faults and the tolerated-miss policy, clock wrap, fall limits, ring full/empty/wrap behavior, runtime notification backpressure, epoch mismatch, sensor-gap recovery, gain selection and torque/knee bounds, gyro yaw and its bias estimate, pitch trim, CROUCH, servo-rail droop, AS5048A frame decoding, dropout coasting, gear efficiency by quadrant and the diagnostics ring, in synthetic cases.
These are valuable regression tests for the portable library. They do not cover every boundary combination, native drivers, real-time scheduling, cross-core races, sensor calibration quality, joint strength or physical stability. The source test functions are the authority for exact coverage; a table in a document should never silently expand it.
Closed-loop simulation of the C code
balance_closed_loop (tests/integration/test_closed_loop.c) runs the real bal_runtime_tick at 400 Hz and bal_drive_fast_tick at 10 kHz against a nonlinear planar wheel-pendulum with body reaction torque, the plant that design_lqr.py linearizes. The plant is the design robot of design/robot_params.json: 40 mm wheels and 80/80 mm legs at the middle knot, with estimated masses. The sensors are modelled where the port will connect them: 14-bit AS5048A replies with parity, one 100 µs frame old; BMI323 raw bursts at their scale factors with noise; and knee AS5048A replies, fresh every tick. The motor current is the dq steady state of the PWM duties actually applied, so angle error, back EMF and the SVPWM voltage limit act on the plant. The gains come from tools/control/design_lqr.py and tests/fixtures/closed_loop_plant.json, which is tools/control/robot_params.py lqr (make params-check fails if they drift apart).
Four scenarios must pass: release from a 0.05 rad lean at 14.8 V and at 12.2 V, and again with every 50th left and 70th right rotor frame lost, each of which must settle with odometry equal to ground travel and every command within the drive's published limits; and driving at a commanded 0.2 m/s. It found a real problem while being written: without the accelerometer's axle-acceleration compensation (6.4), recovery settles into a slow, lightly damped oscillation.
The masses are estimates, and the model omits knee dynamics, yaw, slip, current dynamics, bridge dead time and timing jitter. It checks signs, units, frames and limits end to end. It does not establish that the physical robot balances.
After a control change, choose tests that would fail if the new behavior were wrong. A test that merely repeats the implementation's own formula with the same mistake provides weak independent evidence. Dimensional checks, known physical signs and analytical special cases are often stronger oracles.
13.3 Reproducible offline lab
The book supplies an illustrative plant input and a Python experiment:
python3 docs/book/examples/lqr_lab.py --output build/book-lab
The experiment loads the supplied synthetic plant, obtains a discrete LQR at the middle knot, checks its poles and propagates the discrete linear plant from a small pitch disturbance. It writes a CSV trajectory and a standalone SVG figure showing pitch, position and requested/applied total torque. The default run is fixed geometry with a total torque limit.
To explore delay without changing source:
python3 docs/book/examples/lqr_lab.py --delay-steps 1 --output build/book-lab-delay
This is a model experiment, not a test of bal_step. It uses exact model states, omits the complementary filter and models only the frozen planar system. It helps you ask whether delay and saturation matter before building a more complete simulator; it does not answer whether the actual C controller and actuators will balance. The C simulation in 13.2 is the complement: the real code against a simpler-than-reality plant.
The input file deliberately uses 150 mm-class geometry consistent with the existing mathematical examples, not the 80 mm CAD links. Every plot and report identifies the plant as synthetic. Preserve input, software versions and output when comparing runs.
The figure was generated by the included lab with zero added delay. Its recorded input and numerical summary preserve the example conditions.
13.4 Read a trajectory quantitatively
For a response θ[k], calculate peak absolute angle, settling behavior, control saturation duration and remaining position offset. Look at torque and angle together. A small angle achieved by demanding impossible torque is not a successful design.
A settling time requires a defined tolerance and observation interval. If the criterion is abs(θ)<0.005 rad for the rest of the run, state that criterion. Do not infer infinite-horizon stability from a finite-duration plot. A slowly growing mode can look harmless for a short experiment.
For discrete eigenvalues λ, magnitudes below one establish asymptotic stability of the unsaturated time-invariant linear model. Saturation and delay alter the closed-loop system; therefore plot and analyze the actual simulated update rule, not only the ideal eigenvalues printed before those effects were introduced.
13.5 Build the next simulator in layers
Start by wrapping the actual C controller with a plant simulator so its state management and fault paths are exercised. Use an independent nonlinear plant implementation based on the equations in Chapter 7. Integrate the plant at a sufficiently smaller step and perform a convergence study by reducing that step until results stop changing materially.
Add one physical effect at a time: output delay, motor voltage/current dynamics, wheel friction and slip, IMU noise/bias, accelerometer translational terms, encoder quantization, knee lag, geometry changes and communication disturbances. Each layer should include a test showing that the effect is actually active.
A simulator that derives both its sensor outputs and its estimated state from the same erroneous sign convention can hide an error. Keep an independent sign/units reference and compare controlled special cases: static inclination, pure translation, constant-rate rotation and known wheel displacement.
13.6 Parameter uncertainty and sensitivity
Measured mass, inertia, radius and motor constants have uncertainty. Sweep plausible ranges rather than treating one estimate as exact. Record which uncertainty ranges are measured and which are exploratory. For each sample, evaluate both stability-related behavior and actuator feasibility.
A Monte Carlo experiment can reveal failure regions but cannot prove their absence outside the sampled points. Structured worst-case searches or robust analysis may be needed if the operating envelope is tight. Chapter 15 explains why gain scheduling adds another dimension: both the geometry value and its rate of change matter.
A simple sensitivity question is useful early: if effective wheel radius is 5% different, what changes in predicted speed, odometry and required voltage? Following that one parameter through all layers can expose inconsistent calibrations before a broad numerical sweep.
13.7 Fault injection matrix
| Injected condition | Expected property to observe | Test level |
|---|---|---|
| Nonfinite IMU value | No enabled output; sensor fault | Host and runtime |
| Old acquisition timestamp | Stale data rejected | Host and target |
| Lost Linux command stream | Defined timeout/disable behavior | Integration |
| Full state ring | Balance invocation continues without wait | Host and target |
| Busy mailbox | Pending notification retried once per tick | Host and target |
| I2C device unresponsive | Bounded acquisition failure, no control hang | Target |
| Driver nFAULT | Output inhibited through adapter | Target |
| C906 stops executing | Hardware watchdog disables drive | Hardware |
| Linux process restart | No stale-arm replay or live ring reset | Integration |
| Joint feedback disagrees with command | Geometry policy detects/contains problem | Future physical integration |
Do not claim every row passes because one related host assertion exists. Attach the exact test procedure and observed result to each row when it is performed.
13.8 Long-duration and boundary behavior
Run long enough to cross time and sequence boundaries or accelerate those boundaries in controlled tests. A 32-bit microsecond timer wraps roughly every 71.58 minutes; a short demo will not naturally cover it. Likewise, full-ring and restart tests should deliberately establish the relevant index relationships rather than waiting for chance.
Check sustained thermal and supply conditions. A mechanism that balances for ten seconds on a fresh supply may fail after motor heating, battery sag or servo temperature rise. Those are changes in the plant and actuator envelope, not merely software performance concerns.
Exercises
- Why simulate with a smaller integration step than control period? Answer: the plant evolves continuously between held controller updates; convergence testing reduces numerical integration error.
- What does a stable unsaturated pole calculation omit? Answer: saturation, nonlinear motion, disturbances, estimator/actuator effects and any unmodeled delay.
- Why keep a synthetic plant file in the book? Answer: it makes the mathematical lab reproducible without implying a calibration for unknown hardware.
- What evidence distinguishes a watchdog test from a software-disable test? Answer: control execution is deliberately prevented and the independent hardware path must still remove drive.
14. Commissioning and tuning through measured stages
Prerequisites: Chapters 3–13.
Learning objectives: turn an assembled prototype into an identified system, establish polarity and geometry, and tune within an evidence-based operating envelope.
14.1 Commissioning is model verification
A robot is not ready for balancing just because it is assembled and the compiler succeeds. Commissioning establishes the relationship between software quantities and physical behavior: metres per encoder count, torque per ampere, actual knee angle, IMU orientation, output polarity, timing and attainable actuator effort.
The exact procedure depends on the final mechanism and power system. Begin with a support fixture that permits the intended observation while preventing uncontrolled motion. Keep the operator able to remove wheel drive independently of the application. These provisions are part of making measurements meaningful, not a substitute for choosing appropriate hardware.
The current code's commissioned flag is an application input. Document who sets it, under which hardware revision and calibration file, and which evidence allows that setting.
14.2 Stage A: inspect and measure the unpowered assembly
Measure link pivot distances, joint range, wheel radius under load, track width and total/component masses. Check that the hip mechanism physically enforces the geometry assumed by the model. Move the joints by hand through their intended range and inspect cable travel, bearing alignment and wheel/leg clearance.
Printed parts need inspection after cooling and assembly. Hole dimensions, layer orientation, fastener clamp force and bearing support can change the mechanism's stiffness and backlash. The OpenSCAD starter is a packaging mockup; it does not establish those load paths.
Record the mass distribution for several fixed knee configurations. Convert CAD millimetres to metres and CAD inertia units to kg m² explicitly. Chapter 5 shows the parallel-axis calculations. A large discrepancy between predicted and weighed mass is a reason to revisit the CAD material assignment before generating gains.
14.3 Stage B: observation-only electronics
Power the logic and sensors with wheel drive inhibited. Verify supply levels and input voltage compatibility. Confirm BMI323 identity (chip ID 0x43) and acquisition timing. Rotate the robot about each positive body axis and compare raw/calibrated measurements with the frame definitions.
Keep the robot stationary during gyro bias estimation. Repeat the estimate after warm-up and across sessions to assess stability. Check both magnitude and sign of accelerometer readings in known orientations. A plausible norm cannot reveal all axis mistakes.
Turn each wheel manually and verify forward-positive counts on both sides. Count a known number of physical revolutions and compare with the software scale. The phrase “encoder counts per revolution” may refer to electrical pulses, decoded edges or motor-shaft revolutions; resolve it experimentally and include gearing exactly once.
14.4 Stage C: validate output polarity and actuator conversion
With wheels supported clear of contact and at a deliberately restricted output level, verify each motor direction and corresponding encoder sign. This experiment verifies electrical direction; it does not verify balancing response under load. Reverse the mapping in one documented place if needed, rather than scattering sign changes through the estimator and controller.
Measure battery/motor-terminal voltage, speed and current where suitable instrumentation is available. The voltage-mode FOC in foc.c approximates phase voltage from torque, resistance, back EMF, inductance and supply. The carrier's DRV8313 stages have no current sensing, so the drive cannot verify delivered torque. Current limiting, bridge drops, winding heating and friction can all invalidate an ideal feedforward prediction.
A nominal stall specification should not be treated as a continuous operating point. Motor, driver, wiring and power source must all satisfy the intended current and thermal envelope. Establish an attainable torque/speed region before selecting the control saturation limit.
14.5 Stage D: freeze leg geometry
The first balancing model should use mechanically fixed, measured legs or trustworthy joint feedback with the geometry held constant. Ordinary servo command angle alone does not justify geometry_verified=true. If using a fixed fixture, the actuator adapter must also prevent accidental knee commands from changing that verified geometry.
Match the controller configuration to that fixture: link lengths, operating range, track, wheel radius, CoM height and inertia. The code requires a nonzero configured geometry interval and ordered three-knot table even when early testing is physically fixed at one point. Satisfy the interface structurally while treating only the tested geometry as commissioned; do not infer approval of the whole interval.
Generate a gain table from the identified model and examine the frozen plant's poles and torque demands. Then run simulations including anticipated delay, supply limitations and estimator behavior. The LQR Q/R choices should reflect acceptable errors and actuator effort, not imitate numbers from another robot.
14.6 Stage E: verify timing before closing the loop
Measure the control release period, sensor age and output latency with concurrent Linux work. Exercise error paths and communication backpressure. A control law derived at 2.5 ms should not be deployed with an accidental 5 ms timer while retaining the same gain table.
Check that disabling the Linux process, delaying a sensor update, triggering a driver fault and stopping the real-time task each lead to the designed outcome. These experiments verify different paths. A functioning software fault does not establish an independent watchdog response.
14.7 Stage F: first supported balance observations
Use the support fixture to observe small motions around the modeled upright equilibrium. Start with deliberately restricted operating conditions and a recorded stop criterion. Examine pitch sign, pitch rate, requested torque, applied electrical output and measured wheel response together.
If a small forward lean commands wheels backward under the established conventions, stop and resolve the sign chain. Do not try to hide an inconsistent sign by increasing a gain. If the response oscillates, inspect delay, motor deadband, estimator lag and mechanical compliance before assuming the pitch coefficient is the only cause.
The current wire format does not include applied torque or electrical output. Add a bounded observation channel or an instrumented adapter if tuning needs those signals. Comparing a requested torque to measured pitch without knowing actuator saturation can lead to incorrect conclusions.
14.8 Tune by hypotheses
A useful tuning record states a hypothesis, a single change and a measurable outcome. For example: “Oscillation appears after adding a 10 ms smoothing filter; reduce that added delay while leaving gains unchanged and compare phase/overshoot.” This is more informative than changing five gains until one run looks better.
Increasing an LQR state penalty often increases the controller's willingness to spend effort on that error, but the resulting gain changes are coupled. Increasing R generally discourages effort within the modeled optimization. Always regenerate and inspect the full K vector and closed-loop result.
For the complementary filter, a slower accelerometer correction reduces acceleration contamination but permits more gyro drift. For the speed reference, smaller permitted acceleration or a future reference filter can reduce violent demand, but that filter must be included in the validation. No tuning change should be judged only by one stationary observation.
14.9 Introduce turning and geometry changes separately
After fixed-geometry balance is repeatable, test small yaw requests while measuring the remaining torque headroom. The implemented allocator reduces yaw authority when balance demand is large, so sluggish steering during a disturbance can be expected rather than a fault.
Only then evaluate knee motion with actual geometry feedback and a physically constrained mechanism. Begin with small, slow changes through validated configurations. Check whether the measured CoM/inertia map remains adequate and whether servo lag creates asymmetric legs. Expand the tested rate range deliberately rather than assuming that static gain stability supports any crouch speed.
A successful moving-knee experiment should log actual joint angles, desired angles, estimated body attitude, wheel response, saturation, supply and timing. If these cannot be observed, diagnosing a fall becomes guesswork.
14.10 Preserve a commissioning record
Record hardware and firmware revisions, plant measurements, sensor calibration, motor conversion parameters, gain table provenance, allowed geometry/rate range, timeout policies and passed/failed test conditions. Save the raw logs. Recommission relevant parts after moving a battery, changing tires, replacing a servo or modifying a printed link.
A configuration is an engineering artifact: version it and make the active identity observable in diagnostics. A future runtime protocol should support reporting that identity, because the present packet does not tell Linux which physical calibration is running.
Exercises
- Why freeze the legs initially? Answer: it removes geometry-motion dynamics and makes the first plant easier to identify and validate.
- What distinguishes requested torque from delivered torque? Answer: electrical limits, current dynamics, supply, motor constants and losses intervene between the command and mechanical output.
- Why retest after moving the battery? Answer: CoM, pitch inertia and possibly wiring/power behavior have changed.
- What is the first response to an apparent feedback sign error? Answer: trace and verify coordinate, sensor, encoder and actuator signs; do not compensate with arbitrary gain changes.
15. Advanced control and research directions
Prerequisites: the entire modeling, estimation, runtime and validation sequence.
Learning objectives: understand when the reduced controller needs a richer model, formulate candidate extensions and define the evidence each extension would require.
Every method in this chapter is a proposed extension. The repository currently implements a complementary filter and scheduled four-state wheel feedback, not an EKF, MPC solver, full multibody controller or automatic recovery system.
15.1 Identify the missing physics before adding complexity
A more sophisticated algorithm cannot rescue an incorrect coordinate frame or unknown actuator scale. Begin advanced work with a residual: the difference between a model prediction and measured behavior under recorded conditions. Ask whether the residual correlates with wheel speed, knee rate, supply voltage, temperature, direction reversal or elapsed time.
Speed correlation may indicate back EMF or friction. Knee-rate correlation may indicate omitted internal motion. Direction-dependent behavior may indicate backlash or asymmetric drive. A persistent angular offset may come from an IMU mounting error rather than insufficient controller state dimension.
Fit additional model structure only when data can identify it. Adding many poorly observable parameters can make an estimator numerically impressive but physically unreliable.
15.2 From frozen geometry to multibody dynamics
The reduced model treats h and I as constants at each operating point. A moving mechanism has generalized coordinates such as wheel travel, body pitch, hip coordinates and left/right knee angles. A general constrained rigid-body form is:
M(q) q̈ + C(q,q̇) q̇ + g(q) = B(q) τ + J_contact(q)^T λ
Here M is the configuration-dependent inertia matrix, C captures velocity-related effects under a chosen representation, g is gravity loading, B maps actuator torques and λ represents contact forces. Constraint equations and contact conditions complete the model. Position-controlled hobby servos add their own internal dynamics between the commanded knee position and actual torque.
Even a scalar expression illustrates the problem: if angular momentum is approximated by I(t) θ̇, its derivative includes I θ̈ + İ θ̇. A frozen-I controller omits the second term. The full coupled mechanism generally has additional terms beyond this illustration; do not add only İ and declare the multibody model complete.
For useful identification, measure actual joints, body motion and inputs during slow and fast configuration changes. Compare predicted reactions with data while keeping wheel contact conditions known. If hip motion is unconstrained and unmeasured, an average knee length is not enough state to describe the geometry.
15.3 Gain scheduling as a parameter-varying system
Write the closed-loop frozen model as e[k+1]=F(ρ)e[k], where ρ denotes leg configuration. Each fixed F can be stable while switching or time variation still produces growth. Eigenvalue checks at knots therefore answer a narrower question than stability during a crouch trajectory.
One stronger condition is a common positive-definite matrix P satisfying
F(ρ)^T P F(ρ) - P < 0
throughout a specified family. Then the same quadratic measure V=e^T P e decreases for that family under the modeled update assumptions. Certifying the condition over a continuous uncertain family requires an appropriate analytical or computational argument; checking a finite grid alone is still a grid check.
A parameter-dependent P may reduce conservatism but introduces terms associated with changing ρ, so bounds on geometry rate matter. The MIT Lyapunov-analysis notes provide a primary educational treatment of using decreasing functions to establish stability. Applying such analysis here requires a plant and uncertainty set grounded in measurements.
An achievable early research question is: “For which verified geometry interval and knee-rate bound does this controller retain a specified margin under measured actuator delay?” That question is more useful than claiming stability for every possible crouch motion.
15.4 Integral action and disturbance estimation
Constant disturbances can leave tracking offsets. An LQI design augments the state with an integrated tracking error, for example:
η[k+1] = η[k] + T * (v[k]-v_reference[k])
u[k] = -K_z z_error[k] - K_η η[k]
Check augmented-system stabilizability before solving the new Riccati problem. Choose which quantity is integrated carefully: integrating velocity error while independently constraining a position reference can introduce competing objectives or redundant structure.
Actuator saturation requires anti-windup or another state-management policy. Otherwise an integrator can keep accumulating while the output cannot increase, producing an excessive response when the actuator leaves saturation. Define initialization, freeze/reset conditions and behavior during mode changes. The present position-reference integration is not this LQI state and should not be described as such.
An alternative is a disturbance observer that estimates a slowly varying torque bias. It still requires observability and a justified process model; it cannot separate every possible disturbance from an incorrect motor constant without additional information.
15.5 Delay as a state
If the plant applies the previous input, the actual update is z[k+1]=A_d z[k]+B_d u[k-1]. Augment the state with the stored input to make that delay explicit:
z_aug[k] = [z[k], u[k-1]]
z_aug[k+1] = [[A_d, B_d], [0, 0]] z_aug[k] + [[0], [1]] u[k]
Designing against this augmented model can account for a fixed one-step delay. Variable delay and missed updates require a broader model or robust bounds. Reducing avoidable delay in the driver is often a better first step than asking the controller to tolerate a needlessly slow pipeline.
A numerical example makes the scale clear: at 400 Hz, two missed update intervals add 5 ms. At a 15 Hz motion component, that pure delay contributes 27 degrees of phase lag. Whether the remaining margin is adequate depends on the full loop.
15.6 State estimation beyond complementary filtering
A candidate estimator might include body attitude, gyro bias, wheel motion and selected geometry states. An extended Kalman filter predicts the nonlinear state and covariance, linearizes measurement relations, and corrects them using residuals. Its core structure is:
x_minus = f(x_previous, u)
P_minus = F P_previous F^T + Q_process
innovation = y - h(x_minus)
S = H P_minus H^T + R_measurement
L = P_minus H^T S⁻¹
x_plus = x_minus + L * innovation
Use numerical solves rather than explicit matrix inverses. A Joseph-form covariance update can help preserve symmetry/positive-semidefinite structure in finite precision. Process and measurement covariance values describe uncertainty assumptions; they are not arbitrary knobs whose only purpose is making plots smooth.
An estimator cannot observe absolute heading from gravity alone. Wheel contact and non-slip assumptions may provide useful constraints, but they fail when the wheels lift or slide. Innovation tests can detect some inconsistent observations; they do not prove which sensor is correct without a fault model.
For implementation, define timestamp handling, delayed measurements, initialization, numerical bounds and a failure fallback. Adding an EKF to the fast loop also changes its memory and execution budget. Measure the implementation on the target before promoting it into the deterministic path.
15.7 Model predictive control
MPC repeatedly predicts a finite trajectory and optimizes a sequence of inputs, applies the first input, then replans from the next state estimate. It can represent torque, speed, tilt and knee-rate constraints explicitly, which is attractive when balance and body-height requests compete.
A typical formulation penalizes state error and effort over N steps subject to the chosen discrete dynamics and bounds. Terminal cost/constraints, solver behavior and feasibility handling matter; a cost function alone is not a stability proof. The MIT trajectory-optimization chapter is a primary introduction to optimization formulations useful for this direction.
On the C906, establish a hard solver-time budget, bounded iterations and a validated fallback controller. An occasional failed optimization should not leave the last torque applied indefinitely. Warm starts can improve typical speed, but typical speed is insufficient evidence for a deadline guarantee.
A practical division may place slower trajectory/reference optimization on Linux while retaining the small deterministic balancing controller on the RTOS. Communication delay and feasibility checks must then be included in the reference contract.
15.8 Current control and actuator models
The cleanest route from commanded wheel torque to predictable actuation is a measured current loop with a characterized torque constant and drivetrain model. This generally runs faster than the body balance loop and uses current sensing, synchronized PWM sampling, electrical limits and anti-windup.
The DRV8313's overcurrent latch and the firmware's estimated-current clamp should not be confused with an accurate torque servo. Low-side sense resistors on the DRV8313's PGND1–3 pins with a current loop, or a motor with its own current-controlled driver, are the upgrade paths. Board wiring and accessible measurements determine what can be implemented. Back EMF limits torque at speed because finite supply voltage must overcome both resistive drop and generated voltage.
Identify friction and gear efficiency under the relevant direction/load conditions. A single constant efficiency may be adequate near one operating point and poor near zero speed or reversal. A richer actuator model is justified when it improves prediction on independent measurements, not only the data used to fit it.
15.9 Contact loss, recovery and hybrid behavior
When a wheel loses contact, the rolling constraint changes. Applying the same balancing law can accelerate a free wheel without producing the expected body correction. Recovery from a fall likewise involves contact changes and large attitude excursions outside the linear model.
These behaviors call for a hybrid supervisor with explicit modes, guards and reset behavior tied to measurable conditions. A mode name alone is not an implementation. Define what sensors establish contact, what each mode may command, and how transitions avoid discontinuous or stale actuator intent.
The current RECOVERY state disables outputs. A future self-righting maneuver must be designed around the actual mechanism, available torque and support contacts, then tested as a separate controller with a justified transition back to balance.
Exercises
- Why do stable frozen poles not prove moving-geometry stability? Answer: the matrices vary over time and the resulting transitions can amplify state despite stable individual members.
- What extra term appears when differentiating
I(t) θ̇? Answer:İ θ̇, in addition toI θ̈. - What must an MPC implementation do when it misses its solution deadline? Answer: take a defined bounded fallback action validated with the plant and actuator constraints.
- Why is sensor addition sometimes more valuable than estimator complexity? Answer: independent information can make previously unobservable states identifiable; a more elaborate algorithm cannot create that information.
16. Troubleshooting, maintenance and reproducibility
Prerequisites: the implementation and commissioning chapters.
Learning objectives: diagnose failures by layer, interpret fault bits and preserve the evidence needed to maintain a changing robot.
16.1 Begin with the first violated assumption
A visible fall is the final symptom of a chain. Start with the earliest unexpected event in a timestamped trace: missing acquisition, wrong geometry, expired command, increased delay, output saturation or driver fault. Tuning the last visible oscillation without this sequence often hides the real cause.
Collect configuration identity, session epoch, sequence numbers, sensor times, runtime timing, state estimates and actuator observations where available. The current packet lacks several of these, so distinguish directly available fields from those requiring additional instrumentation.
16.2 Interpret the fault mask
| Bit/value | Name | Questions to ask first |
|---|---|---|
| 1 | BAL_SENSOR |
Is the snapshot valid, finite, fresh and geometrically verified? Did sample time reverse or gap? |
| 2 | BAL_TIMING |
Did a tick interval leave 0.5–2 × dt, did more than max_timing_misses intervals leave 0.75–1.25 × dt without enough good ticks between them, or did the runtime exceed its budget? The diagnostics packet carries the outstanding misses |
| 4 | BAL_FALL |
Are pitch/roll outside limits, or are signs/units/IMU mounting wrong? |
| 8 | BAL_COMMAND |
Did command age expire, protocol fields fail or epoch/header change? |
| 16 | BAL_GEOMETRY |
Are FK lengths inside the configured range and IK requests reachable? |
| 32 | BAL_ACTUATOR |
Did output computation become nonfinite or the actuator callback fail? |
| 64 | BAL_SUPPLY |
Did VSERVO stay below min_vservo for more than vservo_droop_ticks armed ticks (both knees stalled, or the 6 V buck in current limit)? |
Bits can coexist. BAL_SENSOR can result from geometry_verified=false, so the name is broader than an IMU bus error. BAL_COMMAND similarly groups several communication/command causes. A future diagnostic extension should carry more detailed reason codes while retaining a stable high-level fault classification.
A reset can clear a software latch after the stated checks, but repeated reset requests do not solve persistent driver or mechanical failures. Preserve the original event information before clearing it.
16.3 Symptoms across the layers
| Symptom | Plausible cause | Discriminating observation |
|---|---|---|
| Cannot arm | Not commissioned, invalid geometry, stale command, tilt or fault | Inspect each arming predicate from Chapter 8 |
| Falls faster when enabled | Sensor/actuator sign error or gross model mismatch | Supported direction/sign experiment |
| Oscillates at fixed geometry | Delay, gain choice, compliance, backlash, estimator lag | Compare timing, torque saturation and phase |
| Good upright estimate at rest, wrong under motion | Acceleration contamination | Compare acceleration vector and gate behavior |
| Drives steadily despite zero request | Bias, asymmetric motors, wrong reference, disturbance | Trace reference, wheel scale and attitude offset |
| Turns when commanded straight | Wheel-radius/polarity mismatch, friction asymmetry | Independent wheel speed and distance calibration |
| Odometry jumps | Counter/reset or timestamp/session interpretation | Compare raw counts, epoch and sequence |
| Works locally but fails under Linux load | Shared resource contention, latency or memory mapping | Target timing and IPC stress tests |
| Knee movement destabilizes | Unmeasured geometry, servo lag, missing hip constraint or dynamics | Actual versus desired joint logs |
| Intermittent reset at high effort | Supply sag, current demand, watchdog or EMI | Supply trace, reset reason and output timing |
These are hypotheses, not diagnoses. Use a measurement that would differ between competing causes. For example, a voltage trace distinguishes a supply collapse from a pure command timeout even if both stop the motors.
16.4 Separate observation errors from plant errors
If a plot shows an impossible jump, inspect timestamp extension, unit conversion and sequence order before concluding the robot physically jumped. If wheel speed spikes at low speed, inspect encoder differentiation and quantization. If accelerometer tilt changes during translation, remember the sensor measures specific force rather than gravity alone.
The state packet is produced after the actuator stage but before the runtime's last timing check. A late overrun can therefore disable the robot after a packet was formed; telemetry can lag the final fault decision by a tick. Treat status as a sampled report, not a hardware guarantee that remains true until the next message.
When sample_try fails, the runtime's local sample starts zeroed while the controller may retain earlier estimated state. A faulted telemetry record can therefore mix retained estimates and zero snapshot fields. Linux should flag the record as faulted rather than feed every field into a downstream estimator as a fresh simultaneous observation.
16.5 Regression after changes
Re-run relevant host tests after changes to equations, configuration validation, serialization or state handling. Repeat target tests when changing compiler options, SDK, kernel, clock configuration or interrupt priorities. Recheck physical identification when changing the battery position, wheels, gearing, servo, link dimensions or printed material.
A small mechanical modification may change CoM more than a substantial code refactor. Maintenance therefore needs hardware revision tracking as well as source versioning. Keep photographs, CAD parameters, weights and calibration records together.
16.6 Operational records
A useful run record contains: date, operator, hardware revision, firmware/SDK revision, configuration/gain identifier, software environment, power source condition, surface, load, initial pose, intended experiment, stop criterion, measurements and interpretation. Separate raw observation from interpretation so later review can challenge the explanation without losing the data.
Avoid naming an artifact only latest. Give released configurations stable identifiers and retain previous known-good versions. If the workspace is not yet a Git repository, a recorded source archive and checksums are better than relying on memory; version control can be introduced as a separate deliberate project action.
16.7 Release criteria for a research prototype
A useful internal release states what operating envelope was tested and what remains unsupported. For example: fixed verified leg geometry, specified surface, bounded speed/yaw requests, documented supply range, named hardware revision and observed timing bounds. It should include known failure cases and recovery procedure.
Do not label an experimental gain change production-ready solely because one demonstration succeeds. Reproducibility means another person can reconstruct the conditions and obtain the same bounded result, including the behavior when a required condition is violated.
Exercises
- Why can a sensor fault be caused by missing joint feedback? Answer: the current sample validator includes the geometry-verification gate.
- Why preserve pre-fault logs? Answer: the first violated assumption may occur before the final visible failure or latched status.
- What changes after fitting a different tire? Answer: effective radius, traction, deformation and possibly odometry/actuator calibration.
- Is a telemetry enabled flag an instantaneous hardware readback? Answer: no; it is a sampled controller status and can precede a later fault in the same runtime invocation.
Appendix A. Glossary, units and notation
| Term | Meaning in this project |
|---|---|
| AMP | Asymmetric multiprocessing: processors with different roles and operating environments |
| ABI | Application binary interface: the exact cross-boundary representation and calling/layout contract |
| Acquire/release | Ordering operations around ownership transfer; not automatic cache flushing |
| Back EMF | Speed-dependent generated motor voltage opposing applied drive |
| CoM | Center of mass, obtained from weighted mass distribution |
| Complementary filter | Combination of gyro prediction and slower inclination correction |
| DARE | Discrete algebraic Riccati equation used for the gain calculation |
| Deadline | Latest acceptable completion time of a required operation |
| DMA | Direct memory access by a peripheral/controller without CPU copying each byte |
| Epoch | Identifier of a coordinated shared-memory session |
| Euler angles | A convention-dependent orientation parameterization using ordered rotations |
| FK / IK | Forward / inverse kinematics |
| FPU | Floating-point unit and its execution/context state |
| Gain scheduling | Varying feedback gains with a measured operating parameter |
| GPIO | General-purpose digital input/output, not a motor power source |
| HIL | Hardware-in-the-loop: some real hardware interacts with a simulated plant/environment |
| IMU | Inertial measurement unit; here accelerometer plus gyro |
| IPC | Interprocessor communication |
| ISR | Interrupt service routine |
| Jitter | Variation in timing relative to the intended schedule |
| LQI | LQR with appropriately designed integral-state augmentation |
| LQR | Linear quadratic regulator for a defined plant, state and cost |
| Mailbox | Hardware communication/notification facility used as a doorbell here |
| MPC | Model predictive control with repeated finite-horizon optimization |
| Odometry | Local motion estimate accumulated from motion measurements |
| Observability | Whether internal state can be inferred from measurements over time under a model |
| PWM | Pulse-width modulation; duty and supply affect average electrical drive |
| QoS | ROS2 communication policy choices, distinct from actuator safety logic |
| RTOS | Real-time operating system |
| Saturation | Actuator or software limit preventing larger requested effort |
| SPSC | Single producer, single consumer; an ownership restriction |
| Specific force | Accelerometer-measured nongravitational force per unit mass |
| WCET | Worst-case execution time under stated assumptions; different from mean measured time |
| ZOH | Zero-order hold: input held between discrete updates |
| Symbol/field | Unit | Interpretation |
|---|---|---|
dt, T |
s | Nominal controller interval |
| timestamp | μs | Wrapping local RTOS time, not ROS wall time |
| a, b / thigh, shin | m | Pivot-center link lengths |
| l, L / controller height | m | Hip-to-wheel distance in the current geometry model |
h / com_height |
m | Effective body CoM distance from axle for upright dynamic model |
r / wheel_radius |
m | Effective rolling radius |
| b_track / track | m | Wheel-center separation for yaw kinematics |
| q | rad | Knee flexion, zero at straight |
| θ, φ, ψ | rad | Pitch, roll and yaw in defined conventions |
| gyro rates | rad/s | Body angular-rate components after calibration/rotation |
| m, M | kg | Body mass and effective base mass |
| I | kg m² | Body pitch inertia about CoM |
| τ_left, τ_right | Nm | Per-wheel torque requests |
| u | Nm | Total wheel torque, left plus right |
| normalized duty | dimensionless | Electrical PWM duty request in [-1,1] |
Useful conversions: 1 mm = 0.001 m; 1 degree = π/180 rad; 1 kg·mm² = 10⁻⁶ kg·m²; 1 ms = 1000 μs; frequency in Hz is the reciprocal of period in seconds. Servo torque advertised in kgf·cm converts at approximately 0.0980665 Nm per kgf·cm; verify the rating's operating conditions rather than interpreting it as continuous available output.
Appendix B. Configuration and laboratory records
B.1 Configuration dictionary
Use control.h as the exact interface authority. The following groups explain how to obtain values, without providing deployment defaults.
| Group | Fields | Source of evidence |
|---|---|---|
| Timing | dt, sensor/command timeout |
Target timing, acquisition age and command-loss policy |
| Leg geometry | thigh, shin, min/max height | Measured pivot distances and constrained reachable range |
| Motion bounds | height rate, knee rate, leg difference | Mechanism/servo characterization and validated trajectory range |
| Wheel geometry | radius, track | Rolling calibration and measured geometry |
| Effort and reference limits | max torque, speed, yaw rate, position error | Attainable actuator/traction envelope and test policy |
| Attitude envelope | max pitch, max roll, arm angle | Valid estimator/model domain and commissioning procedure |
| Estimation | filter tau, acceleration gate | Noise, bias and dynamic measurement experiments |
| Auxiliary feedback | yaw kp, roll kp/kd | Signed physical response and measured closed-loop behavior |
| LQR schedule | three length knots and four gains each | Identified plant, design period, Q/R and validation |
| Commission declaration | commissioned | Reviewed evidence for the specific configuration |
The implementation requires several strictly positive limits even if an early experiment holds the corresponding motion fixed. Structural acceptance does not authorize the full numerical range for physical use. Keep the experimentally permitted subset in the supervisory configuration and commissioning record.
B.2 Suggested plant measurement record
Hardware revision:
Linkage/hip constraint description:
Upper and lower pivot distances [m]:
Wheel effective radius [m] and measurement method:
Track [m] and turn-calibration method:
Body/base mass split [kg]:
Wheel inertia contribution and assumptions:
For each geometry knot:
actual joint angles [rad]:
leg length [m]:
body CoM height above axle [m]:
body pitch inertia about CoM [kg m^2]:
measurement uncertainty and method:
Motor/gearbox model and encoder definition:
Supply range [V]:
Torque constant, back EMF constant and resistance:
Sensor mounting rotation and bias calibration:
B.3 Suggested experiment record
Question/hypothesis:
Date and operator:
Hardware / firmware / SDK / kernel revisions:
Configuration and gain-table identifier:
Build commands, compiler and Python dependency versions:
Fixture, surface, payload and power conditions:
Initial state and commanded trajectory:
Independent stop mechanism and test stop criterion:
Observed acquisition age / release latency / execution time:
Measured result with units:
Faults, missing samples and saturation intervals:
Raw data path:
Interpretation:
Remaining uncertainty and next experiment:
B.4 Completion evidence by layer
| Layer | Minimum useful evidence |
|---|---|
| Software arithmetic | Host assertions and independent numerical examples |
| Numerical model | State/input conventions, identified parameters, documented approximations |
| Mechanical design | Measured geometry, constrained degrees of freedom, load paths and motion clearances |
| Sensors | Configuration readback, signs/scales, bias and bounded acquisition age |
| Actuators | Polarity, available effort, supply/current behavior and disable path |
| Target runtime | Release-to-output measurements and exercised failure paths |
| AMP transport | Matching mappings/layout, stress transfers, restart and backpressure behavior |
| ROS2 | Command freshness, consistent frames/time, truthful status and diagnostics |
| Physical balance | Repeatable trials within a stated operating envelope |
These records are templates. Empty forms are not evidence, and documenting a planned test does not mark that test as passed.
Appendix C. Primary references and further reading
These sources support device-specific facts, platform interfaces and further study. The book's descriptions of implemented behavior come from the local source files, while its synthetic calculations are explicitly identified as such. Online references were checked during preparation on 2026-10-02, and the hardware references updated on 2026-11-05; confirm revision-specific details during hardware integration.
Hardware and board support
- Milk-V Duo S hardware, architecture and connector information. Use the board-specific pinout and voltage-domain information.
- Milk-V image download documentation. Distinguishes official Buildroot image families and third-party images.
- Milk-V RTOS and mailbox example. Starting point for vendor communication paths; example timing is not a deterministic controller design.
- Bosch BMI323 datasheet. Sensor ranges, registers, electrical interfaces and startup; chip ID 0x43; two dummy bytes on every I2C read. Its pin table (table 56) gives each pin the same function as on the BMI160, except pins 2, 3, 10 and 11 (the BMI160's auxiliary interface), which are do-not-connect. The datasheet itself does not mention the BMI160.
- TI DRV8313 datasheet. Three-phase driver logic thresholds, overcurrent latch and nRESET behavior.
- iPower GM4108H-120T gimbal motor. Pole count, winding resistance and rated torque.
- ams OSRAM AS5048A. SPI frame format, parity, error flag and DIAG/AGC register (rotor and knee sensors).
- TI TPS54560B datasheet. Buck design equations, enable thresholds and layout guidance (5 V rail).
- TI LM61480/LM61495 datasheet. Component selection, current limits and the RPH0016B land pattern (6 V servo rail).
- DSSERVO DS3240 datasheet. Servo voltage range, stall current and pulse range.
Communication and ROS2
- Linux circular buffers. Producer/consumer ownership and ordering concepts.
- Linux memory barriers. Memory access ordering and caveats around devices and caches.
- REP 2000: ROS2 releases and target platforms. Distribution/platform compatibility; not a board-port guarantee.
- REP 105: coordinate frames. Meaning of map, odom and body frame roles.
- ROS2 Jazzy QoS documentation source. Communication-policy semantics and compatibility.
- ROS2 Jazzy Odometry message definition. Frame semantics for pose and twist.
Numerical methods and advanced control
- SciPy zero-order-hold discretization interface.
- SciPy discrete algebraic Riccati solver.
- MIT Underactuated Robotics: LQR. Original course treatment of optimal linear feedback and local nonlinear applications.
- MIT Underactuated Robotics: Lyapunov analysis. Stability reasoning and regions of validity.
- MIT Underactuated Robotics: trajectory optimization. Optimization formulations relevant to advanced planning/control work.
Documentation tooling
- latex2mathml project. The book builder uses this library to turn its own LaTeX equations into offline browser MathML.
Source files that define current behavior
Read the control interface, control implementation, runtime, IPC layout, IPC implementation, device conversions, host tests, and gain tool alongside their chapters. These are the correct references when documentation and a later edited implementation diverge.