def evaluate_grasp_point(sim, pos, normal, num_rotations=6): # define initial grasp frame on object surface z_axis = -normal x_axis = np.r_[1.0, 0.0, 0.0] if np.isclose(np.abs(np.dot(x_axis, z_axis)), 1.0, 1e-4): x_axis = np.r_[0.0, 1.0, 0.0] y_axis = np.cross(z_axis, x_axis) x_axis = np.cross(y_axis, z_axis) R = Rotation.from_matrix(np.vstack((x_axis, y_axis, z_axis)).T) # try to grasp with different yaw angles yaws = np.linspace(0.0, np.pi, num_rotations) outcomes, widths = [], [] for yaw in yaws: ori = R * Rotation.from_euler("z", yaw) sim.restore_state() candidate = Grasp(Transform(ori, pos), width=sim.gripper.max_opening_width) outcome, width = sim.execute_grasp(candidate, remove=False) outcomes.append(outcome) widths.append(width) # detect mid-point of widest peak of successful yaw angles # TODO currently this does not properly handle periodicity successes = (np.asarray(outcomes) == Label.SUCCESS).astype(float) if np.sum(successes): peaks, properties = signal.find_peaks( x=np.r_[0, successes, 0], height=1, width=1 ) idx_of_widest_peak = peaks[np.argmax(properties["widths"])] - 1 ori = R * Rotation.from_euler("z", yaws[idx_of_widest_peak]) width = widths[idx_of_widest_peak] return Grasp(Transform(ori, pos), width), int(np.max(outcomes))
def select_grasp(self, grasps, scores): # select the highest grasp heights = np.empty(len(grasps)) for i, grasp in enumerate(grasps): heights[i] = grasp.pose.translation[2] idx = np.argmax(heights) grasp, score = grasps[idx], scores[idx] # make sure camera is pointing forward rot = grasp.pose.rotation axis = rot.as_matrix()[:, 0] if axis[0] < 0: grasp.pose.rotation = rot * Rotation.from_euler("z", np.pi) return grasp, score