Exemplo n.º 1
0
def mirror_robot(robot1, node_points):
    # TODO: place robots side by side or diagonal across
    set_extrusion_camera(node_points, theta=-np.pi / 3)
    #draw_pose(Pose())
    centroid = np.average(node_points, axis=0)
    centroid_pose = Pose(point=centroid)
    #draw_pose(Pose(point=centroid))

    # print(centroid)
    scale = 0.  # 0.15
    vector = get_point(robot1) - centroid
    set_pose(robot1, Pose(point=Point(*+scale * vector[:2])))
    # Inner product of end-effector z with base->centroid or perpendicular to this line
    # Partition by sides

    robot2 = load_robot()
    set_pose(
        robot2,
        Pose(point=Point(*-(2 + scale) * vector[:2]), euler=Euler(yaw=np.pi)))

    # robots = [robot1]
    robots = [robot1, robot2]
    for robot in robots:
        set_configuration(robot, DUAL_CONF)
        # joint1 = get_movable_joints(robot)[0]
        # set_joint_position(robot, joint1, np.pi / 8)
        draw_pose(get_pose(robot), length=0.25)
    return robots
Exemplo n.º 2
0
def simulate_parallel(robots,
                      plan,
                      time_step=0.1,
                      speed_up=10.,
                      record=None):  # None | video.mp4
    # TODO: ensure the step size is appropriate
    makespan = compute_duration(plan)
    print('\nMakespan: {:.3f}'.format(makespan))
    trajectories = extract_parallel_trajectories(plan)
    if trajectories is None:
        return
    wait_if_gui('Begin?')
    num_motion = sum(action.name == 'move' for action in plan)
    with VideoSaver(record):
        for t in inclusive_range(0, makespan, time_step):
            # if action.start <= t <= get_end(action):
            executing = Counter(traj.robot for traj in trajectories
                                if traj.at(t) is not None)
            print('t={:.3f}/{:.3f} | executing={}'.format(
                t, makespan, executing))
            for robot in robots:
                num = executing.get(robot, 0)
                if 2 <= num:
                    raise RuntimeError(
                        'Robot {} simultaneously executing {} trajectories'.
                        format(robot, num))
                if (num_motion == 0) and (num == 0):
                    set_configuration(robot, DUAL_CONF)
            #step_simulation()
            wait_for_duration(time_step / speed_up)
    wait_if_gui('Finish?')
Exemplo n.º 3
0
def compute_door_paths(world,
                       joint_name,
                       door_conf1,
                       door_conf2,
                       obstacles=set(),
                       teleport=False):
    door_paths = []
    if door_conf1 == door_conf2:
        return door_paths
    door_joint = joint_from_name(world.kitchen, joint_name)
    door_joints = [door_joint]
    # TODO: could unify with grasp path
    door_extend_fn = get_extend_fn(world.kitchen,
                                   door_joints,
                                   resolutions=[DOOR_RESOLUTION])
    door_path = [door_conf1.values] + list(
        door_extend_fn(door_conf1.values, door_conf2.values))
    if teleport:
        door_path = [door_conf1.values, door_conf2.values]
    # TODO: open until collision for the drawers

    sign = world.get_door_sign(door_joint)
    pull = (sign * door_path[0][0] < sign * door_path[-1][0])
    # door_obstacles = get_descendant_obstacles(world.kitchen, door_joint)
    for handle_grasp in get_handle_grasps(world, door_joint, pull=pull):
        link, grasp, pregrasp = handle_grasp
        handle_path = []
        for door_conf in door_path:
            set_joint_positions(world.kitchen, door_joints, door_conf)
            # if any(pairwise_collision(door_obst, obst)
            #       for door_obst, obst in product(door_obstacles, obstacles)):
            #    return
            handle_path.append(get_link_pose(world.kitchen, link))
            # Collide due to adjacency

        # TODO: check pregrasp path as well
        # TODO: check gripper self-collisions with the robot
        set_configuration(world.gripper, world.open_gq.values)
        tool_path = [
            multiply(handle_pose, invert(grasp)) for handle_pose in handle_path
        ]
        for i, tool_pose in enumerate(tool_path):
            set_joint_positions(world.kitchen, door_joints, door_path[i])
            set_tool_pose(world, tool_pose)
            # handles = draw_pose(handle_path[i], length=0.25)
            # handles.extend(draw_aabb(get_aabb(world.kitchen, link=link)))
            # wait_for_user()
            # for handle in handles:
            #    remove_debug(handle)
            if any(
                    pairwise_collision(world.gripper, obst)
                    for obst in obstacles):
                break
        else:
            door_paths.append(
                DoorPath(door_path, handle_path, handle_grasp, tool_path))
    return door_paths
