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 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 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
def sample_grasp_traj(obj, pose, pose2):
enable_all(bodies, False)
body = bodies[obj]
#body.Enable(True) # TODO: fix this
set_pose(body, pose.value)
# TODO: check if on the same surface
point = point_from_pose(pose.value)
point2 = point_from_pose(pose2.value)
distance = length(point2 - point)
if (distance <= 0.0) or (0.6 <= distance):
return
direction = normalize(point2 - point)
orientation = quat_from_angle_vector(angle_from_vector(direction),
[0, 0, 1])
#rotate_direction = trans_from_quat_point(orientation, unit_point())
steps = int(math.ceil(distance / PUSH_MAX_DISTANCE) + 1)
distances = np.linspace(0., distance, steps)
points = [point + d * direction for d in distances]
poses = [pose_from_quat_point(orientation, point) for point in points]
pose_objects = [pose] + map(Pose, poses[1:-1]) + [pose2]
print distance, steps, distances, len(poses)
radius, height = get_mesh_radius(body), get_mesh_height(body)
contact = cylinder_contact(radius, height)
pushes = []
# TODO: I could do all trajectories after all pushes planned
for i in xrange(len(poses) - 1):
p1, p2 = poses[i:i + 2]
# TODO: choose midpoint for base
ir_world_from_gripper = manip_from_pose_grasp(p1, contact)
set_manipulator_conf(arm, carry_arm_conf)
for world_from_base in islice(
random_inverse_reachability(ir_model,
ir_world_from_gripper),
max_failures):
set_trans(robot, world_from_base)
set_manipulator_conf(arm, carry_arm_conf)
if check_collision(robot):
continue
q = Conf(robot.GetConfigurationValues())
push_arm_confs = []
approach_paths = []
for p in (p1, p2):
# TODO: make sure I have the +x push conf
world_from_gripper = manip_from_pose_grasp(p, contact)
set_manipulator_conf(arm, carry_arm_conf)
grasp_arm_conf = solve_inverse_kinematics(
arm, world_from_gripper, collisions=False)
if grasp_arm_conf is None:
break
push_arm_confs.append(grasp_arm_conf)
set_manipulator_conf(arm, grasp_arm_conf)
pregrasp_arm_conf = solve_inverse_kinematics(
arm,
world_from_gripper.dot(gripper_from_pregrasp),
collisions=False)
if pregrasp_arm_conf is None:
break
#if DISABLE_MOTIONS:
if True:
approach_paths.append([
carry_arm_conf, pregrasp_arm_conf, grasp_arm_conf
])
continue
"""
set_manipulator_conf(arm, pregrasp_arm_conf)
grasp_path = plan_straight_path(robot, grasp_arm_conf, pregrasp_arm_conf)
# robot.Release(body)
if grasp_path is None:
continue
pregrasp_path = plan_path(base_manip, pregrasp_arm_conf, carry_arm_conf)
if pregrasp_path is None:
continue
t = Prehensile(full_from_active(robot, grasp_path + pregrasp_path[1:]), obj, pose, grasp)
yield [(q, t)]
reset_env()
return
"""
else:
# Start and end may have different orientations
pq1, pq2 = push_arm_confs
push_path = plan_straight_path(robot, pq1, pq2)
# TODO: make sure the straight path is actually straight
if push_path is None:
continue
po1, po2 = pose_objects[i:i + 2]
ap1, ap2 = approach_paths
m = Push(
full_from_active(robot, ap1),
full_from_active(robot, push_arm_confs),
#full_from_active(robot, push_path),
full_from_active(robot, ap2[::-1]),
obj,
po1,
po2,
contact)
pushes.append(PushMotion(obj, po1, po2, q, m))
break
else:
print 'Failure', len(pushes)
return
print 'Success', len(pushes)
yield pushes