def __init__(self, radius):
swap_xz = trans_from_quat(
quat_from_angle_vector(math.pi / 2, [0, 1, 0]))
swap_xy = trans_from_quat(
quat_from_angle_vector(math.pi / 2, [0, 0, 1]))
translate = trans_from_point(0, 0, radius)
self.origin_grasp = translate.dot(swap_xy).dot(swap_xz)
pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0, 0, -1]))
self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
def get_top_grasps(mesh, under=False):
w, l, h = np.max(mesh.vertices, axis=0) - \
np.min(mesh.vertices, axis=0)
reflect_z = trans_from_quat(quat_from_angle_vector(math.pi, [0, 1, 0]))
translate = trans_from_point(0, 0, h / 2 - GRASP_LENGTH)
if w < MAX_GRASP_WIDTH:
for i in range(1 + under):
rotate_z = trans_from_quat(
quat_from_angle_vector(math.pi / 2 + i * math.pi, [0, 0, 1]))
yield translate.dot(rotate_z).dot(reflect_z), np.array([w])
if l < MAX_GRASP_WIDTH:
for i in range(1 + under):
rotate_z = trans_from_quat(
quat_from_angle_vector(i * math.pi, [0, 0, 1]))
yield translate.dot(rotate_z).dot(reflect_z), np.array([l])
def top_grasps(box): # Rotate around z axis
# returns gripper_from_obj
(w, l, h) = get_box_dimensions(box)
origin = trans_from_point(0, 0, -h)
reflect = trans_from_quat(quat_from_axis_angle(0, -math.pi, 0))
for i in range(4):
rotate_z = trans_from_axis_angle(0, 0, i * math.pi / 2)
yield reflect.dot(origin).dot(rotate_z)
def top_grasps(box):
(w, l, h) = get_box_dimensions(box)
origin = trans_from_point(0, 0, -h)
bottom = trans_from_point(0, 0, -h)
reflect = trans_from_quat(quat_from_axis_angle(0, -math.pi, 0))
for i in range(4):
rotate_z = trans_from_axis_angle(0, 0, i * math.pi / 2)
yield reflect.dot(origin).dot(bottom).dot(rotate_z)
def get_prepush_setting(mesh, under=False):
######## write
w, l, h = np.max(mesh.vertices, axis=0) - \
np.min(mesh.vertices, axis=0)
for j in range(1 + under):
swap_xz = trans_from_quat(
quat_from_angle_vector(math.pi / 2 + j * math.pi, [0, 1, 0]))
# if w < MAX_GRASP_WIDTH:
# translate = trans_from_point(0, 0, -2*l)
# for i in range(2):
# rotate_z = trans_from_quat(quat_from_angle_vector(math.pi / 2 + i * math.pi, [1, 0, 0]))
# yield translate.dot(rotate_z).dot(swap_xz), np.array([w])
if l < MAX_GRASP_WIDTH:
translate = trans_from_point(0, 0, l + GRASP_LENGTH)
for i in range(2):
rotate_z = trans_from_quat(
quat_from_angle_vector(i * math.pi, [1, 0, 0]))
yield translate.dot(rotate_z).dot(swap_xz), np.array([l])
def get_side_grasps(mesh, under=False):
w, l, h = np.max(mesh.vertices, axis=0) - \
np.min(mesh.vertices, axis=0)
for j in range(1 + under):
swap_xz = trans_from_quat(
quat_from_angle_vector(math.pi / 2 + j * math.pi, [0, 1, 0]))
if w < MAX_GRASP_WIDTH:
translate = trans_from_point(0, 0, l / 2 - GRASP_LENGTH)
for i in range(2):
rotate_z = trans_from_quat(
quat_from_angle_vector(math.pi / 2 + i * math.pi,
[1, 0, 0]))
yield translate.dot(rotate_z).dot(swap_xz), np.array([w])
if l < MAX_GRASP_WIDTH:
translate = trans_from_point(0, 0, w / 2 - GRASP_LENGTH)
for i in range(2):
rotate_z = trans_from_quat(
quat_from_angle_vector(i * math.pi, [1, 0, 0]))
yield translate.dot(rotate_z).dot(swap_xz), np.array([l])
def cylinder_contact(radius, height, base_offset=0.01): # base_offset=0.01
swap_xz = trans_from_quat(quat_from_angle_vector(-math.pi, [0, 1, 0]))
translate = trans_from_point(-radius, 0, -height / 2 + base_offset)
return translate.dot(swap_xz)
def __init__(self, height):
bottom = trans_from_point(0, 0, -height / 2)
reflect = trans_from_quat(quat_from_axis_angle(0, -math.pi, 0))
self.origin_grasp = reflect.dot(bottom)
pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0, 0, -1]))
self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
def sample(self):
rotation = quat_from_angle_vector(random.uniform(0, 2 * np.pi),
[0, 0, 1])
gripper_from_obj = self.origin_grasp.dot(trans_from_quat(rotation))
return gripper_from_obj, self.gripper_from_pregrasp