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 sample_placement(self, body, max_attempts=10):
with body.CreateKinBodyStateSaver():
for _ in irange(0, max_attempts):
quat = quat_from_axis_angle(0, 0, uniform(0, 2 * math.pi))
set_quat(body, quat)
aabb = body.ComputeAABB()
low = self.xy_min + aabb.extents()[:2]
high = self.xy_max - aabb.extents()[:2]
if np.any(low >= high):
continue
xy = (high - low) * np.random.rand(*low.shape) + low
z = self.z + aabb.extents()[2]
point = np.concatenate([xy, [z]
]) + (get_point(body) - aabb.pos())
pose = pose_from_quat_point(quat, point)
#set_pose(body, pose)
return pose
# TODO - collision here?
return None
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)