Exemplo n.º 4
0
def load_robot():
    root_directory = os.path.dirname(os.path.abspath(__file__))
    kuka_path = os.path.join(root_directory, KUKA_DIR, KUKA_PATH)
    with HideOutput():
        robot = load_pybullet(kuka_path, fixed_base=True)
        #print([get_max_velocity(robot, joint) for joint in get_movable_joints(robot)])
        set_static(robot)
        set_configuration(robot, INITIAL_CONF)
    return robot
Exemplo n.º 5
0
    def test(name1, command1, name2, command2):
        robot1, robot2 = index_from_name(robots, name1), index_from_name(
            robots, name2)
        if (robot1 == robot2) or not collisions:
            return False
        # TODO: check collisions between pairs of inflated adjacent element
        for traj1, traj2 in randomize(
                product(command1.trajectories, command2.trajectories)):
            # TODO: use swept aabbs for element checks
            aabbs1, aabbs2 = traj1.get_aabbs(), traj2.get_aabbs()
            swept_aabbs1 = {
                link: aabb_union(link_aabbs[link] for link_aabbs in aabbs1)
                for link in aabbs1[0]
            }
            swept_aabbs2 = {
                link: aabb_union(link_aabbs[link] for link_aabbs in aabbs2)
                for link in aabbs2[0]
            }
            swept_overlap = [
                (link1, link2)
                for link1, link2 in product(swept_aabbs1, swept_aabbs2)
                if aabb_overlap(swept_aabbs1[link1], swept_aabbs2[link2])
            ]
            if not swept_overlap:
                continue
            # for l1 in set(map(itemgetter(0), swept_overlap)):
            #     draw_aabb(swept_aabbs1[l1], color=RED)
            # for l2 in set(map(itemgetter(1), swept_overlap)):
            #     draw_aabb(swept_aabbs2[l2], color=BLUE)

            for index1, index2 in product(randomize(range(len(traj1.path))),
                                          randomize(range(len(traj2.path)))):
                overlap = [(link1, link2)
                           for link1, link2 in swept_overlap if aabb_overlap(
                               aabbs1[index1][link1], aabbs2[index2][link2])]
                #overlap = list(product(aabbs1[index1], aabbs2[index2]))
                if not overlap:
                    continue
                set_configuration(robot1, traj1.path[index1])
                set_configuration(robot2, traj2.path[index2])
                #wait_if_gui()
                #if pairwise_collision(robot1, robot2):
                #    return True
                for link1, link2 in overlap:
                    if pairwise_link_collision(robot1, link1, robot2, link2):
                        #wait_if_gui()
                        return True
        return False
