Skip to content

Commit c1bfea2

Browse files
committed
Keep saved review and single-joint calibration accessible
1 parent e490639 commit c1bfea2

6 files changed

Lines changed: 196 additions & 100 deletions

File tree

‎runtime/src/commissioning/session.rs‎

Lines changed: 56 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ pub struct SessionView {
2929
pub gripper_opening: f64,
3030
pub estimated: bool,
3131
pub can_recalibrate_joint: bool,
32+
pub can_reopen_saved_review: bool,
3233
pub recalibrating_joint: Option<usize>,
3334
pub message: String,
3435
pub configuration_path: String,
@@ -43,6 +44,7 @@ impl Default for SessionView {
4344
gripper_opening: 0.5,
4445
estimated: false,
4546
can_recalibrate_joint: false,
47+
can_reopen_saved_review: false,
4648
recalibrating_joint: None,
4749
message: String::new(),
4850
configuration_path: String::new(),
@@ -75,6 +77,7 @@ impl ReviewSource {
7577
enum State {
7678
Reference {
7779
message: String,
80+
can_reopen_saved_review: bool,
7881
},
7982
Sweep {
8083
recording: Sweep,
@@ -88,6 +91,15 @@ enum State {
8891
Failed(String),
8992
}
9093

94+
impl State {
95+
fn reference(message: impl Into<String>) -> Self {
96+
Self::Reference {
97+
message: message.into(),
98+
can_reopen_saved_review: false,
99+
}
100+
}
101+
}
102+
91103
#[derive(PartialEq, Eq)]
92104
struct MotorSnapshot {
93105
offsets: [i64; 6],
@@ -147,9 +159,7 @@ impl<Bus: SetupBus> Session<Bus> {
147159
setup,
148160
files,
149161
snapshot,
150-
state: State::Reference {
151-
message: String::new(),
152-
},
162+
state: State::reference(""),
153163
positions,
154164
previous_positions: positions,
155165
moving_until: [None; 6],
@@ -160,35 +170,31 @@ impl<Bus: SetupBus> Session<Bus> {
160170

161171
fn initial_state(&self) -> Result<State> {
162172
if self.files.reset_incomplete()? {
163-
return Ok(State::Reference {
164-
message: "Retry the incomplete reset before calibrating.".into(),
165-
});
173+
return Ok(State::reference(
174+
"Retry the incomplete reset before calibrating.",
175+
));
166176
}
167177
if self.files.calibration_incomplete()? {
168-
return Ok(State::Reference {
169-
message: "The previous save did not finish. Inspect the backups and capture a new calibration.".into(),
170-
});
178+
return Ok(State::reference(
179+
"The previous save did not finish. Inspect the backups and capture a new calibration.",
180+
));
171181
}
172182
let path = self.files.calibration_path();
173183
if !path.try_exists()? {
174-
return Ok(State::Reference {
175-
message: String::new(),
176-
});
184+
return Ok(State::reference(""));
177185
}
178186
let calibration = match self.setup.motor_ids.load_named_calibration(&path) {
179187
Ok(calibration) => calibration,
180188
Err(error) => {
181-
return Ok(State::Reference {
182-
message: format!("Saved calibration could not be reused: {error:#}"),
183-
});
189+
return Ok(State::reference(format!(
190+
"Saved calibration could not be reused: {error:#}"
191+
)));
184192
}
185193
};
186194
if !self.snapshot.matches(&calibration) {
187-
return Ok(State::Reference {
188-
message:
189-
"Saved calibration differs from the motor registers. Capture a new reference."
190-
.into(),
191-
});
195+
return Ok(State::reference(
196+
"Saved calibration differs from the motor registers. Capture a new reference.",
197+
));
192198
}
193199
let source = if self.files.matches_setup(&self.setup) {
194200
ReviewSource::Saved
@@ -212,6 +218,7 @@ impl<Bus: SetupBus> Session<Bus> {
212218
}
213219

214220
fn reload_baseline(&mut self) -> Result<CalibrationEntries> {
221+
self.files.require_completed_reset()?;
215222
let calibration = self
216223
.setup
217224
.motor_ids
@@ -265,12 +272,36 @@ impl<Bus: SetupBus> Session<Bus> {
265272
_ => {
266273
self.state = State::Reference {
267274
message: String::new(),
275+
can_reopen_saved_review: matches!(
276+
self.initial_state()?,
277+
State::Review {
278+
source: ReviewSource::Saved,
279+
..
280+
}
281+
),
268282
}
269283
}
270284
}
271285
Ok(())
272286
}
273287

288+
pub fn reopen_saved_review(&mut self) -> Result<()> {
289+
ensure!(
290+
matches!(
291+
self.state,
292+
State::Reference {
293+
can_reopen_saved_review: true,
294+
..
295+
}
296+
),
297+
"Start from a saved setup before returning to its review"
298+
);
299+
let baseline = self.reload_baseline()?;
300+
self.sample()?;
301+
self.state = self.review_saved(&baseline, ReviewSource::Saved)?;
302+
Ok(())
303+
}
304+
274305
pub fn sample(&mut self) -> Result<()> {
275306
let ids = self.setup.motor_ids.ordered();
276307
let positions = read_positions(self.bus.as_mut().context("Reconnect the arm")?, ids)?;
@@ -490,8 +521,12 @@ impl<Bus: SetupBus> Session<Bus> {
490521
})
491522
.collect();
492523
}
493-
State::Reference { message } => {
524+
State::Reference {
525+
message,
526+
can_reopen_saved_review,
527+
} => {
494528
view.message.clone_from(message);
529+
view.can_reopen_saved_review = *can_reopen_saved_review;
495530
let (signs, offsets) = alignment_arrays(&self.setup.alignment);
496531
view.joints = MOTOR_NAMES
497532
.into_iter()

‎runtime/tests/commissioning.rs‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -120,13 +120,31 @@ impl SetupBus for MemoryBus {
120120
}
121121
}
122122

123+
fn verify_return_to_saved_review(session: &mut Session<MemoryBus>, bus: &MemoryBus) -> Result<()> {
124+
assert_eq!(session.stage(), Stage::Review);
125+
session.restart()?;
126+
assert_eq!(session.stage(), Stage::Reference);
127+
assert!(session.view().can_reopen_saved_review);
128+
let saved_limits = bus.registers()["min_position_limit"];
129+
let mut changed_limits = saved_limits;
130+
changed_limits[0] = (changed_limits[0] + 1).rem_euclid(4096);
131+
bus.set("min_position_limit", changed_limits);
132+
assert!(session.reopen_saved_review().is_err());
133+
assert_eq!(session.stage(), Stage::Reference);
134+
bus.set("min_position_limit", saved_limits);
135+
session.reopen_saved_review()?;
136+
assert_eq!(session.stage(), Stage::Review);
137+
Ok(())
138+
}
139+
123140
#[test]
124141
fn calibration_keeps_physical_sweeps_when_remapped_and_saves_runtime_files() -> Result<()> {
125142
let root = Files::new()?;
126143
let files = root.arm()?;
127144
let bus = MemoryBus::new();
128145
let mut session = Session::connect(bus.clone(), files.clone())?;
129146
assert_eq!(session.stage(), Stage::Reference);
147+
assert!(!session.view().can_reopen_saved_review);
130148
let initial = bus.registers();
131149
session.start_sweep(None)?;
132150
assert!(
@@ -182,6 +200,7 @@ fn calibration_keeps_physical_sweeps_when_remapped_and_saves_runtime_files() ->
182200
assert_eq!(bus.registers()["p_coefficient"], [16, 16, 16, 32, 32, 16]);
183201

184202
let mut reopened = Session::connect(bus.clone(), files.clone())?;
203+
verify_return_to_saved_review(&mut reopened, &bus)?;
185204
assert!(reopened.view().can_recalibrate_joint);
186205
reopened.start_sweep(Some(0))?;
187206
let mut positions = bus.registers()["present_position"];

‎setup/native/src/app.rs‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -340,12 +340,13 @@ impl Application {
340340
self.retry_at = None;
341341
}
342342
Action::Reopen => {
343-
let session = self.session.as_ref().context("Connect an arm first")?;
344-
ensure!(
345-
matches!(session.stage(), Stage::Saved),
346-
"Reopen is available after saving"
347-
);
348-
self.session = Some(self.open_session(&session.setup.serial)?);
343+
let session = self.session.as_mut().context("Connect an arm first")?;
344+
if session.stage() == Stage::Saved {
345+
let serial = session.setup.serial.clone();
346+
self.session = Some(self.open_session(&serial)?);
347+
} else {
348+
session.reopen_saved_review()?;
349+
}
349350
}
350351
Action::ResetServos => {
351352
let session = self.session.as_ref().context("Connect an arm first")?;

‎setup/src/so101_setup/calibration.py‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,7 @@ def entries(self, motor_ids: list[int] = MOTOR_IDS) -> Calibration:
132132
@dataclass(frozen=True)
133133
class Reference:
134134
message: str = ""
135+
can_reopen_saved_review: bool = False
135136

136137

137138
@dataclass
@@ -206,6 +207,7 @@ class CalibrationView(BaseModel):
206207
gripper_opening: float
207208
estimated: bool = False
208209
can_recalibrate_joint: bool = False
210+
can_reopen_saved_review: bool = False
209211
recalibrating_joint: int | None = None
210212
message: str = ""
211213
command: str = ""
@@ -319,7 +321,17 @@ def restart(self) -> None:
319321
if isinstance(self.state, (SweepRecording, Review)) and self.state.recalibration:
320322
self.state = self.review_calibration(self.reload_saved_baseline(), "saved")
321323
else:
322-
self.state = Reference()
324+
initial_state = self.initial_state()
325+
self.state = Reference(
326+
can_reopen_saved_review=isinstance(initial_state, Review)
327+
and initial_state.source == "saved"
328+
)
329+
330+
def reopen_saved_review(self) -> None:
331+
if not isinstance(self.state, Reference) or not self.state.can_reopen_saved_review:
332+
raise ValueError("Start from a saved setup before returning to its review")
333+
self.require_torque_off()
334+
self.state = self.review_calibration(self.reload_saved_baseline(), "saved")
323335

324336
def reload_saved_baseline(self) -> Calibration:
325337
calibration = self.load_matching_calibration()
@@ -553,6 +565,8 @@ def view(self) -> CalibrationView:
553565
gripper_opening=max(0.0, min(1.0, opening)),
554566
estimated=review is not None and review.estimated,
555567
can_recalibrate_joint=review is not None and review.source == "saved",
568+
can_reopen_saved_review=isinstance(self.state, Reference)
569+
and self.state.can_reopen_saved_review,
556570
recalibrating_joint=review.recalibration.index
557571
if review and review.recalibration
558572
else None,

‎setup/src/so101_setup/server.py‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -129,6 +129,7 @@ class ApplicationView(BaseModel):
129129
joints: list[JointView] = Field(default_factory=list)
130130
estimated: bool = False
131131
can_recalibrate_joint: bool = False
132+
can_reopen_saved_review: bool = False
132133
offer_servo_reset: bool = False
133134
motor_ids: dict[str, int] = Field(default_factory=lambda: motor_id_mapping(MOTOR_IDS))
134135
checking_motor_order: bool = False
@@ -337,6 +338,7 @@ def view(self) -> ApplicationView:
337338
raise
338339
return ApplicationView(stage="error", serial=session.serial, message=str(error))
339340
view.servo_reset_incomplete = session.serial in self.incomplete_servo_resets
341+
view.can_reopen_saved_review &= not view.servo_reset_incomplete
340342
view.offer_servo_reset = view.stage == "reference" and not self.servo_reset_decided
341343
view.motor_ids = session.setup.motor_ids
342344
view.checking_motor_order = self.motor_order_sample is not None
@@ -486,11 +488,14 @@ def apply_session_action(self, session: CalibrationSession, action: Action) -> N
486488
case MapMotorIdsAction(motor_ids=motor_ids):
487489
self.apply_motor_mapping(session, motor_ids)
488490
case SimpleAction(action="reopen"):
489-
if session.view().stage != "saved":
490-
raise ValueError("Reopen is available after saving")
491-
serial = session.serial
492-
self.session = None
493-
self.connect(serial)
491+
if isinstance(session.state, Saved):
492+
serial = session.serial
493+
self.session = None
494+
self.connect(serial)
495+
else:
496+
if session.serial in self.incomplete_servo_resets:
497+
raise ValueError("An incomplete reset must be retried before calibration")
498+
session.reopen_saved_review()
494499
case SimpleAction(action="restart"):
495500
session.restart()
496501
self.servo_reset_decided = False

0 commit comments

Comments
 (0)