1010import math
1111import os
1212import tempfile
13+ import time
1314from copy import deepcopy
1415from dataclasses import dataclass , field
1516from datetime import UTC , datetime
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.
3334MINIMUM_SWEEP_DEGREES = (130 , 130 , 120 , 130 , 220 , 75 )
34- REFERENCE_JOINTS = [0.0 ] * 5
35- REFERENCE_GRIPPER_RADIANS = 0.75
3635CALIBRATION_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
159126class 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
167148class 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
196177class 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
204186class 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