@@ -188,23 +188,66 @@ def add_camera_module(
188188 )
189189
190190
191- def build_model () -> None :
192- manifest = tomllib .loads ((SOURCE_DIRECTORY / "sources.toml" ).read_text ())
193- source = manifest ["gripper" ]
194- source_path = SOURCE_DIRECTORY / source ["file" ]
195- if hashlib .sha256 (source_path .read_bytes ()).hexdigest () != source ["sha256" ]:
191+ def carrier_reference_angle (
192+ carrier : cadquery .Shape , attachment : Vector , joint_position : Vector
193+ ) -> float :
194+ """Use the horn's square bolt pattern as the carrier's angular datum."""
195+ horn_centers : list [Vector ] = []
196+ for face in carrier .Faces ():
197+ if face .geomType () != "CYLINDER" :
198+ continue
199+ cylinder = face ._geomAdaptor ().Cylinder ()
200+ if not math .isclose (cylinder .Radius (), 1.6 , abs_tol = 1e-5 ):
201+ continue
202+ location = numpy .array (cylinder .Location ().Coord ())
203+ center = attachment [:3 , :3 ] @ location + attachment [:3 , 3 ]
204+ if math .isclose (
205+ numpy .linalg .norm (center [[0 , 2 ]] * 0.001 - joint_position [[0 , 2 ]]),
206+ 0.007 ,
207+ abs_tol = 0.00001 ,
208+ ) and not any (
209+ numpy .linalg .norm (center [[0 , 2 ]] - existing [[0 , 2 ]]) < 0.001
210+ for existing in horn_centers
211+ ):
212+ horn_centers .append (center )
213+ if len (horn_centers ) != 4 :
214+ raise ValueError ("Expected four 7 mm-radius horn screw axes" )
215+ lower_holes = sorted (horn_centers , key = lambda point : point [0 ])[:2 ]
216+ horn_direction = lower_holes [0 ] - lower_holes [1 ]
217+ if horn_direction [2 ] < 0 :
218+ horn_direction = - horn_direction
219+ return math .atan2 (- horn_direction [0 ], horn_direction [2 ])
220+
221+
222+ def read_assembly_solids (
223+ source_path : Path , expected_sha256 : str
224+ ) -> tuple [
225+ cadquery .Solid , cadquery .Solid , cadquery .Solid , cadquery .Solid , cadquery .Solid
226+ ]:
227+ if hashlib .sha256 (source_path .read_bytes ()).hexdigest () != expected_sha256 :
196228 raise ValueError ("Gripper CAD does not match its pinned checksum" )
197- imported_solids = cadquery .importers .importStep (str (source_path )).solids ().vals ()
198- solids : list [cadquery .Shape ] = []
199- for imported_solid in imported_solids :
229+ solids : list [cadquery .Solid ] = []
230+ for imported_solid in (
231+ cadquery .importers .importStep (str (source_path )).solids ().vals ()
232+ ):
200233 if not isinstance (imported_solid , cadquery .Solid ):
201234 raise TypeError ("Expected solid CAD components" )
202235 solids .append (imported_solid )
203- if len (solids ) != 5 :
204- raise ValueError (
205- "Expected body, carrier, fixed finger, moving finger and camera mount"
206- )
207- body , carrier , fixed_finger , moving_finger , camera_mount = solids
236+ match solids :
237+ case [body , carrier , fixed_finger , moving_finger , camera_mount ]:
238+ return body , carrier , fixed_finger , moving_finger , camera_mount
239+ case _:
240+ raise ValueError (
241+ "Expected body, carrier, fixed finger, moving finger and camera mount"
242+ )
243+
244+
245+ def build_model () -> None :
246+ manifest = tomllib .loads ((SOURCE_DIRECTORY / "sources.toml" ).read_text ())
247+ source = manifest ["gripper" ]
248+ body , carrier , fixed_finger , moving_finger , camera_mount = read_assembly_solids (
249+ SOURCE_DIRECTORY / source ["file" ], source ["sha256" ]
250+ )
208251 attachment = wrist_attachment_transform (body )
209252 mesh_prefix = "soft-gripper-camera"
210253 meshes = {
@@ -222,46 +265,21 @@ def build_model() -> None:
222265 if gripper is None or moving_jaw is None or gripper_joint_origin is None :
223266 raise ValueError ("Stock model is missing its gripper joint frames" )
224267 for geometry_kind in ("visual" , "collision" ):
225- for element in list ( gripper .findall (geometry_kind ) ):
268+ for element in gripper .findall (geometry_kind ):
226269 mesh_element = element .find ("geometry/mesh" )
227270 if (
228271 mesh_element is not None
229272 and mesh_element .get ("filename" ) == "wrist_roll_follower_so101_v1.stl"
230273 ):
231274 gripper .remove (element )
232- for element in list ( moving_jaw .findall (geometry_kind ) ):
275+ for element in moving_jaw .findall (geometry_kind ):
233276 moving_jaw .remove (element )
234277 joint_position = numpy .array (
235278 [float (value ) for value in gripper_joint_origin .attrib ["xyz" ].split ()]
236279 )
237280 joint_rotation = trimesh .transformations .euler_matrix (math .pi / 2 , 0 , 0 )
238281 joint_rotation [:3 , 3 ] = joint_position
239- # The horn's square bolt pattern establishes the carrier's angular datum.
240- horn_centers = []
241- for face in carrier .Faces ():
242- if face .geomType () != "CYLINDER" :
243- continue
244- cylinder = face ._geomAdaptor ().Cylinder ()
245- if not math .isclose (cylinder .Radius (), 1.6 , abs_tol = 1e-5 ):
246- continue
247- location = numpy .array (cylinder .Location ().Coord ())
248- center = attachment [:3 , :3 ] @ location + attachment [:3 , 3 ]
249- if math .isclose (
250- numpy .linalg .norm (center [[0 , 2 ]] * 0.001 - joint_position [[0 , 2 ]]),
251- 0.007 ,
252- abs_tol = 0.00001 ,
253- ) and not any (
254- numpy .linalg .norm (center [[0 , 2 ]] - existing [[0 , 2 ]]) < 0.001
255- for existing in horn_centers
256- ):
257- horn_centers .append (center )
258- if len (horn_centers ) != 4 :
259- raise ValueError ("Expected four 7 mm-radius horn screw axes" )
260- lower_holes = sorted (horn_centers , key = lambda point : point [0 ])[:2 ]
261- horn_direction = lower_holes [0 ] - lower_holes [1 ]
262- if horn_direction [2 ] < 0 :
263- horn_direction = - horn_direction
264- reference_angle = math .atan2 (- horn_direction [0 ], horn_direction [2 ])
282+ reference_angle = carrier_reference_angle (carrier , attachment , joint_position )
265283 reference_rotation = trimesh .transformations .rotation_matrix (
266284 reference_angle , [0 , 0 , 1 ]
267285 )
0 commit comments