Fix Iter IK bug

src/compas_fab/backends/interfaces/backend_features.py

@@ -313,11 +313,14 @@ def inverse_kinematics(self, target, robot_cell_state=None, group=None, options=
         request_hash = (target.sha256(), robot_cell_state.sha256(), str(group), str(options))
 
         if self._last_ik_request["request_hash"] == request_hash and self._last_ik_request["solutions"] is not None:
+            print("Inverse Kinematics Hash: Last IK Generator reused")
             solution = next(self._last_ik_request["solutions"], None)
             # NOTE: If the iterator is exhausted, solution will be None, subsequent code outside will reset the generator
             if solution is not None:
                 return solution
+            print("- Unfortunately, the last IK generator is exhausted, re-initializing the generator...")
 
+        print("Inverse Kinematics Hash: New IK Generator created")
         solutions = self.iter_inverse_kinematics(target, robot_cell_state, group, options)
         self._last_ik_request["request_hash"] = request_hash
         self._last_ik_request["solutions"] = solutions
@@ -374,13 +377,6 @@ def iter_inverse_kinematics(self, target, robot_cell_state=None, group=None, opt
         robot_cell = client.robot_cell  # type: RobotCell
         group = group or robot_cell.main_group_name
 
-        # Make a copy of the options before passing it to the planner's iter_inverse_kinematics method,
-        # which is likely to modify its content.
-        # Note: Without making a copy, the options dict will break the hashing function in the inverse_kinematics()
-        options = options or {}
-        options = deepcopy(options) if options else {}
-        robot_cell_state = deepcopy(robot_cell_state)
-
         # Unit conversion from user scale to meter scale can be done here because they are shared.
         # This will be triggered too, when entering from the inverse_kinematics method.
         target = target.normalized_to_meters()

src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py

@@ -167,6 +167,13 @@ def iter_inverse_kinematics(self, target, robot_cell_state, group=None, options=
         # Keep track of user input for checking against the final result
         initial_start_configuration = deepcopy(robot_cell_state.robot_configuration)
 
+        # Make a copy of the options before passing it to the iter_inverse_kinematics method,
+        # which is likely to modify its content.
+        # Note: Without making a copy, the changed `options` and `robot_cell_state` will break the hashing function in the inverse_kinematics()
+        options = options or {}
+        options = deepcopy(options) if options else {}
+        robot_cell_state = deepcopy(robot_cell_state)
+
         if isinstance(target, FrameTarget):
             ik_generator = self._iter_inverse_kinematics_frame_target(target, robot_cell_state, group, options)
         elif isinstance(target, PointAxisTarget):

src/compas_fab/robots/robot_library.py

@@ -140,7 +140,7 @@ def printing_tool(cls, load_geometry=True, tool_size=1.0):
             tool_mesh.scale(tool_size)
             meshes.append(tool_mesh)
 
-        tool_model.add_link("printing_tool_link", visual_mesh=meshes, collision_mesh=meshes)
+        tool_model.add_link("printing_tool_link", visual_meshes=meshes, collision_meshes=meshes)
 
         return tool_model
 

tests/backends/pybullet/test_pybullet_planner_fk_ik.py

@@ -755,7 +755,11 @@ def test_iter_ik_point_axis_target(planner_with_test_cell):
     }
 
     # This target is reachable and collision free
-    target = PointAxisTarget([0.5, 0.5, 1.0], [1.0, 0.0, 0.0], TargetMode.ROBOT)
+    target = PointAxisTarget(
+        [0.5, 0.5, 1.0],
+        [1.0, 0.0, 0.0],
+        TargetMode.ROBOT,
+    )
 
     # Assert that if someone forgot to provide a group or an invalid one, it will raise an error
     generator = planner._iter_inverse_kinematics_point_axis_target(target, robot_cell_state, options)

tests/robots/test_targets.py

@@ -178,12 +178,12 @@ def test_serialization_constraint_sets(target_frame, target_configuration, tool_
     tolerances_orientation = [0.0123] * 3
     orientation_constraint_weight = 0.789
     orientation_constraint = OrientationConstraint.from_frame(
-        target_frame, tolerances_orientation, link_name, tool_coordinate_frame, orientation_constraint_weight
+        target_frame, tolerances_orientation, link_name, orientation_constraint_weight
     )
     position_constraint_weight = 0.456
     tolerances_position = 0.567
     position_constraint = PositionConstraint.from_frame(
-        target_frame, tolerances_position, link_name, tool_coordinate_frame, position_constraint_weight
+        target_frame, tolerances_position, link_name, position_constraint_weight
     )
     target = ConstraintSetTarget([orientation_constraint, position_constraint], name)
     nt = ConstraintSetTarget.__from_data__(target.__data__)