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