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
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?')
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
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
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
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?')
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
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()
def assign(self):
set_configuration(self.robot, self.positions)
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)
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)