Throbol Cookbook

If you're used to programming robots with imperative languages where you can update state variables in-place, it may take a while to get used to the purely functional style, where new states are calculated as functions of previous states and sensor inputs.

This cookbook gives recipies for solving many common problems with updating state.

Simple State machines

You can build state machines by calculating the new state as a function of the previous state, like:

light = turnon ? 1 : turnoff ? 0 : light[-dt]

Integration

foo = foo[-dt] + dt * bar // foo is the integral of bar

The value of every cell at time < 0 is 0 (or the equivalent of 0 for the type: empty string, empty set, etc.). So this integrator starts from 0.

foo = initial ? 123 : foo[-dt] + dt * bar // add an initial condition other than zero.

foo = integrate(bar) + 123 // equivalent to above.

PID loops

A simple PID feedback loop to make arm.pos follow target.pos and target.vel

arm.torque = 
  p.gain * (target.pos - arm.pos) + 
  d.gain * (target.vel - vel(arm.pos)) + 
  i.gain * integrate(target.pos - arm.pos)

The vel(x) function returns essentially (x - x[-dt])/dt

PID loops suffer from a phenomenon called windup, where if for some reason it isn't able to achieve the desired position, the integral of position error gets larger and larger. So it's best to clamp the value of the integrator within some limits. The integrate(x, reset, lo, hi) function is useful here:

arm.torque = 
  p.gain * (target.pos - arm.pos) + 
  d.gain * (vel(target.pos) - vel(arm.pos)) + 
  i.gain * integrate(target.pos - arm.pos, 0, -0.5~1, 0.5~1)

Filters

When the position sensor is noisy, it may help to add low-pass filtering on the output of a feedback loop. Thus a complete joint controller looks like:

arm.torque = clamp(lpfilter1(0.02~+0.1,
  10~50 * (target.pos - arm.pos[-dt]) + 
  5~20 * (vel(target.pos) - vel(arm.pos[-dt])) + 
  1~10 * integrate(target.pos - arm.pos[-dt], 0, -0.5~1, 0.5~1)),
  -1, 1)

Open in Throbol Editor

Behavior trees

Throbol includes behavior trees to make it easier. A behavior cell can be in one of 3 states: running, failure, success. In running mode, it contains a set of symbols representing what is running. Consider a state machine for testing a walking robot.

harness.state = seq(
    !robot.fallen || run('recover.robot),
    robot.ready || run('prepare.robot),
    robot.onground || run('lower.robot),
    robot.stable || run('stabilize.robot),
    robot.fallen || run('stand.robot),
  )

The seq operator runs through each argument until it finds one that is running or success.

Other cells can change their behavior based on what's running. For instance, we set the harness actuator to -0.5 meters in some run modes, 0 otherwise:

act.harness.desiredPos = isrunning(harness.state, 
    'lower.robot |
    'stabilize.robot |
    'stand.robot) ? 
  -0.5 : 0