From 2393472d7fd5917e0ead47f44a81ae2c8e27fcc3 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 6 Nov 2024 11:24:35 +0800 Subject: [PATCH] Fix Iter IK bug --- src/compas_fab/backends/interfaces/backend_features.py | 10 +++------- .../backend_features/pybullet_inverse_kinematics.py | 7 +++++++ src/compas_fab/robots/robot_library.py | 2 +- tests/backends/pybullet/test_pybullet_planner_fk_ik.py | 6 +++++- tests/robots/test_targets.py | 4 ++-- 5 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index b4c667374..67e04ba60 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/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() diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index 6f87d327d..fc928c62f 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/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): diff --git a/src/compas_fab/robots/robot_library.py b/src/compas_fab/robots/robot_library.py index 1b9020f2c..07582e2fc 100644 --- a/src/compas_fab/robots/robot_library.py +++ b/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 diff --git a/tests/backends/pybullet/test_pybullet_planner_fk_ik.py b/tests/backends/pybullet/test_pybullet_planner_fk_ik.py index 6cd3774a3..2be232d5c 100644 --- a/tests/backends/pybullet/test_pybullet_planner_fk_ik.py +++ b/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) diff --git a/tests/robots/test_targets.py b/tests/robots/test_targets.py index d49612ab2..a6dbd794a 100644 --- a/tests/robots/test_targets.py +++ b/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__)