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+ )
3643from .device import RegisterBus , RegisterReadError
3744from .discovery import ControllerChoice , discover_controllers
3845from .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+
7385class 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):
99111Action = 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
0 commit comments