Class to execute grasps on the Franka Emika fr3 robot.
| 43 | |
| 44 | |
| 45 | class GraspExecutor: |
| 46 | """Class to execute grasps on the Franka Emika fr3 robot.""" |
| 47 | |
| 48 | |
| 49 | def computeIK(self, orientation, position, ik_link_name = "fr3_hand_tcp", move_group="fr3_manipulator") -> bool: |
| 50 | """Check if a given pose is reachable for the robot. Return True if it is, False otherwise.""" |
| 51 | |
| 52 | # Create a pose to compute IK for |
| 53 | pose_stamped = get_stamped_pose(position, orientation, self.frame) |
| 54 | |
| 55 | ik_request = PositionIKRequest() |
| 56 | ik_request.group_name = move_group |
| 57 | ik_request.ik_link_name = ik_link_name |
| 58 | ik_request.pose_stamped = pose_stamped |
| 59 | ik_request.robot_state = self.robot.get_current_state() |
| 60 | ik_request.avoid_collisions = True |
| 61 | |
| 62 | |
| 63 | request_value = self.compute_ik(ik_request) |
| 64 | |
| 65 | if request_value.error_code.val == -31: |
| 66 | return False |
| 67 | |
| 68 | if request_value.error_code.val == 1: |
| 69 | # Check if the IK is at the limits |
| 70 | joint_positions = np.array(request_value.solution.joint_state.position[:7] ) |
| 71 | upper_diff = np.min(np.abs(joint_positions - self.upper_limit)) |
| 72 | lower_diff = np.min(np.abs(joint_positions - self.lower_limit)) |
| 73 | return min(upper_diff, lower_diff) > 0.1 |
| 74 | else: |
| 75 | return False |
| 76 | |
| 77 | |
| 78 | def reset_scene(self): |
| 79 | """Reset the scene to the initial state.""" |
| 80 | self.scene.clear() |
| 81 | self.objects = {} |
| 82 | self.load_scene(load_wall=self.load_wall) |
| 83 | |
| 84 | def load_scene(self, load_wall:bool=True): |
| 85 | """Load the scene in the MoveIt! planning scene. |
| 86 | |
| 87 | Loads a static floor and if needed, a static wall to the MoveIt! planning scene. |
| 88 | """ |
| 89 | if load_wall: |
| 90 | wall_pose = get_stamped_pose([0, 0.47, 0.0], [0, 0, 0, 1], "fr3_link0") |
| 91 | self.scene.add_box("wall", wall_pose, size=(3, 0.02, 3)) |
| 92 | # Misc variables |
| 93 | wall_pose = get_stamped_pose([1.7, 0.0, -0.025], [0, 0, 0, 1], "fr3_link0") |
| 94 | self.scene.add_box("floor", wall_pose, size=(3, 3, 0)) |
| 95 | |
| 96 | wall_pose = get_stamped_pose([-0.50, 0.0, 0.0], [0, 0, 0, 1], "fr3_link0") |
| 97 | self.scene.add_box("wall", wall_pose, size=(0.02, 3, 3)) |
| 98 | |
| 99 | cam_pose = get_stamped_pose([0.03, 0, 0.01], [0, 0, 0, 1], "fr3_hand") |
| 100 | self.scene.add_box("cam", cam_pose, size = (0.04, 0.14, 0.02)) |
| 101 | self.scene.attach_mesh("fr3_hand", f"cam", touch_links=[*self.robot.get_link_names(group= "fr3_hand"), "fr3_joint7"]) |
| 102 |
no outgoing calls
no test coverage detected