Reports how far the body has fallen away from upright, as the :lean joint's
configuration.
The IMU knows which way is down and the topology knows how the chip is bolted
to the body, but nothing joined the two: forward kinematics reported the robot
bolt upright no matter what the accelerometer said. This closes that loop, so
BB.Robot.State carries a lean that means something and anything reasoning
about the robot's pose gets it for free.
Where the angle comes from
The fused orientation is a rotation from the chip's frame into the estimator's
world, whose Z is up. Taking the world's up vector back through it gives up in
the chip's frame, and the static rotation from imu_link to base_link — read
from the topology rather than written down here — puts it in the body's. Level,
that vector is the body's own +Z; leaning forward tips it towards -X. So the
angle is atan2(-x, z), positive forwards, which is the sense :lean is
declared with.
The rate comes from the gyroscope through the same rotation, taking the
component about the body's Y. A balance controller wants both, and
differentiating the angle would be a worse rate than the one the gyro
measures directly. Strictly the pitch rate is wy * cos(roll) - wz * sin(roll)
and this is the small-roll approximation of it, which for a robot that is
supposed to stay upright is the case that matters.
What it can't tell you
Only tilt. The estimator has no magnetometer, so rotation about gravity is unobservable and its yaw wanders — but yaw doesn't reach this calculation, since it moves the up vector not at all.
It also can't tell a lean from an acceleration. An accelerometer measures specific force, so a robot accelerating forwards on the level reads as one leaning back, and the estimator only suppresses that to the extent its gyro disagrees. That is the classic failure of balancing on an IMU alone and it wants watching once the wheels are driving.
Summary
Functions
The static rotation taking a direction in imu_link's frame into body_link's.
The lean angle in radians for an orientation, positive leaning forwards.
How fast the body is leaning, in radians per second, for a gyroscope reading.
Functions
@spec body_from_imu(module(), atom(), atom()) :: BB.Math.Quaternion.t()
The static rotation taking a direction in imu_link's frame into body_link's.
Both links hang off the same joints above them, so those cancel and the configuration they are evaluated at doesn't matter.
@spec lean(BB.Math.Quaternion.t(), BB.Math.Quaternion.t()) :: float()
The lean angle in radians for an orientation, positive leaning forwards.
body_from_imu rotates a direction in the chip's frame into the body's.
Decoupled from roll, which the obvious formula is not. Pitched by theta
and rolled by phi, the world's up vector in body coordinates is
{-sin theta, cos theta * sin phi, cos theta * cos phi} — so taking
atan2(-x, z) gives atan2(sin theta, cos theta * cos phi), and the roll
shrinks the denominator. A five degree lean reads as 5.8 at thirty degrees of
roll and 9.9 at sixty.
That is not a rounding error on a balance bot, it is a coupling: any rolling
motion modulates the measured lean, the controller corrects a pitch error that
was never there, and the oscillation is driven from an axis the pitch loop
cannot see. No gain reaches it. Dividing by the whole magnitude perpendicular
to X instead removes cos phi and leaves theta for any roll.
@spec lean_rate(BB.Math.Vec3.t(), BB.Math.Quaternion.t()) :: float()
How fast the body is leaning, in radians per second, for a gyroscope reading.