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