def __init__(self, radius): swap_xz = trans_from_quat( quat_from_angle_vector(math.pi / 2, [0, 1, 0])) translate = trans_from_point(0, 0, radius) self.origin_grasp = translate.dot(swap_xz) pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0.5, 0, -1])) self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
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_grasp_traj(pose, grasp): enable_all(all_bodies, False) body1.Enable(True) set_pose(body1, pose.value) manip_trans = manip_from_pose_grasp(pose.value, grasp.value) grasp_conf = solve_inverse_kinematics(manipulator, manip_trans) if grasp_conf is None: return if DISABLE_MOTIONS: yield [(Conf(grasp_conf), Traj([]))] return set_manipulator_conf(manipulator, grasp_conf) robot.Grab(body1) pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR)) pregrasp_conf = solve_inverse_kinematics(manipulator, pregrasp_trans) if pregrasp_conf is None: return if has_mp(): path = mp_straight_line(robot, grasp_conf, pregrasp_conf) else: path = linear_motion_plan(robot, pregrasp_conf) robot.Release(body1) if path is None: return grasp_traj = Traj(path) grasp_traj.pose = pose grasp_traj.grasp = grasp yield [(Conf(pregrasp_conf), grasp_traj)]
def side_grasps(box, under=True): # Convert to z then rotate around it? (w, l, h) = get_box_dimensions(box) origin = trans_from_point(0, 0, -h / 2) for j in range(1 + under): swap_xz = trans_from_axis_angle(0, -math.pi / 2 + j * math.pi, 0) for i in range(4): rotate_z = trans_from_axis_angle(0, 0, i * math.pi / 2) yield swap_xz.dot(rotate_z).dot(origin)
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 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 sample_edge_pose(surface, mesh): world_from_surface = trans_from_pose(surface.pose) radius = max(length(v[:2]) for v in mesh.vertices) origin_from_base = trans_from_point(0, 0, np.min(mesh.vertices[:, 2])) for point in sample_edge_point(surface.convex_hull, radius): theta = random.uniform(0, 2 * np.pi) surface_from_origin = trans_from_quat_point(quat_from_z_rot(theta), point) yield pose_from_trans( world_from_surface.dot(surface_from_origin).dot(origin_from_base))
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 sample_grasp_traj(obj, pose, grasp): enable_all(bodies, False) body = bodies[obj] body.Enable(True) set_pose(body, pose.value) set_manipulator_conf(manipulator, carry_config) manip_tform = manip_from_pose_grasp(pose.value, grasp.value) for base_tform in islice(random_inverse_reachability(ir_model, manip_tform), max_failures): set_trans(robot, base_tform) if env.CheckCollision(robot) or robot.CheckSelfCollision(): continue approach_robot_conf = robot.GetConfigurationValues() grasp_manip_conf = solve_inverse_kinematics( manipulator, manip_tform) if grasp_manip_conf is None: continue set_manipulator_conf(manipulator, grasp_manip_conf) grasp_robot_conf = robot.GetConfigurationValues() if DISABLE_MOTIONS: path = [approach_robot_conf, grasp_robot_conf, approach_robot_conf] yield [(Conf(approach_robot_conf), make_traj(path, obj, pose, grasp))] return robot.Grab(body) pregrasp_tform = manip_tform.dot( trans_from_point(*APPROACH_VECTOR)) pregrasp_conf = solve_inverse_kinematics( manipulator, pregrasp_tform) if pregrasp_conf is None: continue path = linear_motion_plan(robot, pregrasp_conf) robot.Release(body) if path is None: continue yield [(Conf(approach_robot_conf), make_traj(path, obj, pose, grasp))] return
def sample_grasp_traj( pose, grasp ): # Sample pregrasp config and motion plan that performs a grasp enable_all(all_bodies, False) body1.Enable(True) set_pose(body1, pose.value) manip_trans = manip_from_pose_grasp(pose.value, grasp.value) grasp_conf = solve_inverse_kinematics( manipulator, manip_trans) # Grasp configuration if grasp_conf is None: return if DISABLE_MOTIONS: yield [(Conf(grasp_conf), Traj([]))] return set_manipulator_conf(manipulator, grasp_conf) robot.Grab(body1) pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR)) pregrasp_conf = solve_inverse_kinematics( manipulator, pregrasp_trans) # Pre-grasp configuration if pregrasp_conf is None: return # Trajectory from grasp configuration to pregrasp if has_mp(): path = mp_straight_line(robot, grasp_conf, pregrasp_conf) else: path = linear_motion_plan(robot, pregrasp_conf) #grasp_traj = vector_traj_helper(env, robot, approach_vector) #grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if path is None: return grasp_traj = Traj(path) grasp_traj.pose = pose grasp_traj.grasp = grasp yield [(Conf(pregrasp_conf), grasp_traj)]
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 __init__(self, origin_grasp): self.origin_grasp = origin_grasp pregrasp_vector = PREGRASP_DISTANCE * normalize(np.array([0, 0, -1])) self.gripper_from_pregrasp = trans_from_point(*pregrasp_vector)
def sample_push_traj_fn(ir_model, bodies, surfaces, carry_arm_conf, max_failures=200): arm = ir_model.manip robot = ir_model.robot pregrasp_vector = 0.1 * normalize(np.array([1, 0, -1])) gripper_from_pregrasp = trans_from_point(*pregrasp_vector) # TODO: include pregrasp here 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 # TODO: I need to return individual pushes (not just noes that worked on the trajectory) return sample_grasp_traj