Exemplo n.º 6
0
def simulate_parallel(robots,
                      plan,
                      time_step=0.1,
                      speed_up=10.,
                      record=None):  # None | video.mp4
    # TODO: ensure the step size is appropriate
    makespan = compute_duration(plan)
    print('\nMakespan: {:.3f}'.format(makespan))
    if plan is None:
        return
    trajectories = []
    for action in plan:
        command = action.args[-1]
        if (action.name == 'move') and (command.start_conf is
                                        action.args[-2].positions):
            command = command.reverse()
        command.retime(start_time=action.start)
        #print(action)
        #print(action.start, get_end(action), action.duration)
        #print(command.start_time, command.end_time, command.duration)
        #for traj in command.trajectories:
        #    print(traj, traj.start_time, traj.end_time, traj.duration)
        trajectories.extend(command.trajectories)
    #print(sum(traj.duration for traj in trajectories))
    num_motion = sum(action.name == 'move' for action in plan)

    wait_if_gui('Begin?')
    with VideoSaver(record):
        for t in inclusive_range(0, makespan, time_step):
            # if action.start <= t <= get_end(action):
            executing = Counter(traj.robot for traj in trajectories
                                if traj.at(t) is not None)
            print('t={:.3f}/{:.3f} | executing={}'.format(
                t, makespan, executing))
            for robot in robots:
                num = executing.get(robot, 0)
                if 2 <= num:
                    raise RuntimeError(
                        'Robot {} simultaneously executing {} trajectories'.
                        format(robot, num))
                if (num_motion == 0) and (num == 0):
                    set_configuration(robot, DUAL_CONF)
            #step_simulation()
            wait_for_duration(time_step / speed_up)
    wait_if_gui('Finish?')
Exemplo n.º 7
0
 def test(j1, a1, a2, o2, wp):
     if not collisions or (o2 in j1):  # (j1 == JOINT_TEMPLATE.format(o2)):
         return True
     # TODO: check pregrasp path as well
     # TODO: pull path collisions
     wp.assign()
     set_configuration(world.gripper, world.open_gq.values)
     status = compute_door_paths(world,
                                 j1,
                                 a1,
                                 a2,
                                 obstacles=get_link_obstacles(world, o2))
     #print(j1, a1, a2, o2, wp)
     #if not status:
     #    set_renderer(enable=True)
     #    for a in [a1, a2]:
     #        a.assign()
     #        wait_for_user()
     #    set_renderer(enable=False)
     return status
Exemplo n.º 8
0
def main(use_turtlebot=True):
    parser = argparse.ArgumentParser()
    parser.add_argument('-sim', action='store_true')
    parser.add_argument('-video', action='store_true')
    args = parser.parse_args()
    video = 'video.mp4' if args.video else None

    connect(use_gui=True, mp4=video)
    #set_renderer(enable=False)
    # print(list_pybullet_data())
    # print(list_pybullet_robots())

    draw_global_system()
    set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
                    target_point=Point(-1.5, +1.5, 0))

    plane = load_plane()
    #door = load_pybullet('models/door.urdf', fixed_base=True) # From drake
    #set_point(door, Point(z=-.1))
    door = create_door()
    #set_position(door, z=base_aligned_z(door))
    set_point(door, base_aligned(door))
    #set_collision_margin(door, link=0, margin=0.)
    set_configuration(door, [math.radians(-5)])
    dump_body(door)

    door_joint = get_movable_joints(door)[0]
    door_link = child_link_from_joint(door_joint)
    #draw_pose(get_com_pose(door, door_link), parent=door, parent_link=door_link)
    draw_pose(Pose(), parent=door, parent_link=door_link)
    wait_if_gui()

    ##########

    start_x = +2
    target_x = -start_x
    if not use_turtlebot:
        side = 0.25
        robot = create_box(w=side, l=side, h=side, mass=5., color=BLUE)
        set_position(robot, x=start_x)
        #set_velocity(robot, linear=Point(x=-1))
    else:
        turtlebot_urdf = os.path.abspath(TURTLEBOT_URDF)
        print(turtlebot_urdf)
        #print(read(turtlebot_urdf))
        robot = load_pybullet(turtlebot_urdf, merge=True, fixed_base=True)
        robot_joints = get_movable_joints(robot)[:3]
        set_joint_positions(robot, robot_joints, [start_x, 0, PI])
    set_all_color(robot, BLUE)
    set_position(robot, z=base_aligned_z(robot))
    dump_body(robot)

    ##########

    set_renderer(enable=True)
    #test_door(door)
    if args.sim:
        test_simulation(robot, target_x, video)
    else:
        assert use_turtlebot  # TODO: extend to the block
        test_kinematic(robot, door, target_x)
    disconnect()
Exemplo n.º 9
0
 def assign(self):
     set_configuration(self.robot, self.positions)
