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