Skip to content

Commit 1e3bbad

Browse files
committed
Unify calibration around the resting-pose sweep
1 parent 3771a11 commit 1e3bbad

5 files changed

Lines changed: 203 additions & 410 deletions

File tree

‎setup/README.md‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,13 @@ then install and daisy chain the motors during assembly.
2424

2525
Open **Calibrate arm** after assembly and select its USB controller. On a fresh
2626
launch, the calibration page selects a single connected controller automatically.
27-
For the soft-gripper-and-camera assembly, support the disabled arm in the shown
27+
Support the disabled arm in the shown
2828
resting pose with the gripper closed, then start recording and gently sweep
2929
each joint through its safe travel, including wrist roll and gripper opening.
3030
The tool flags unusually short sweeps so you can continue recording any missed
3131
joint. Never force a joint to satisfy the check. Review and adjust the live
3232
model before saving. The home pose is an estimate,
33-
so compare multiple positions. The stock-gripper variant uses its reference
34-
pose and explicit gripper endpoints.
33+
so compare multiple positions.
3534

3635
If one joint needs a fresh calibration, open the arm’s saved setup and choose
3736
**Recalibrate one joint** on the review screen. This option is available when
@@ -66,7 +65,7 @@ verify the correspondence. Save records the mapping in the `[motor_ids]`
6665
table and leaves the programmed servo IDs intact.
6766

6867
New arms default to the soft gripper with wrist camera and start calibration in
69-
the resting pose. For a stock gripper and midpoint reference pose, start with
68+
the resting pose. For stock gripper geometry, start with
7069
`so101-calibrate --model-variant stock`.
7170
Existing arms keep the `model_variant` in their `setup.toml`; to change an
7271
existing arm's model, edit that field before starting the tool. The viewer selects the corresponding model and saving calibration

‎setup/src/so101_setup/calibration.py‎

Lines changed: 48 additions & 142 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
import math
1111
import os
1212
import tempfile
13+
import time
1314
from copy import deepcopy
1415
from dataclasses import dataclass, field
1516
from datetime import UTC, datetime
@@ -31,8 +32,6 @@
3132
# Generous lower bounds based on the observed soft-gripper SO-101 assembly.
3233
# These detect a missed sweep, not physical or commanded joint limits.
3334
MINIMUM_SWEEP_DEGREES = (130, 130, 120, 130, 220, 75)
34-
REFERENCE_JOINTS = [0.0] * 5
35-
REFERENCE_GRIPPER_RADIANS = 0.75
3635
CALIBRATION_REGISTERS = (
3736
"homing_offset",
3837
"min_position_limit",
@@ -97,11 +96,6 @@ class DraftCalibration:
9796
maximums: list[int]
9897
signs: list[int]
9998
offsets: list[float]
100-
use_swept_wrist_limits: bool = False
101-
previous_steps: list[int] = field(init=False)
102-
103-
def __post_init__(self) -> None:
104-
self.previous_steps = list(self.reference_steps)
10599

106100
def positions(self, positions: list[int], old_homing: list[int]) -> list[int]:
107101
return [
@@ -111,33 +105,13 @@ def positions(self, positions: list[int], old_homing: list[int]) -> list[int]:
111105
)
112106
]
113107

114-
def record(self, positions: list[int]) -> None:
115-
if any(
116-
abs(position - previous) > RESOLUTION // 2
117-
for index, (position, previous) in enumerate(
118-
zip(positions, self.previous_steps, strict=True)
119-
)
120-
if index != 4
121-
):
122-
raise ValueError(
123-
"A joint crossed the encoder boundary. Return to the middle reference pose and capture again before sweeping."
124-
)
125-
self.previous_steps = list(positions)
126-
for index, position in enumerate(positions):
127-
self.minimums[index] = min(self.minimums[index], position)
128-
self.maximums[index] = max(self.maximums[index], position)
129-
130108
def entries(self, motor_ids: list[int] = MOTOR_IDS) -> Calibration:
131109
return {
132110
name: MotorCalibration(
133111
id=motor_id,
134112
homing_offset=self.homing_offsets[index],
135-
range_min=0
136-
if name == "wrist_roll" and not self.use_swept_wrist_limits
137-
else self.minimums[index],
138-
range_max=4095
139-
if name == "wrist_roll" and not self.use_swept_wrist_limits
140-
else self.maximums[index],
113+
range_min=self.minimums[index],
114+
range_max=self.maximums[index],
141115
)
142116
for index, (name, motor_id) in enumerate(zip(MOTOR_NAMES, motor_ids, strict=True))
143117
}
@@ -148,13 +122,6 @@ class Reference:
148122
message: str = ""
149123

