Hardware feedback: unpin the shipped config, fall is a report not a gate, Start homes first
Three findings from the first hardware run of the skills branch: Standing felt soft — and the daemon was innocent: the board's /etc/robot/robotd.toml, copied once at first install and never overwritten, was main 0.6.0's file pinning action_scale = 0.7 and standing_gain_ratio = 0.6. Every release since says 0.9 / 0.8 and never got a vote. The shipped config now leaves every tuning value commented out — the file documents the defaults instead of freezing them — so first installs track the release. (Boards that already carry a pinned file need one manual refresh.) Fall → limp is now a mode, not a rule, and it ships OFF — matching the prototype, whose --fall-detect defaults off. A fallen robot kept being driven there, and the humans stayed in charge; our unconditional limp meant a misread lean sat the robot down and then refused robot.init until gravity agreed. The fall verdict is still tracked and reported in the state stream; [safety] fall_limp arms the old behaviour, and fall_recover implies it (recovery starts with the limp settle). The init/enable/skill refusals gate on the same flag. Start now means 'stand at home, then drive': padd sends robot.init before robot.enable, and robot.init on an already-Ready robot re-ramps to home from the current pose — the prototype's init_position runs on every Start, and driving the policy from whatever crouch the last stop froze is not a starting pose it was trained on. The policy holds off during the ~2 s ramp (label 'homing') and resumes at Ready. Verified on the fake bus from both limp and Ready. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
parent
018f42cd7c
commit
d218a3edc5
@ -18,6 +18,12 @@
|
||||
# `robotd --port`, which is the dev escape hatch rather than the thing to edit here.
|
||||
port = "/dev/ttyS2"
|
||||
|
||||
# Every value below that is commented out shows the built-in default. LEAVE IT COMMENTED
|
||||
# unless this robot genuinely needs a different value: install.sh copies this file to
|
||||
# /etc/robot/robotd.toml once and never overwrites it, so an uncommented value is frozen on
|
||||
# the board forever while releases move on. That is exactly how a fleet ended up standing at
|
||||
# kP 120 while the release default said 160.
|
||||
|
||||
[control]
|
||||
# Control loop rate, Hz.
|
||||
#
|
||||
@ -31,8 +37,8 @@ hz = 50
|
||||
# prototype's `--cmd-alpha` / `--head-alpha` — a stick snap becomes a ramp the gait can
|
||||
# follow. 1.0 is pass-through. `cmd_alpha` covers the velocity twist; `head_alpha` covers
|
||||
# head targets and the body pose.
|
||||
cmd_alpha = 0.2
|
||||
head_alpha = 0.2
|
||||
# cmd_alpha = 0.2
|
||||
# head_alpha = 0.2
|
||||
|
||||
[update_gate]
|
||||
# What decides `healthy`, and therefore what the update system rolls back on.
|
||||
@ -50,7 +56,7 @@ head_alpha = 0.2
|
||||
# Achieved rate floor. Below this, unhealthy. 45 is 90% of the default 50 Hz: loose enough
|
||||
# not to trip on one slow tick, tight enough that a loop dropping every tenth cycle is not
|
||||
# called healthy. Raise it in step with `control.hz`.
|
||||
min_achieved_hz = 45.0
|
||||
# min_achieved_hz = 45.0
|
||||
|
||||
# How many periods may pass with no tick at all before the loop counts as WEDGED. This is
|
||||
# what turns a hung control thread into an honest "unhealthy" instead of a socket that
|
||||
@ -60,12 +66,12 @@ min_achieved_hz = 45.0
|
||||
# two apart. This was originally 3 (60 ms at 50 Hz), which ordinary scheduler jitter on a
|
||||
# busy board exceeds routinely; a health check that trips on jitter rolls back good
|
||||
# releases. 25 periods is 500 ms: a loop silent that long is genuinely gone.
|
||||
stall_periods = 25
|
||||
# stall_periods = 25
|
||||
|
||||
# Consecutive failed bus reads tolerated. One dropped transaction is ordinary on a serial
|
||||
# bus; a run of them means the bus is gone, and a robot that cannot read its own joints is
|
||||
# not healthy whatever rate the loop is spinning at.
|
||||
max_consecutive_errors = 10
|
||||
# max_consecutive_errors = 10
|
||||
|
||||
[policy]
|
||||
# Whether to load a policy at all.
|
||||
@ -118,11 +124,11 @@ mode = "walk"
|
||||
# action_scale = 0.9
|
||||
|
||||
# The standing policy is trained to be applied whole.
|
||||
standing_action_scale = 1.0
|
||||
# standing_action_scale = 1.0
|
||||
# Standing runs softer, at this fraction of `gain`. The prototype's --standing-kp-ratio.
|
||||
standing_gain_ratio = 0.8
|
||||
# standing_gain_ratio = 0.8
|
||||
# Position P gain while running.
|
||||
gain = 200
|
||||
# gain = 200
|
||||
|
||||
# First-order low-pass on the head and leg joint targets, as a blend factor in (0, 1].
|
||||
# 1.0 is pass-through (off). Unset resolves per mode: 0.5 / 0.7 walking — the values the
|
||||
@ -136,50 +142,56 @@ gain = 200
|
||||
# period/scale resolve per mode: 4.0 / 1.0 walking, 3.0 / 0.8 for the roller crouch.
|
||||
# ground_pick_period = 4.0
|
||||
# ground_pick_action_scale = 1.0
|
||||
ground_pick_gain_ratio = 1.0
|
||||
# ground_pick_gain_ratio = 1.0
|
||||
|
||||
# How long a kick window stays on the kick network, seconds.
|
||||
kick_duration = 0.5
|
||||
# kick_duration = 0.5
|
||||
|
||||
# One roulade — one forward roll, seconds (the prototype's measured single-roll time).
|
||||
# Holding X chains rolls of this length; the roll in flight always completes. The scale
|
||||
# and gain multiplier apply while a roll runs — 1.0 means full walking values, which is
|
||||
# what the roll is trained against.
|
||||
roulade_duration = 1.0
|
||||
roulade_action_scale = 1.0
|
||||
roulade_gain_ratio = 1.0
|
||||
# roulade_duration = 1.0
|
||||
# roulade_action_scale = 1.0
|
||||
# roulade_gain_ratio = 1.0
|
||||
|
||||
# Scale actions with battery voltage: effective scale = action_scale * (nominal / measured
|
||||
# EMA, clamped 6.0..9.5 V). The servos' effective kP tracks their supply, so this holds the
|
||||
# robot's response steady as the pack sags. Off by default, as in the prototype.
|
||||
voltage_adapt = false
|
||||
nominal_voltage = 7.4
|
||||
# voltage_adapt = false
|
||||
# nominal_voltage = 7.4
|
||||
|
||||
[safety]
|
||||
# Fall detection, on projected gravity in the trunk frame. Upright reads about -1.0 on z;
|
||||
# a robot on its side reads near 0. Debounced, because the impulse from a firm footfall
|
||||
# would otherwise read as a fall — and dropping the robot mid-stride is how you cause one.
|
||||
fall_gravity_z = -0.5
|
||||
fall_debounce_ms = 200
|
||||
# fall_gravity_z = -0.5
|
||||
# fall_debounce_ms = 200
|
||||
|
||||
# How long the robot goes without a velocity intent before it stops.
|
||||
#
|
||||
# Stop, not limp: losing contact with a client should leave a biped standing, because
|
||||
# standing is its safe state. Losing balance is the other event, and it goes limp instead.
|
||||
deadman_ms = 500
|
||||
# deadman_ms = 500
|
||||
|
||||
# Gain once fallen — low enough to yield rather than fight the floor. Refusing to command
|
||||
# a fallen robot only freezes it in the pose it fell in; going soft lets it settle.
|
||||
gain_limp = 50
|
||||
# Whether a detected fall preempts anything: hold at gain_limp, refuse init/enable/skills
|
||||
# until upright. OFF by default, as the prototype is — a fallen robot keeps being driven
|
||||
# and the humans stay in charge; the fall verdict still shows in robotctl monitor either
|
||||
# way. fall_recover = true implies this (recovery starts with the limp settle).
|
||||
# fall_limp = false
|
||||
|
||||
# Gain once fallen (only used when fall_limp/fall_recover arms the gate) — low enough to
|
||||
# yield rather than fight the floor.
|
||||
# gain_limp = 50
|
||||
|
||||
# Stand back up after a fall, on its own: limp 0.3 s so the robot settles, then the
|
||||
# standing network drives until the robot has been solidly upright (gravity z < -0.85) for
|
||||
# a full second. While this is on, the standing network is RESERVED for recovery and body
|
||||
# pose — command magnitude no longer selects it. Off by default, matching the prototype's
|
||||
# `--fall-detect` flag.
|
||||
fall_recover = false
|
||||
# fall_recover = false
|
||||
|
||||
# Sit down gracefully and power the machine off when the battery EMA reaches the empty
|
||||
# floor (6.6 V — the same value the battery percentage maps to 0%). The EMA moves over
|
||||
# ~10 s, so a load sag cannot trip it; reaching 6.6 requires a genuinely spent pack.
|
||||
battery_empty_shutdown = true
|
||||
# battery_empty_shutdown = true
|
||||
|
||||
@ -91,7 +91,10 @@ corrupt each other's replies:
|
||||
sudo systemctl stop robotd && sudo /opt/robot/daemon/current/bin/robotd init && sudo systemctl start robotd
|
||||
```
|
||||
|
||||
A fallen robot refuses `init`: the fall gate holds it limp on purpose. Stand it up by hand first.
|
||||
`init` works whether or not the robot has fallen — by default a fall is a *report* (visible in
|
||||
`robotctl monitor`), not a gate, matching the prototype. A board that sets `[safety] fall_limp`
|
||||
or `fall_recover` in `robotd.toml` arms the gate: there a fallen robot goes limp and refuses
|
||||
`init`/`enable`/skills until it is stood up.
|
||||
|
||||
### Gamepad (`configd`)
|
||||
|
||||
|
||||
@ -9,19 +9,22 @@
|
||||
//! once something has already gone wrong is the code most likely to be quietly broken, so
|
||||
//! make the broken state unrepresentable instead.
|
||||
//!
|
||||
//! Three rules, all unconditional:
|
||||
//! Two rules, unconditional:
|
||||
//!
|
||||
//! - **Non-finite rejection.** A `NaN` target is not clamped, it is refused outright.
|
||||
//! - **Range clamp.** Targets are held inside the actuator's travel.
|
||||
//! - **Fall → limp.** Debounced, and it preempts whatever the policy wanted.
|
||||
//!
|
||||
//! Plus a deadman on the command itself: if intents stop arriving, the velocity goes to
|
||||
//! zero. **Stop is not limp** — losing comms makes the robot *stand still*, because standing
|
||||
//! is the safe state for a biped; losing balance makes it yield. Two events, two responses.
|
||||
//!
|
||||
//! In the prototype fall detection is `--fall-detect`, a flag, evaluated inline among the
|
||||
//! gamepad handling and skipped while a scripted move is running. Safety that the
|
||||
//! interesting cases opt out of is the anti-pattern `architecture.md` §6 names.
|
||||
//! **Fall → limp is a mode, not a rule** ([`SafetyConfig::fall_limp`]), and it ships OFF.
|
||||
//! The prototype never limps on a fall — its `--fall-detect` defaults off and doubles as
|
||||
//! auto-recovery when on — and a robot that yields the moment gravity misreads a lean is a
|
||||
//! robot that keeps sitting down while someone handles it. The fall *verdict* is always
|
||||
//! tracked and reported; what changes with the flag is only whether it preempts the
|
||||
//! policy. Fall recovery requires the limp (its settle phase), so `robotd` turns this on
|
||||
//! whenever `fall_recover` is on.
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
@ -53,6 +56,9 @@ pub struct SafetyConfig {
|
||||
pub gain_running: u16,
|
||||
/// Gain once fallen. Low enough to yield rather than fight the floor.
|
||||
pub gain_limp: u16,
|
||||
/// Whether a fall preempts the policy: hold at `gain_limp` until upright. Off matches
|
||||
/// the prototype, whose `--fall-detect` ships off; the fall verdict reports either way.
|
||||
pub fall_limp: bool,
|
||||
}
|
||||
|
||||
impl Default for SafetyConfig {
|
||||
@ -64,6 +70,7 @@ impl Default for SafetyConfig {
|
||||
deadman: Duration::from_millis(500),
|
||||
gain_running: 200,
|
||||
gain_limp: 50,
|
||||
fall_limp: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -254,12 +261,13 @@ impl<T: RobotIo> Safety<T> {
|
||||
) -> Result<Applied, IoError> {
|
||||
let mut applied = Applied::default();
|
||||
|
||||
// A fallen robot yields. This precedes everything else: whatever the policy
|
||||
// computed for a robot it believes is upright is not something to send to one that
|
||||
// is on its side. The one exception is an active recovery ([`Self::set_recovery`]),
|
||||
// where the stand-up network is deliberately driving a robot gravity still calls
|
||||
// fallen — its targets flow through the ordinary checks below.
|
||||
if self.fallen && !self.recovering {
|
||||
// A fallen robot yields — when the fall gate is on. It precedes everything else:
|
||||
// whatever the policy computed for a robot it believes is upright is not something
|
||||
// to send to one that is on its side. Two exceptions: the gate itself off (the
|
||||
// prototype's default — the policy keeps driving and the humans stay in charge),
|
||||
// and an active recovery ([`Self::set_recovery`]), where the stand-up network is
|
||||
// deliberately driving a robot gravity still calls fallen.
|
||||
if self.fallen && self.config.fall_limp && !self.recovering {
|
||||
applied.limits.push(Limit::Fallen);
|
||||
self.set_gain(self.config.gain_limp)?;
|
||||
self.io.write(&JointTargets::new(hold))?;
|
||||
@ -333,6 +341,18 @@ mod tests {
|
||||
Safety::new(FakeIo::at(DEFAULT_POSITION), SafetyConfig::default())
|
||||
}
|
||||
|
||||
/// A safety with the fall gate ON — what `robotd` builds when `fall_limp` or
|
||||
/// `fall_recover` is configured.
|
||||
fn gated() -> Safety<FakeIo> {
|
||||
Safety::new(
|
||||
FakeIo::at(DEFAULT_POSITION),
|
||||
SafetyConfig {
|
||||
fall_limp: true,
|
||||
..SafetyConfig::default()
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
/// A hard footfall spikes gravity briefly. Treating that as a fall would drop the robot
|
||||
/// mid-stride — which is itself how you cause a fall.
|
||||
#[test]
|
||||
@ -405,9 +425,39 @@ mod tests {
|
||||
);
|
||||
}
|
||||
|
||||
/// With the gate OFF — the default, the prototype's behaviour — a fall changes
|
||||
/// nothing about what gets written: the verdict is reported, the policy keeps
|
||||
/// driving, the gain stays the caller's. Going limp under someone adjusting the
|
||||
/// robot's lean is exactly the annoyance this default removes.
|
||||
#[test]
|
||||
fn by_default_a_fall_reports_but_does_not_preempt() {
|
||||
let mut s = safety();
|
||||
for _ in 0..11 {
|
||||
s.observe(&on_its_side(), Duration::from_millis(20));
|
||||
}
|
||||
assert!(s.fallen(), "the verdict is still tracked");
|
||||
|
||||
let mut wanted = DEFAULT_POSITION;
|
||||
wanted[0] = 0.9;
|
||||
let applied = s
|
||||
.apply(
|
||||
wanted,
|
||||
DEFAULT_POSITION,
|
||||
SafetyConfig::default().gain_running,
|
||||
)
|
||||
.unwrap();
|
||||
assert!(applied.limits.is_empty(), "{:?}", applied.limits);
|
||||
assert_eq!(
|
||||
s.io().last_written.unwrap().positions,
|
||||
wanted,
|
||||
"the policy keeps driving"
|
||||
);
|
||||
assert_eq!(s.io().last_gain, Some(SafetyConfig::default().gain_running));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn falling_goes_limp_rather_than_freezing() {
|
||||
let mut s = safety();
|
||||
let mut s = gated();
|
||||
for _ in 0..11 {
|
||||
s.observe(&on_its_side(), Duration::from_millis(20));
|
||||
}
|
||||
@ -522,7 +572,7 @@ mod tests {
|
||||
/// cleared, or fall → limp would be silently dead after the first recovery.
|
||||
#[test]
|
||||
fn recovery_lets_the_caller_drive_a_fallen_robot() {
|
||||
let mut s = safety();
|
||||
let mut s = gated();
|
||||
for _ in 0..11 {
|
||||
s.observe(&on_its_side(), Duration::from_millis(20));
|
||||
}
|
||||
|
||||
@ -343,6 +343,16 @@ fn main() -> std::process::ExitCode {
|
||||
|
||||
if toggle_enable {
|
||||
enabled = !enabled;
|
||||
if enabled {
|
||||
// Start means "stand at home, then drive", as the prototype's Start runs
|
||||
// init_position before the policy — not "drive from whatever pose the last
|
||||
// stop left the legs in". robotd ramps (~2 s) and the policy holds off
|
||||
// until the ramp completes, so the order here is all the sequencing needed.
|
||||
if let Err(e) = request(&mut stream, &mut next_id, &proto::Call::RobotInit) {
|
||||
tracing::error!(error = %e, "init failed");
|
||||
return std::process::ExitCode::FAILURE;
|
||||
}
|
||||
}
|
||||
let call = proto::Call::RobotEnable(proto::EnableParams { on: enabled });
|
||||
if let Err(e) = request(&mut stream, &mut next_id, &call) {
|
||||
tracing::error!(error = %e, "enable failed");
|
||||
|
||||
@ -289,8 +289,8 @@ enum RobotCommand {
|
||||
/// robot with no walking network can still stand — and it is what the gamepad's Start does on
|
||||
/// its way to driving, so running this by hand is for the bench rather than the everyday path.
|
||||
///
|
||||
/// Refused on a robot that has fallen: the fall gate holds a fallen robot limp on purpose. Stand
|
||||
/// it up by hand first.
|
||||
/// Refused on a fallen robot only when `[safety] fall_limp` or `fall_recover` arms the
|
||||
/// fall gate — by default it works whatever gravity says, as the prototype does.
|
||||
Init {
|
||||
#[arg(long)]
|
||||
json: bool,
|
||||
|
||||
@ -259,6 +259,10 @@ struct RobotState {
|
||||
policy_roulade: Option<String>,
|
||||
/// `walk` or `roller` — constant for the life of the process; `robot.mode` reports it.
|
||||
mode: &'static str,
|
||||
/// Whether a fall preempts anything (`fall_limp` or `fall_recover`). When false —
|
||||
/// the default, the prototype's behaviour — `fallen` is a report, not a wall, and
|
||||
/// none of the `robot.*` calls refuse for it.
|
||||
fall_gate: bool,
|
||||
/// Published by the loop so the IPC side can answer without consulting it.
|
||||
fallen: AtomicBool,
|
||||
/// The policy is driving and has been asked for a non-zero velocity.
|
||||
@ -310,6 +314,7 @@ impl RobotState {
|
||||
policy_kick_right: named_policy(params, |p| p.kick_right.clone()),
|
||||
policy_roulade: named_policy(params, |p| p.roulade.clone()),
|
||||
mode: params.policy.mode.as_str(),
|
||||
fall_gate: params.safety.fall_limp || params.safety.fall_recover,
|
||||
fallen: AtomicBool::new(false),
|
||||
moving: AtomicBool::new(false),
|
||||
homed: AtomicBool::new(false),
|
||||
@ -902,6 +907,9 @@ async fn control_loop<T: RobotIo>(
|
||||
poweroff: PowerOff,
|
||||
) {
|
||||
let policy_cfg = params.policy.resolved();
|
||||
// Whether a fall preempts anything. Mirrors `RobotState::fall_gate` for the loop's own
|
||||
// gates — with it off, `fallen` is a report in the state stream and nothing more.
|
||||
let fall_gate = params.safety.fall_limp || params.safety.fall_recover;
|
||||
let mut safety = Safety::new(
|
||||
io,
|
||||
SafetyConfig {
|
||||
@ -910,6 +918,8 @@ async fn control_loop<T: RobotIo>(
|
||||
deadman: Duration::from_millis(params.safety.deadman_ms),
|
||||
gain_running: policy_cfg.gain,
|
||||
gain_limp: params.safety.gain_limp,
|
||||
// Recovery cannot work without the limp settle, so it implies the gate.
|
||||
fall_limp: params.safety.fall_limp || params.safety.fall_recover,
|
||||
},
|
||||
);
|
||||
|
||||
@ -1105,8 +1115,20 @@ async fn control_loop<T: RobotIo>(
|
||||
}
|
||||
Err(e) => tracing::warn!(error = %e, "cannot enable torque"),
|
||||
},
|
||||
// Already up, or up and driving: nothing to do, and saying so beats a silent no-op.
|
||||
(state, _) => tracing::info!(?state, "robot.init: already brought up"),
|
||||
// Already up: ramp back to home from wherever the joints are, as the
|
||||
// prototype's init_position always does — Start on a robot stopped
|
||||
// mid-crouch must not hand the policy that crouch as its starting pose.
|
||||
// The policy holds off while Homing runs and resumes at Ready.
|
||||
(Bringup::Ready, Some(sensors)) => {
|
||||
tracing::warn!(?HOME_RAMP, "robot.init: re-homing from the current pose");
|
||||
bringup = Bringup::Homing {
|
||||
from: sensors.positions,
|
||||
since: tick_start,
|
||||
};
|
||||
}
|
||||
// Mid-ramp, or no sample to ramp from: nothing to do, and saying so beats
|
||||
// a silent no-op.
|
||||
(state, _) => tracing::info!(?state, "robot.init: nothing to bring up"),
|
||||
},
|
||||
Some(intents::PowerRequest::Relax) => match safety.set_torque(false) {
|
||||
Ok(()) => {
|
||||
@ -1131,7 +1153,7 @@ async fn control_loop<T: RobotIo>(
|
||||
Some(controller)
|
||||
if snapshot.enabled
|
||||
&& bringup == Bringup::Ready
|
||||
&& !safety.fallen()
|
||||
&& !(fall_gate && safety.fallen())
|
||||
&& shutdown_sit.is_none() =>
|
||||
{
|
||||
let outcome = |what: &str, result: Result<(), &'static str>| match result {
|
||||
@ -1306,11 +1328,10 @@ async fn control_loop<T: RobotIo>(
|
||||
|
||||
// Bring the robot up when someone asks it to drive and it has no torque yet.
|
||||
//
|
||||
// Gated on `!safety.fallen()` as well as on the request, and that is not belt-and-braces: a
|
||||
// robot the IMU calls fallen is one `apply` will command at limp gain anyway, so ramping it
|
||||
// would be writing a stand-up that cannot happen. `robot.enable` refuses in that state and
|
||||
// says to stand the robot up first; `robotd init`, with the daemon stopped, is still the way
|
||||
// to do that.
|
||||
// Gated on the fall only when the fall gate is armed, and there it is not
|
||||
// belt-and-braces: a robot `apply` holds at limp gain is one a ramp cannot stand
|
||||
// up. With the gate off (the default) a fallen robot ramps like any other, which
|
||||
// is the prototype's behaviour.
|
||||
//
|
||||
// Needs a sample: `from` is where the joints actually are, and starting a ramp from a
|
||||
// position nobody read would be the lurch this exists to avoid.
|
||||
@ -1322,7 +1343,7 @@ async fn control_loop<T: RobotIo>(
|
||||
bringup,
|
||||
snapshot.enabled,
|
||||
controller.is_some(),
|
||||
safety.fallen(),
|
||||
fall_gate && safety.fallen(),
|
||||
sensors.as_ref(),
|
||||
) {
|
||||
match safety.set_torque(true) {
|
||||
@ -1850,7 +1871,7 @@ fn dispatch(
|
||||
};
|
||||
let result = if !configured {
|
||||
proto::IntentResult::refused("no policy configured for that skill")
|
||||
} else if state.fallen.load(Ordering::Relaxed) {
|
||||
} else if state.fall_gate && state.fallen.load(Ordering::Relaxed) {
|
||||
proto::IntentResult::refused("the robot is down; stand it up first")
|
||||
} else {
|
||||
intents.request_skill(p.skill);
|
||||
@ -1910,7 +1931,7 @@ fn dispatch(
|
||||
// Refusing to enable a fallen robot is a normal answer with a reason, not an
|
||||
// error: the client asked something reasonable and safety declined.
|
||||
proto::Call::RobotEnable(p) => {
|
||||
let result = if p.on && state.fallen.load(Ordering::Relaxed) {
|
||||
let result = if p.on && state.fall_gate && state.fallen.load(Ordering::Relaxed) {
|
||||
proto::IntentResult::refused("the robot is down; stand it up first")
|
||||
} else {
|
||||
intents.set_enabled(p.on);
|
||||
@ -1925,13 +1946,14 @@ fn dispatch(
|
||||
// Both only *ask*. The control loop owns the only `RobotIo` handle, so nothing here touches
|
||||
// the bus — which is also why `robotd init` needs the daemon stopped and these do not.
|
||||
proto::Call::RobotInit => {
|
||||
let result = if state.fallen.load(Ordering::Relaxed) {
|
||||
// The same wall `robot.enable` hits, and for the same reason: `Safety::apply`
|
||||
// commands a fallen robot at limp gain and holds it, so a ramp would be writing a
|
||||
// stand-up that cannot happen. Named in the refusal, with the escape hatch, because
|
||||
// "it refused exactly when I needed it" is otherwise the whole experience.
|
||||
// Refused only when the fall gate is armed (`fall_limp`/`fall_recover`): there,
|
||||
// `Safety::apply` commands a fallen robot at limp gain and holds it, so a ramp
|
||||
// would be writing a stand-up that cannot happen. With the gate off — the
|
||||
// default — init works whatever gravity says, which is the prototype's
|
||||
// behaviour and what a bench actually needs.
|
||||
let result = if state.fall_gate && state.fallen.load(Ordering::Relaxed) {
|
||||
proto::IntentResult::refused(
|
||||
"the robot is down. Stand it up by hand, or stop robotd and run `robotd init` — the fall gate holds a fallen robot limp on purpose",
|
||||
"the robot is down and the fall gate is armed. Stand it up first, or drop `fall_limp`/`fall_recover` from robotd.toml",
|
||||
)
|
||||
} else {
|
||||
intents.request_init();
|
||||
@ -2250,7 +2272,8 @@ mod tests {
|
||||
assert!(!refused.accepted);
|
||||
assert!(!intents.take_skills().kick_left, "a refusal must not queue");
|
||||
|
||||
// A fallen robot is refused whatever is configured — same wall as robot.enable.
|
||||
// A fall refuses a skill only when the fall gate is armed. By default it is not —
|
||||
// the prototype never limps on a fall — so `fallen` is a report, not a wall.
|
||||
s.fallen.store(true, Ordering::Relaxed);
|
||||
let down: proto::IntentResult = dispatch(
|
||||
&s,
|
||||
@ -2262,7 +2285,26 @@ mod tests {
|
||||
)
|
||||
.result_as()
|
||||
.unwrap();
|
||||
assert!(down.accepted, "with the gate off, fallen must not refuse");
|
||||
assert!(intents.take_skills().sit_toggle);
|
||||
|
||||
// With `fall_limp` armed, the same call hits the wall — same as robot.enable.
|
||||
let mut params = Params::default();
|
||||
params.safety.fall_limp = true;
|
||||
let gated = RobotState::new(¶ms, false, false);
|
||||
gated.fallen.store(true, Ordering::Relaxed);
|
||||
let down: proto::IntentResult = dispatch(
|
||||
&gated,
|
||||
&intents,
|
||||
id(),
|
||||
&proto::Call::RobotDo(proto::DoParams {
|
||||
skill: proto::Skill::SitToggle,
|
||||
}),
|
||||
)
|
||||
.result_as()
|
||||
.unwrap();
|
||||
assert!(!down.accepted);
|
||||
assert!(!intents.take_skills().sit_toggle, "a refusal must not queue");
|
||||
}
|
||||
|
||||
/// The pose and mouth intents land in their slots like move and head do — including via
|
||||
|
||||
@ -237,6 +237,13 @@ pub struct SafetyParams {
|
||||
pub deadman_ms: u64,
|
||||
/// Gain once fallen — low enough to yield rather than fight the floor.
|
||||
pub gain_limp: u16,
|
||||
/// Whether a detected fall preempts the policy: hold at `gain_limp`, refuse
|
||||
/// `robot.init`/`robot.enable`/skills until the robot is upright again. Off by
|
||||
/// default, as the prototype is — its `--fall-detect` ships off, so a fallen robot
|
||||
/// keeps being driven and the humans stay in charge. The fall verdict is reported in
|
||||
/// the state stream either way. `fall_recover = true` implies this: recovery starts
|
||||
/// with the limp settle.
|
||||
pub fall_limp: bool,
|
||||
/// Stand back up after a fall, on its own: limp 0.3 s, then the standing network drives
|
||||
/// until the robot has been solidly upright for a second. Reserves the standing network
|
||||
/// for recovery, so command magnitude stops selecting it. Off by default, as the
|
||||
@ -287,6 +294,7 @@ impl Default for SafetyParams {
|
||||
fall_debounce_ms: 200,
|
||||
deadman_ms: 500,
|
||||
gain_limp: 50,
|
||||
fall_limp: false,
|
||||
fall_recover: false,
|
||||
battery_empty_shutdown: true,
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user