Executes a grasp at a given pose with given orientation. Args: position: The position of the grasp. orientation: The orientation of the grasp (scipy format, xyzw). width: The width of the gripper. pre_grasp_approach: The distance to
(self, position, orientation, width = 0.025, pre_grasp_approach = 0.05, dryrun = False, object_id = None, verbose = True)
| 261 | |
| 262 | @_block |
| 263 | def grasp(self, position, orientation, width = 0.025, pre_grasp_approach = 0.05, dryrun = False, object_id = None, verbose = True): |
| 264 | """Executes a grasp at a given pose with given orientation. |
| 265 | |
| 266 | Args: |
| 267 | position: The position of the grasp. |
| 268 | orientation: The orientation of the grasp (scipy format, xyzw). |
| 269 | width: The width of the gripper. |
| 270 | pre_grasp_approach: The distance to move towards the object before grasping. |
| 271 | dryrun: If true, the robot will not call the action to close the gripper (not available in simulation). |
| 272 | verbose: If true, the robot will print information about the grasp. |
| 273 | """ |
| 274 | |
| 275 | self.group.set_max_velocity_scaling_factor(0.2) |
| 276 | self.group.set_max_acceleration_scaling_factor(0.2) |
| 277 | self.group.set_goal_position_tolerance(0.001) |
| 278 | self.group.set_goal_orientation_tolerance(0.02) |
| 279 | self.group.set_pose_reference_frame(self.frame) |
| 280 | |
| 281 | pre_grasp_pose = get_pose_msg(position + Rotation.from_quat(orientation).as_matrix() @ (pre_grasp_approach*np.array([0,0,-1])), orientation) |
| 282 | waypoints = [pre_grasp_pose] |
| 283 | |
| 284 | self.group.set_pose_target(waypoints[0]) |
| 285 | |
| 286 | # (plan, fraction) = self.group.compute_cartesian_path(waypoints, 0.02, 0.0) |
| 287 | # if fraction < 1: |
| 288 | # print("Could not plan to pre-grasp pose. Plan accuracy", fraction) |
| 289 | # return False |
| 290 | |
| 291 | if verbose: |
| 292 | print("Moving to pre-grasp pose") |
| 293 | |
| 294 | # self._execute(self.group, plan) |
| 295 | plan = self._go(self.group) |
| 296 | self.group.stop() |
| 297 | self.group.clear_pose_targets() |
| 298 | if not plan: |
| 299 | print("Failed") |
| 300 | return False |
| 301 | |
| 302 | |
| 303 | if verbose: |
| 304 | print("Moved to pre grasp. Remmoving object") |
| 305 | |
| 306 | if object_id is not None and object_id in self.objects: |
| 307 | self.scene.remove_world_object(f"inst_{object_id}") |
| 308 | self.objects[object_id]["active"] = False |
| 309 | |
| 310 | waypoints = [get_pose_msg(position, orientation)] |
| 311 | # (plan, fraction) = self.group.compute_cartesian_path(waypoints, 0.003, 0.001, True) |
| 312 | # if fraction < 0.7: |
| 313 | # print("Could not plan to pre-grasp pose. Plan Accuracy", fraction) |
| 314 | # return False |
| 315 | |
| 316 | if verbose: |
| 317 | print("Moving to grasp pose") |
| 318 | |
| 319 | self.group.set_pose_target(waypoints[0]) |
| 320 | self.error_recovery_client.send_goal_and_wait(ErrorRecoveryActionGoal()) |
no test coverage detected