150124

151-
@dataclass
152-
class Recording:
153-
draft: DraftCalibration
154-
closed_gripper_steps: int | None = None
155-
open_gripper_steps: int | None = None
156-
157-
158125
@dataclass
159126
class SweepRecording:
160127
sweeps: dict[int, EncoderSweep]
@@ -163,6 +130,20 @@ class SweepRecording:
163130
recalibration: JointRecalibration | None = None
164131

165132

133+
def remap_full_sweep(
134+
recording: SweepRecording,
135+
old_motor_ids: list[int],
136+
setup: ArmSetup,
137+
) -> SweepRecording:
138+
"""Keep encoder data with its physical motor and alignment with its joint."""
139+
old_indices = [old_motor_ids.index(motor_id) for motor_id in setup.motor_ids_in_joint_order()]
140+
return SweepRecording(
141+
{index: recording.sweeps[old] for index, old in enumerate(old_indices)},
142+
list(recording.signs),
143+
[recording.home_positions[old] for old in old_indices],
144+
)
145+
146+
166147
@dataclass
167148
class Review:
168149
draft: DraftCalibration
@@ -190,7 +171,7 @@ class Failed:
190171
message: str
191172

192173

193-
CalibrationState = Reference | Recording | SweepRecording | Review | Saved | Failed
174+
CalibrationState = Reference | SweepRecording | Review | Saved | Failed
194175

195176

196177
class JointView(BaseModel):
@@ -199,17 +180,15 @@ class JointView(BaseModel):
199180
range_min: int
200181
range_max: int
201182
sign: int
183+
moving: bool = False
202184

203185

204186
class CalibrationView(BaseModel):
205187
stage: Literal["reference", "sweep", "review", "saved", "error"]
206188
serial: str
207189
joints: list[JointView]
208190
gripper_opening: float
209-
gripper_closed_recorded: bool = False
210-
gripper_open_recorded: bool = False
211191
estimated: bool = False
212-
sweep_only: bool = False
213192
can_recalibrate_joint: bool = False
214193
recalibrating_joint: int | None = None
215194
message: str = ""
@@ -242,6 +221,8 @@ class CalibrationSession:
242221
state: CalibrationState = field(default_factory=Reference)
243222
original_homing: list[int] = field(init=False)
244223
last_view: CalibrationView = field(init=False)
224+
previous_encoder_positions: list[int] | None = field(default=None, init=False)
225+
moving_until: list[float] = field(default_factory=lambda: [0.0] * 6, init=False)
245226

246227
@property
247228
def serial(self) -> str:
@@ -286,7 +267,6 @@ def initial_state(self) -> Reference | Review:
286267
reference_steps=[HOMING_STEPS] * 6,
287268
minimums=minimums,
288269
maximums=maximums,
289-
use_swept_wrist_limits=True,
290270
signs=[*settings.joint_signs, settings.gripper_sign],
291271
offsets=[
292272
*settings.joint_offsets_radians,
@@ -322,10 +302,6 @@ def start_sweep(self, joint_index: int | None = None) -> None:
322302
raise ValueError("Reconnect before creating a new draft")
323303
recalibration = None
324304
if joint_index is None:
325-
if self.setup.model_variant != "soft-gripper-camera":
326-
raise ValueError(
327-
"Sweep-only calibration is not yet available for the stock gripper"
328-
)
329305
indices = range(len(MOTOR_NAMES))
330306
else:
331307
if not 0 <= joint_index < len(MOTOR_NAMES):
@@ -356,7 +332,7 @@ def require_saved_setup(self) -> None:
356332
):
357333
raise ValueError("Saved setup changed. Reconnect before recalibrating a joint.")
358334

