Skip to content

Commit 51d8fdd

Browse files
committed
Use an explicit motor mapping for SO-101 calibration
1 parent 711d2d8 commit 51d8fdd

9 files changed

Lines changed: 207 additions & 85 deletions

File tree

‎runtime/src/setup.rs‎

Lines changed: 37 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -16,21 +16,43 @@ pub enum ModelVariant {
1616
SoftGripperCamera,
1717
}
1818

19-
#[derive(Clone, Copy, Default, Deserialize)]
20-
#[serde(rename_all = "kebab-case")]
21-
pub enum MotorIdOrder {
22-
#[default]
23-
Ascending,
24-
Descending,
19+
#[derive(Clone, Copy, Deserialize)]
20+
#[serde(deny_unknown_fields)]
21+
pub struct MotorIds {
22+
shoulder_pan: u8,
23+
shoulder_lift: u8,
24+
elbow_flex: u8,
25+
wrist_flex: u8,
26+
wrist_roll: u8,
27+
gripper: u8,
2528
}
2629

27-
impl MotorIdOrder {
30+
impl MotorIds {
2831
#[must_use]
2932
pub fn motor_ids_in_joint_order(self) -> ([u8; 5], u8) {
30-
match self {
31-
Self::Ascending => ([1, 2, 3, 4, 5], 6),
32-
Self::Descending => ([6, 5, 4, 3, 2], 1),
33-
}
33+
(
34+
[
35+
self.shoulder_pan,
36+
self.shoulder_lift,
37+
self.elbow_flex,
38+
self.wrist_flex,
39+
self.wrist_roll,
40+
],
41+
self.gripper,
42+
)
43+
}
44+
45+
fn validate(self) -> anyhow::Result<()> {
46+
let (joints, gripper) = self.motor_ids_in_joint_order();
47+
let mut ids = [
48+
joints[0], joints[1], joints[2], joints[3], joints[4], gripper,
49+
];
50+
ids.sort_unstable();
51+
ensure!(
52+
ids == [1, 2, 3, 4, 5, 6],
53+
"Motor mapping must use IDs 1-6 exactly once"
54+
);
55+
Ok(())
3456
}
3557
}
3658

@@ -40,8 +62,7 @@ pub struct ArmSetup {
4062
pub serial: String,
4163
#[serde(default)]
4264
pub model_variant: ModelVariant,
43-
#[serde(default)]
44-
pub motor_id_order: MotorIdOrder,
65+
pub motor_ids: MotorIds,
4566
#[serde(default)]
4667
pub alignment: Alignment,
4768
#[serde(default)]
@@ -141,11 +162,12 @@ impl ArmSetup {
141162
.all(|id| id.parse::<u8>().is_ok_and(|id| (1..=6).contains(&id))),
142163
"Gain overrides must name an SO-101 motor (IDs 1-6)"
143164
);
165+
setup.motor_ids.validate()?;
144166
Ok(setup)
145167
}
146168
}
147169

148-
impl MotorIdOrder {
170+
impl MotorIds {
149171
/// Check that the named calibration entries agree with the physical joint order.
150172
///
151173
/// # Errors
@@ -155,19 +177,13 @@ impl MotorIdOrder {
155177
serde_json::from_slice(&std::fs::read(path)?)
156178
.with_context(|| format!("Invalid SO-101 calibration {}", path.display()))?;
157179
let joint_names = SO101_JOINT_NAMES.into_iter().chain(["gripper"]);
158-
// Older ascending setups could use calibration keys with only bus-level meaning.
159-
if matches!(self, Self::Ascending)
160-
&& !joint_names.clone().any(|name| entries.contains_key(name))
161-
{
162-
return Ok(());
163-
}
164180
let (joint_ids, gripper_id) = self.motor_ids_in_joint_order();
165181
ensure!(
166182
entries.len() == 6
167183
&& joint_names
168184
.zip(joint_ids.into_iter().chain([gripper_id]))
169185
.all(|(name, id)| entries.get(name).is_some_and(|entry| entry.id == id)),
170-
"Calibration {} does not match the setup's motor ID order; recalibrate the arm",
186+
"Calibration {} does not match the setup's motor ID mapping; recalibrate the arm",
171187
path.display()
172188
);
173189
Ok(())

‎setup/README.md‎

Lines changed: 5 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -59,19 +59,11 @@ The default store is `$SO101_SETUP_DIRECTORY`, otherwise
5959
`$XDG_CONFIG_HOME/so101-setup` (defaulting to `~/.config/so101-setup`).
6060

6161
Motor IDs may run from `1` to `6` or from `6` to `1` from the base to the
62-
gripper. The default is ascending. To calibrate an arm with descending IDs,
63-
use the USB serial shown in the calibration tool:
64-
65-
```sh
66-
so101-calibrate --serial ARM_A --motor-id-order descending
67-
```
68-
69-
For an arm with a saved order, omit both options to reuse it. To change the order
70-
back, use `--motor-id-order ascending` with the serial and record a fresh
71-
calibration. During live review, move shoulder pan and the gripper separately
72-
by hand with torque off. Confirm that each moves its matching joint in the
73-
preview before saving. Save records `motor_id_order` in `setup.toml` and leaves
74-
the programmed servo IDs intact.
62+
gripper. The default is ascending. For a different order, open **Edit motor mapping** before the sweep. Choose
63+
an ascending or descending shortcut, or assign each motor ID to a joint. IDs
64+
1 through 6 must each appear once. Move the joints by hand with torque off to
65+
verify the correspondence. Save records the mapping in the `[motor_ids]`
66+
table and leaves the programmed servo IDs intact.
7567

7668
New arms default to the soft gripper with wrist camera and start calibration in
7769
the resting pose. For a stock gripper and midpoint reference pose, start with

‎setup/src/so101_setup/calibration.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -676,7 +676,7 @@ def save(self, *, model_verified: bool) -> None:
676676
{
677677
"serial": self.serial,
678678
"model_variant": self.setup.model_variant,
679-
"motor_id_order": self.setup.motor_id_order,
679+
"motor_ids": self.setup.motor_ids,
680680
"alignment": {
681681
"joint_signs": draft.signs[:5],
682682
"joint_offsets_radians": draft.offsets[:5],

‎setup/src/so101_setup/configuration.py‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,12 @@
1818
MOTOR_IDS = [1, 2, 3, 4, 5, 6]
1919
ModelVariant = Literal["stock", "soft-gripper-camera"]
2020
DEFAULT_NEW_ARM_MODEL: ModelVariant = "soft-gripper-camera"
21-
MotorIdOrder = Literal["ascending", "descending"]
21+
MOTOR_NAMES = (*SO101_JOINT_NAMES, "gripper")
22+
MotorIds = dict[str, Annotated[int, Field(strict=True, ge=1, le=6)]]
23+
24+
25+
def motor_id_mapping(ids: list[int]) -> dict[str, int]:
26+
return dict(zip(MOTOR_NAMES, ids, strict=True))
2227

2328

2429
class SetupSection(BaseModel):
@@ -71,17 +76,21 @@ class ServoSettings(SetupSection):
7176
class ArmSetup(SetupSection):
7277
serial: ControllerSerial
7378
model_variant: ModelVariant = "stock"
74-
motor_id_order: MotorIdOrder = "ascending"
79+
motor_ids: MotorIds = Field(default_factory=lambda: motor_id_mapping(MOTOR_IDS))
7580
alignment: ModelAlignment = Field(default_factory=ModelAlignment)
7681
gripper: GripperEndpoints = Field(default_factory=GripperEndpoints)
7782
servos: ServoSettings = Field(default_factory=ServoSettings)
7883
servo_registers: ServoRegisterSettings = Field(default_factory=ServoRegisterSettings)
7984

8085
def motor_ids_in_joint_order(self) -> list[int]:
81-
return list(MOTOR_IDS) if self.motor_id_order == "ascending" else MOTOR_IDS[::-1]
86+
return [self.motor_ids[name] for name in MOTOR_NAMES]
8287

8388
@model_validator(mode="after")
8489
def require_known_gain_overrides(self) -> Self:
90+
if set(self.motor_ids) != set(MOTOR_NAMES) or set(self.motor_ids.values()) != set(
91+
MOTOR_IDS
92+
):
93+
raise ValueError("Motor mapping must assign IDs 1-6 exactly once to the six joints")
8594
if set(self.servos.expected_position_gains_by_motor) - set(MOTOR_IDS):
8695
raise ValueError("Gain overrides must name an SO-101 motor (IDs 1-6)")
8796
return self

‎setup/src/so101_setup/server.py‎

Lines changed: 50 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,14 @@
3232
Reference,
3333
Saved,
3434
)
35-
from .configuration import DEFAULT_NEW_ARM_MODEL, ModelVariant, MotorIdOrder
35+
from .configuration import (
36+
DEFAULT_NEW_ARM_MODEL,
37+
MOTOR_IDS,
38+
ArmSetup,
39+
ModelVariant,
40+
MotorIds,
41+
motor_id_mapping,
42+
)
3643
from .device import RegisterBus, RegisterReadError
3744
from .discovery import ControllerChoice, discover_controllers
3845
from .motor_ids import FOLLOWER_MOTORS, program_motor_id, programming_ports
@@ -70,6 +77,11 @@ class SimpleAction(ActionModel):
7077
]
7178

7279

80+
class MapMotorIdsAction(ActionModel):
81+
action: Literal["map_motor_ids"]
82+
motor_ids: MotorIds
83+
84+
7385
class StartSweepAction(ActionModel):
7486
action: Literal["start_sweep"]
7587
joint_index: int | None = Field(default=None, ge=0, le=5, strict=True)
@@ -99,6 +111,7 @@ class SaveAction(ActionModel):
99111
Action = Annotated[
100112
ConnectAction
101113
| SimpleAction
114+
| MapMotorIdsAction
102115
| StartSweepAction
103116
| ReverseAction
104117
| AdjustAction
@@ -122,7 +135,7 @@ class ApplicationView(BaseModel):
122135
sweep_only: bool = False
123136
can_recalibrate_joint: bool = False
124137
offer_servo_reset: bool = False
125-
motor_id_order: MotorIdOrder = "ascending"
138+
motor_ids: dict[str, int] = Field(default_factory=lambda: motor_id_mapping(MOTOR_IDS))
126139
checking_motor_order: bool = False
127140
detected_base_motor: Literal[1, 6] | None = None
128141
servo_reset_incomplete: bool = False
@@ -143,13 +156,10 @@ def __init__(
143156
calibration_root: Path,
144157
store: SetupStore | None = None,
145158
saved_command: Callable[[str], str] | None = None,
146-
*,
147-
motor_id_order_override: tuple[str, MotorIdOrder] | None = None,
148159
) -> None:
149160
self.config_directory = config_directory.resolve()
150161
self.store = NativeSetupStore() if store is None else store
151162
self.saved_command = saved_command
152-
self.motor_id_order_override = motor_id_order_override
153163
self.calibration_root = calibration_root
154164
self.session: CalibrationSession | None = None
155165
self.auto_connect = True
@@ -204,11 +214,7 @@ def sample_views(self) -> None:
204214
self.stream_stop.wait(0.2)
205215

206216
def controllers(self) -> list[ControllerChoice]:
207-
controllers = discover_controllers(self.config_directory, self.store)
208-
if self.motor_id_order_override is None:
209-
return controllers
210-
override_serial, _ = self.motor_id_order_override
211-
return [controller for controller in controllers if controller.serial == override_serial]
217+
return discover_controllers(self.config_directory, self.store)
212218

213219
@staticmethod
214220
def read_calibration_registers(bus: RegisterBus) -> dict[str, list[int]]:
@@ -217,7 +223,7 @@ def read_calibration_registers(bus: RegisterBus) -> dict[str, list[int]]:
217223
for name in ("id", "homing_offset", "min_position_limit", "max_position_limit")
218224
}
219225

220-
def connect(self, serial: str, order: MotorIdOrder | None = None) -> None:
226+
def connect(self, serial: str, motor_ids: dict[str, int] | None = None) -> None:
221227
if self.session is not None:
222228
raise ValueError("Disconnect the current arm before selecting another")
223229
candidates = [
@@ -228,18 +234,8 @@ def connect(self, serial: str, order: MotorIdOrder | None = None) -> None:
228234
controller = candidates[0]
229235
directory = self.config_directory / "arms" / serial
230236
setup = self.store.load(directory, serial)
231-
if order is None and self.motor_id_order_override is not None:
232-
_, order = self.motor_id_order_override
233-
if order is not None and order != setup.motor_id_order:
234-
gains = setup.servos.expected_position_gains_by_motor
235-
servos = setup.servos.model_copy(
236-
update={
237-
"expected_position_gains_by_motor": {
238-
7 - motor_id: values for motor_id, values in gains.items()
239-
}
240-
}
241-
)
242-
setup = setup.model_copy(update={"motor_id_order": order, "servos": servos})
237+
if motor_ids is not None:
238+
setup = self.with_motor_ids(setup, motor_ids)
243239
bus = RegisterBus(controller.port, setup.motor_ids_in_joint_order())
244240
try:
245241
session = CalibrationSession(setup, directory, self.calibration_root, bus, self.store)
@@ -252,6 +248,23 @@ def connect(self, serial: str, order: MotorIdOrder | None = None) -> None:
252248
bus.close()
253249
raise
254250

251+
@staticmethod
252+
def with_motor_ids(setup: ArmSetup, motor_ids: dict[str, int]) -> ArmSetup:
253+
remapped = ArmSetup.model_validate(setup.model_dump() | {"motor_ids": motor_ids})
254+
old_gains = setup.servos.expected_position_gains_by_motor
255+
gains = {
256+
remapped.motor_ids[joint]: old_gains[old_id]
257+
for joint, old_id in setup.motor_ids.items()
258+
if old_id in old_gains
259+
}
260+
return remapped.model_copy(
261+
update={
262+
"servos": setup.servos.model_copy(
263+
update={"expected_position_gains_by_motor": gains}
264+
)
265+
}
266+
)
267+
255268
def reconnect(self, session: CalibrationSession) -> None:
256269
controller = next(
257270
(
@@ -322,7 +335,7 @@ def view(self) -> ApplicationView:
322335
return ApplicationView(stage="error", serial=session.serial, message=str(error))
323336
view.servo_reset_incomplete = session.serial in self.incomplete_servo_resets
324337
view.offer_servo_reset = view.stage == "reference" and not self.servo_reset_decided
325-
view.motor_id_order = session.setup.motor_id_order
338+
view.motor_ids = session.setup.motor_ids
326339
view.checking_motor_order = self.motor_order_sample is not None
327340
view.detected_base_motor = self.detected_base_motor
328341
view.model_variant = self.session.setup.model_variant
@@ -352,7 +365,7 @@ def reset_servos(self, session: CalibrationSession) -> None:
352365
except Exception as error:
353366
raise RuntimeError(f"Reset stopped on motor {motor_id}: {error}") from error
354367
finally:
355-
self.connect(serial, session.setup.motor_id_order)
368+
self.connect(serial, session.setup.motor_ids)
356369
if self.session is not None:
357370
self.session.restart()
358371
self.incomplete_servo_resets.discard(serial)
@@ -430,20 +443,29 @@ def check_motor_order(self, session: CalibrationSession) -> None:
430443
def apply_detected_motor_order(self, session: CalibrationSession) -> None:
431444
if not isinstance(session.state, Reference) or self.detected_base_motor is None:
432445
raise ValueError("Check which motor moves the base first")
433-
order: MotorIdOrder = "ascending" if self.detected_base_motor == 1 else "descending"
434-
if order == session.setup.motor_id_order:
446+
ids = MOTOR_IDS if self.detected_base_motor == 1 else MOTOR_IDS[::-1]
447+
self.apply_motor_mapping(session, motor_id_mapping(ids))
448+
449+
def apply_motor_mapping(self, session: CalibrationSession, motor_ids: dict[str, int]) -> None:
450+
if not isinstance(session.state, Reference):
451+
raise ValueError("Start a new calibration before changing the motor mapping")
452+
self.with_motor_ids(session.setup, motor_ids)
453+
session.require_torque_off()
454+
if motor_ids == session.setup.motor_ids:
435455
self.detected_base_motor = None
436456
return
437457
reset_decided = self.servo_reset_decided
438458
self.close_session()
439459
self.auto_connect = False
440-
self.connect(session.serial, order)
460+
self.connect(session.serial, motor_ids)
441461
if self.session is not None:
442462
self.session.restart()
443463
self.servo_reset_decided = reset_decided
444464

445465
def apply_session_action(self, session: CalibrationSession, action: Action) -> None:
446466
match action:
467+
case MapMotorIdsAction(motor_ids=motor_ids):
468+
self.apply_motor_mapping(session, motor_ids)
447469
case SimpleAction(action="reopen"):
448470
if session.view().stage != "saved":
449471
raise ValueError("Reopen is available after saving")
@@ -743,25 +765,13 @@ def command_line() -> None:
743765
default=DEFAULT_NEW_ARM_MODEL,
744766
help="Model for new arms (default: soft-gripper-camera). Saved arms keep their model.",
745767
)
746-
parser.add_argument("--serial", help="USB serial of the arm whose motor ID order is being set")
747-
parser.add_argument(
748-
"--motor-id-order",
749-
choices=("ascending", "descending"),
750-
help="Motor IDs from base to gripper for the selected serial. Saved after calibration.",
751-
)
752768
parser.add_argument("--assets", type=Path, default=asset_directory())
753769
parser.add_argument("--port", type=int, default=8795)
754770
arguments = parser.parse_args()
755-
if (arguments.serial is None) != (arguments.motor_id_order is None):
756-
parser.error("--serial and --motor-id-order must be used together")
757-
motor_id_order_override = (
758-
(arguments.serial, arguments.motor_id_order) if arguments.serial is not None else None
759-
)
760771
application = CalibrationApplication(
761772
arguments.setup_directory,
762773
calibration_root(),
763774
NativeSetupStore(arguments.model_variant),
764-
motor_id_order_override=motor_id_order_override,
765775
)
766776
serve_application(application, arguments.assets, arguments.port, setup_lock())
767777

‎setup/src/so101_setup/storage.py‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,8 @@ def load(self, directory: Path, serial: str) -> ArmSetup:
4747

4848
@staticmethod
4949
def with_servo_defaults(directory: Path, setup: ArmSetup) -> ArmSetup:
50-
shoulder_id, elbow_id = (2, 3) if setup.motor_id_order == "ascending" else (5, 4)
50+
shoulder_id = setup.motor_ids["shoulder_lift"]
51+
elbow_id = setup.motor_ids["elbow_flex"]
5152
override_path = directory / "servo-overrides.toml"
5253
register_settings = (
5354
ServoRegisterSettings.model_validate(tomllib.loads(override_path.read_text()))

0 commit comments

Comments
 (0)