Exemplo n.º 10
0
    def fn(printed, directed, position, conf):
        # Queue minimizes the statistic
        element = get_undirected(all_elements, directed)
        n1, n2 = directed

        # forward adds, backward removes
        structure = printed | {element} if forward else printed - {element}
        structure_ids = get_extructed_ids(element_from_id, structure)

        normalizer = 1
        #normalizer = len(structure)
        #normalizer = compute_element_distance(node_points, all_elements)

        reduce_op = sum # sum | max | average
        reaction_fn = force_from_reaction  # force_from_reaction | torque_from_reaction

        first_node, second_node = directed if forward else reverse_element(directed)
        layer = sign * layer_from_edge[element]
        #layer = sign * layer_from_directed.get(directed, INF)

        tool_point = position
        tool_distance = 0.
        if heuristic in COST_HEURISTICS:
            if conf is not None:
                if hash_or_id(conf) not in ee_cache:
                    with BodySaver(robot):
                        set_configuration(robot, conf)
                        ee_cache[hash_or_id(conf)] = get_tool_position(robot)
                tool_point = ee_cache[hash_or_id(conf)]
            tool_distance = get_distance(tool_point, node_points[first_node])

        # TODO: weighted average to balance cost and bias
        if heuristic == 'none':
            return 0
        if heuristic == 'random':
            return random.random()
        elif heuristic == 'degree':
            # TODO: other graph statistics
            #printed_nodes = {n for e in printed for n in e}
            #node = n1 if n2 in printed_nodes else n2
            #if node in ground_nodes:
            #    return 0
            raise NotImplementedError()
        elif heuristic == 'length':
            # Equivalent to mass if uniform density
            return get_element_length(element, node_points)
        elif heuristic == 'distance':
            return tool_distance
        elif heuristic == 'layered-distance':
            return (layer, tool_distance)
        # elif heuristic == 'components':
        #     # Ground nodes intentionally omitted
        #     # TODO: this is broken
        #     remaining = all_elements - printed if forward else printed - {element}
        #     vertices = nodes_from_elements(remaining)
        #     components = get_connected_components(vertices, remaining)
        #     #print('Components: {} | Distance: {:.3f}'.format(len(components), tool_distance))
        #     return (len(components), tool_distance)
        elif heuristic == 'fixed-tsp':
            # TODO: layer_from_edge[element]
            # TODO: score based on current distance from the plan in the tour
            # TODO: factor in the distance to the next element in a more effective way
            if order is None:
                return (INF, tool_distance)
            return (sign*order[element], tool_distance) # Chooses least expensive direction
        elif heuristic == 'tsp':
            if printed not in tsp_cache: # not last_plan and
                # TODO: seed with the previous solution
                #remaining = all_elements - printed if forward else printed
                #assert element in remaining
                #printed_nodes = compute_printed_nodes(ground_nodes, printed) if forward else ground_nodes
                tsp_cache[printed] = solve_tsp(all_elements, ground_nodes, node_points, printed, tool_point, initial_point,
                                               bidirectional=True, layers=True, max_time=30, visualize=False, verbose=True)
                #print(tsp_cache[printed])
                if not last_plan:
                    last_plan[:] = tsp_cache[printed][0]
            plan, cost = tsp_cache[printed]
            #plan = last_plan[len(printed):]
            if plan is None:
                #return tool_distance
                return (layer, INF)
            transit = compute_transit_distance(node_points, plan, start=tool_point, end=initial_point)
            assert forward
            first = plan[0] == directed
            #return not first # No info if the best isn't possible
            index = None
            for i, directed2 in enumerate(plan):
                undirected2 = get_undirected(all_elements, directed2)
                if element == undirected2:
                    assert index is None
                    index = i
            assert index is not None
            # Could also break ties by other in the plan
            # Two plans might have the same cost but the swap might be detrimental
            new_plan = [directed] + plan[:index] + plan[index+1:]
            assert len(plan) == len(new_plan)
            new_transit = compute_transit_distance(node_points, new_plan, start=tool_point, end=initial_point)
            #print(layer, cost, transit + compute_element_distance(node_points, plan),
            #      new_transit + compute_element_distance(node_points, plan))
            #return new_transit
            return (layer, not first, new_transit) # Layer important otherwise it shortcuts
        elif heuristic == 'online-tsp':
            if forward:
                _, tsp_distance = solve_tsp(all_elements-structure, ground_nodes, node_points, printed,
                                            node_points[second_node], initial_point, visualize=False)
            else:
                _, tsp_distance = solve_tsp(structure, ground_nodes, node_points, printed, initial_point,
                                            node_points[second_node], visualize=False)
            total = tool_distance + tsp_distance
            return total
        # elif heuristic == 'mst':
        #     # TODO: this is broken
        #     mst_distance = compute_component_mst(node_points, ground_nodes, remaining,
        #                                          initial_position=node_points[second_node])
        #     return tool_distance + mst_distance
        elif heuristic == 'x':
            return sign * get_midpoint(node_points, element)[0]
        elif heuristic == 'z':
            return sign * compute_z_distance(node_points, element)
        elif heuristic == 'pitch':
            #delta = node_points[second_node] - node_points[first_node]
            delta = node_points[n2] - node_points[n1]
            return get_pitch(delta)
        elif heuristic == 'dijkstra': # offline
            # TODO: sum of all element path distances
            return sign*np.average([distance_from_node[node].cost for node in element]) # min, max, average
        elif heuristic == 'online-dijkstra':
            if printed not in distance_cache:
                distance_cache[printed] = compute_distance_from_node(printed, node_points, ground_nodes)
            return sign*min(distance_cache[printed][node].cost
                            if node in distance_cache[printed] else INF
                            for node in element)
        elif heuristic == 'plan-stiffness':
            if order is None:
                return None
            return (sign*order[element], directed not in order)
        elif heuristic == 'load':
            nodal_loads = checker.get_nodal_loads(existing_ids=structure_ids, dof_flattened=False) # get_self_weight_loads
            return reduce_op(np.linalg.norm(force_from_reaction(reaction)) for reaction in nodal_loads.values())
        elif heuristic == 'fixed-forces':
            #printed = all_elements # disable to use most up-to-date
            # TODO: relative to the load introduced
            if printed not in reaction_cache:
                reaction_cache[printed] = compute_all_reactions(extrusion_path, all_elements, checker=checker)
            force = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reaction in reaction_cache[printed].reactions[element])
            return force / normalizer
        elif heuristic == 'forces':
            reactions_from_nodes = compute_node_reactions(extrusion_path, structure, checker=checker)
            #torque = sum(np.linalg.norm(np.sum([torque_from_reaction(reaction) for reaction in reactions], axis=0))
            #            for reactions in reactions_from_nodes.values())
            #return torque / normalizer
            total = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reactions in reactions_from_nodes.values()
                            for reaction in reactions)
            return total / normalizer
            #return max(sum(np.linalg.norm(reaction_fn(reaction)) for reaction in reactions)
            #               for reactions in reactions_from_nodes.values())
        elif heuristic == 'stiffness':
            # TODO: add different variations
            # TODO: normalize by initial stiffness, length, or degree
            # Most unstable or least unstable first
            # Gets faster with fewer all_elements
            #old_stiffness = score_stiffness(extrusion_path, element_from_id, printed, checker=checker)
            stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better
            return stiffness / normalizer
            #return stiffness / old_stiffness
        elif heuristic == 'fixed-stiffness':
            # TODO: invert the sign for regression/progression?
            # TODO: sort FastDownward by the (fixed) action cost
            return stiffness_cache[element] / normalizer
        elif heuristic == 'relative-stiffness':
            stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better
            if normalizer == 0:
                return 0
            return stiffness / normalizer
            #return stiffness / stiffness_cache[element]
        raise ValueError(heuristic)
Exemplo n.º 11
0
 def set_initial(self):
     with ClientSaver(self.client):
         set_configuration(self.robot, self.initial_config)
         for name, pose in self.initial_poses.items():
             set_pose(self.get_body(name), pose)