Bug fix on grasp transform when pose is not identity matrix

yunshengtian · Nov 26, 2024 · 35a67c0 · 35a67c0
1 parent 79f2f71
commit 35a67c0
Show file tree

Hide file tree

Showing 5 changed files with 3 additions and 7 deletions.
diff --git a/plan_robot/render_grasp.py b/plan_robot/render_grasp.py
@@ -188,7 +188,6 @@ def render_path_with_grasp(asset_folder, assembly_dir, move_id, still_ids, remov
         sim.set_body_color_map(body_color_map)
 
     # get gripper path
-    gripper_pos, gripper_quat = get_pos_quat_from_pose(gripper_pos, gripper_quat, pose)
     gripper_path = get_gripper_path_from_part_path(part_path, gripper_pos, gripper_quat)
 
     # transform from global coordinate to local coordinate

diff --git a/plan_robot/render_grasp_arm.py b/plan_robot/render_grasp_arm.py
@@ -293,7 +293,6 @@ def render_path_with_grasp_and_arm(asset_folder, assembly_dir, move_id, still_id
         sim.set_body_color_map(body_color_map)
 
     # get gripper path
-    gripper_pos, gripper_quat = get_pos_quat_from_pose(gripper_pos, gripper_quat, pose)
     gripper_path = get_gripper_path_from_part_path(part_path, gripper_pos, gripper_quat)
 
     # get part to gripper transform

diff --git a/plan_robot/run_grasp_arm_plan.py b/plan_robot/run_grasp_arm_plan.py
@@ -128,8 +128,7 @@ def plan(self, move_id, still_ids, removed_ids, pose, path, early_terminate=True
 
         disassembly_direction = path[-1][:3] - path[0][:3]
         disassembly_direction /= np.linalg.norm(disassembly_direction)
-        disassembly_direction_local = pose[:3, :3].T @ disassembly_direction if pose is not None else disassembly_direction
-        grasps_final = self.generate_grasps(move_mesh_final, disassembly_direction_local)
+        grasps_final = self.generate_grasps(move_mesh_final, disassembly_direction)
 
         grasps = []
         success = False

diff --git a/plan_robot/run_grasp_plan.py b/plan_robot/run_grasp_plan.py
@@ -119,8 +119,7 @@ def plan(self, move_id, still_ids, removed_ids, pose, path, early_terminate=True
 
         disassembly_direction = path[-1][:3] - path[0][:3]
         disassembly_direction /= np.linalg.norm(disassembly_direction)
-        disassembly_direction_local = pose[:3, :3].T @ disassembly_direction if pose is not None else disassembly_direction
-        grasps_final = self.generate_grasps(move_mesh_final, disassembly_direction_local)
+        grasps_final = self.generate_grasps(move_mesh_final, disassembly_direction)
 
         grasps = []
         success = False

diff --git a/plan_sequence/play_logged_plan.py b/plan_sequence/play_logged_plan.py
@@ -122,7 +122,7 @@ def play_logged_plan(asset_folder, assembly_dir, sequence, tree, result_dir, sav
                 grasps = None
 
             parts_fix = sim_info['parts_fix']
-            parts_free = [part_i for part_i in parts_rest if part_i not in parts_fix] + [part_move]
+            parts_free = [part_i for part_i in parts_rest if parts_fix is None or part_i not in parts_fix] + [part_move]
 
             if show_fix:
                 body_color_dict = get_body_color_dict(parts_fix, parts_free) # visualize fixes