27 lines
1.1 KiB
C++
27 lines
1.1 KiB
C++
 
|
#pragma once
|
||
|
|
// Kinematic body model: where the robot's body is in the world, given only
|
||
|
|
// where the firmware puts its feet. No dynamics - the body rests on the
|
||
|
|
// ground plane its feet define, and grounded feet don't slide.
|
||
|
|
//
|
||
|
|
// Robot/world frame: right-handed, mm, +X forward, +Y left, +Z up; world
|
||
|
|
// ground at z = 0 before ground tilt.
|
||
|
|
#include <stdint.h>
|
||
|
|
|
||
|
|
namespace simbody {
|
||
|
|
|
||
|
|
void reset();
|
||
|
|
void setGroundTilt(float roll_deg, float pitch_deg); // tilts the ground (and so the robot) - robot-gyro tests
|
||
|
|
|
||
|
|
// Advances the body to match new body-frame foot positions (18 floats, mm).
|
||
|
|
void update(const float feet_mm[18], float dt);
|
||
|
|
|
||
|
|
// Results.
|
||
|
|
void position(float out_mm[3]); // body origin, world
|
||
|
|
void orientation(float out_wxyz[4]); // body -> world rotation
|
||
|
|
float yawDeg(); // accumulated heading (not wrapped)
|
||
|
|
int contactMask(); // bit i = foot i on the ground
|
||
|
|
|
||
|
|
// What an IMU fixed to the body reads: accel in g (+1 g up at rest), gyro deg/s.
|
||
|
|
void imu(float accel_g[3], float gyro_dps[3]);
|
||
|
|
|
||
|
|
} // namespace simbody
|