359-
def _finish_sweep_estimate(self) -> None:
335+
def finish_sweep(self) -> None:
360336
if not isinstance(self.state, SweepRecording):
361337
raise ValueError("Start a sweep before estimating calibration")
362338
self.view()
@@ -384,14 +360,9 @@ def _finish_sweep_estimate(self) -> None:
384360
[0] * 6,
385361
recording.signs,
386362
[0.0] * 6,
387-
use_swept_wrist_limits=True,
388363
)
389364
)
390-
reference_angles = (
391-
[*HOME_JOINTS, HOME_GRIPPER_RADIANS]
392-
if self.setup.model_variant == "soft-gripper-camera"
393-
else [*REFERENCE_JOINTS, REFERENCE_GRIPPER_RADIANS]
394-
)
365+
reference_angles = [*HOME_JOINTS, HOME_GRIPPER_RADIANS]
395366
for index, sweep in recording.sweeps.items():
396367
sign = recording.signs[index]
397368
old_homing = self.original_homing[index]
@@ -412,49 +383,12 @@ def _finish_sweep_estimate(self) -> None:
412383
joint_radians(position, draft.signs[5], draft.offsets[5])
413384
for position in (draft.minimums[5], draft.maximums[5])
414385
]
415-
endpoints = (min(gripper_angles), max(gripper_angles))
416-
if (
417-
recalibration
418-
and recalibration.baseline.gripper_endpoints[0]
419-
> recalibration.baseline.gripper_endpoints[1]
420-
):
421-
endpoints = (endpoints[1], endpoints[0])
422-
self.state = Review(draft, endpoints, estimated=True, recalibration=recalibration)
423-
424-
def capture_reference(self) -> None:
425-
if isinstance(self.state, (Saved, Failed)):
426-
raise ValueError("Restart calibration to create another draft")
427-
positions = self.read_positions()
428-
raw_positions = [
429-
(position + previous) % RESOLUTION
430-
for position, previous in zip(positions, self.original_homing, strict=True)
431-
]
432-
homing_offsets = [
433-
max(-2047, min(2047, position - HOMING_STEPS)) for position in raw_positions
434-
]
435-
reference_steps = [
436-
rehomed_steps(position, old, new)
437-
for position, old, new in zip(
438-
positions, self.original_homing, homing_offsets, strict=True
386+
# The home sample was captured with the physical gripper closed.
387+
endpoints = tuple(
388+
sorted(gripper_angles, key=lambda angle: abs(angle - HOME_GRIPPER_RADIANS))
439389
)
440-
]
441-
settings = self.setup.alignment
442-
signs: list[int] = [*settings.joint_signs, settings.gripper_sign]
443-
targets = [*REFERENCE_JOINTS, REFERENCE_GRIPPER_RADIANS]
444-
offsets = [
445-
target - joint_radians(position, sign, 0.0)
446-
for target, position, sign in zip(targets, reference_steps, signs, strict=True)
447-
]
448-
self.state = Recording(
449-
DraftCalibration(
450-
homing_offsets,
451-
reference_steps,
452-
list(reference_steps),
453-
list(reference_steps),
454-
signs,
455-
offsets,
456-
)
457-
)
390+
endpoints = (endpoints[0], endpoints[1])
391+
self.state = Review(draft, endpoints, estimated=True, recalibration=recalibration)
458392

459393
def reverse_joint(self, index: int) -> None:
460394
if not 0 <= index < 6:
@@ -466,21 +400,14 @@ def reverse_joint(self, index: int) -> None:
466400
reference_angle = joint_radians(center, draft.signs[index], draft.offsets[index])
467401
draft.signs[index] *= -1
468402
draft.offsets[index] = reference_angle - joint_radians(center, draft.signs[index], 0.0)
469-
if index == 5 and self.state.recalibration:
403+
if index == 5:
470404
closed, opened = self.state.gripper_endpoints
471405
self.state.gripper_endpoints = (
472406
2 * reference_angle - closed,
473407
2 * reference_angle - opened,
474408
)
475409
return
476-
if not isinstance(self.state, Recording):
477-
raise ValueError("Directions can be changed only while calibrating")
478-
draft = self.state.draft
479-
target = REFERENCE_JOINTS[index] if index < 5 else REFERENCE_GRIPPER_RADIANS
480-
draft.signs[index] *= -1
481-
draft.offsets[index] = target - joint_radians(
482-
draft.reference_steps[index], draft.signs[index], 0.0
483-
)
410+
raise ValueError("Directions can be changed during review")
484411

485412
def adjust_joint(self, index: int, delta_radians: float) -> None:
486413
if not isinstance(self.state, Review):
@@ -498,36 +425,19 @@ def require_editable_joint(self, review: Review, index: int) -> None:
498425
raise ValueError("Only the selected joint can be adjusted during single-joint review")
499426
review.saved_baseline = False
500427

501-
def capture_gripper_endpoint(self, endpoint: Literal["closed", "open"]) -> None:
502-
if not isinstance(self.state, Recording):
503-
raise ValueError("Record gripper endpoints during the travel sweep")
504-
positions = self.state.draft.positions(self.read_positions(), self.original_homing)
505-
self.state.draft.record(positions)
506-
steps = positions[5]
507-
if endpoint == "closed":
508-
self.state.closed_gripper_steps = steps
509-
else:
510-
self.state.open_gripper_steps = steps
511-
512-
def finish_sweep(self) -> None:
513-
if isinstance(self.state, SweepRecording):
514-
self._finish_sweep_estimate()
515-
return
516-
if not isinstance(self.state, Recording):
517-
raise ValueError("Capture a reference pose before recording travel")
518-
self.view()
519-
# MotorCalibration checks that every swept joint/gripper has moved.
520-
self.state.draft.entries(self.bus.driven_motor_ids)
521-
draft = self.state.draft
522-
closed_steps = self.state.closed_gripper_steps
523-
open_steps = self.state.open_gripper_steps
524-
if closed_steps is None or open_steps is None or closed_steps == open_steps:
525-
raise ValueError("Record distinct closed and open gripper positions before review")
526-
endpoints = (
527-
joint_radians(closed_steps, draft.signs[5], draft.offsets[5]),
528-
joint_radians(open_steps, draft.signs[5], draft.offsets[5]),
529-
)
530-
self.state = Review(draft, endpoints)
428+
def moving_motors(self, positions: list[int]) -> list[bool]:
429+
previous = self.previous_encoder_positions
430+
if previous is None or not isinstance(self.state, SweepRecording):
431+
self.previous_encoder_positions = list(positions)
432+
self.moving_until = [0.0] * len(positions)
433+
return [False] * len(positions)
434+
now = time.monotonic()
435+
for index, (before, after) in enumerate(zip(previous, positions, strict=True)):
436+
delta = abs(after - before)
437+
if min(delta, RESOLUTION - delta) >= 8:
438+
previous[index] = after
439+
self.moving_until[index] = now + 0.6
440+
return [until > now for until in self.moving_until]
531441

532442
def view(self) -> CalibrationView:
533443
if isinstance(self.state, (Saved, Failed)):
@@ -547,6 +457,7 @@ def view(self) -> CalibrationView:
547457
}
548458
)
549459
positions = self.read_positions()
460+
moving_motors = self.moving_motors(positions)
550461
settings = self.setup.alignment
551462
if isinstance(self.state, SweepRecording):
552463
recording = self.state
@@ -555,7 +466,6 @@ def view(self) -> CalibrationView:
555466
view = CalibrationView(
556467
stage="sweep",
557468
serial=self.serial,
558-
sweep_only=True,
559469
recalibrating_joint=recording.recalibration.index
560470
if recording.recalibration
561471
else None,
@@ -567,20 +477,19 @@ def view(self) -> CalibrationView:
567477
range_min=sweep.minimum,
568478
range_max=sweep.maximum,
569479
sign=recording.signs[index],
480+
moving=moving_motors[index],
570481
)
571482
for index, sweep in recording.sweeps.items()
572483
],
573484
)
574485
self.last_view = view
575486
return view
576-
if isinstance(self.state, (Recording, Review)):
487+
if isinstance(self.state, Review):
577488
draft = self.state.draft
578489
positions = draft.positions(positions, self.original_homing)
579-
if isinstance(self.state, Recording):
580-
draft.record(positions)
581490
signs, offsets = draft.signs, draft.offsets
582491
minimums, maximums = draft.minimums, draft.maximums
583-
stage = "sweep" if isinstance(self.state, Recording) else "review"
492+
stage = "review"
584493
else:
585494
signs: list[int] = [*settings.joint_signs, settings.gripper_sign]
586495
offsets = [*settings.joint_offsets_radians, settings.gripper_offset_radians]
@@ -606,6 +515,7 @@ def view(self) -> CalibrationView:
606515
position_radians=angles[index],
607516
range_min=minimums[index],
608517
range_max=maximums[index],
518+
moving=moving_motors[index],
609519
sign=signs[index],
610520
)
611521
for index, name in enumerate(MOTOR_NAMES)
@@ -616,10 +526,6 @@ def view(self) -> CalibrationView:
616526
recalibrating_joint=review.recalibration.index
617527
if review and review.recalibration
618528
else None,
619-
gripper_closed_recorded=isinstance(self.state, Review)
620-
or (isinstance(self.state, Recording) and self.state.closed_gripper_steps is not None),
621-
gripper_open_recorded=isinstance(self.state, Review)
622-
or (isinstance(self.state, Recording) and self.state.open_gripper_steps is not None),
623529
)
624530
self.last_view = view
625531
return view

0 commit comments

Comments
 (0)