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 to_grasp_list(self, grasp_configs): grasps, scores = [], [] for grasp_config in grasp_configs.grasps: # orientation x_axis = ros_utils.from_vector3_msg(grasp_config.axis) y_axis = -ros_utils.from_vector3_msg(grasp_config.binormal) z_axis = ros_utils.from_vector3_msg(grasp_config.approach) orientation = Rotation.from_matrix(np.vstack([x_axis, y_axis, z_axis]).T) # position position = ros_utils.from_point_msg(grasp_config.position) # width width = grasp_config.width.data # score score = grasp_config.score.data if score < 0.0: continue # negative score is larger than positive score (https://github.com/atenpas/gpd/issues/32#issuecomment-387846534) grasps.append(Grasp(Transform(orientation, position), width)) scores.append(score) return grasps, scores