def main():
connect(use_gui=True)
disable_real_time()
set_default_camera()
problem = pick_problem(arm='left', grasp_type='side')
grasp_gen = get_grasp_gen(problem, collisions=False)
ik_ir_fn = get_ik_ir_gen(problem, max_attempts=100, teleport=False)
pose = Pose(problem.movable[0], support=problem.surfaces[0])
base_conf = Conf(problem.robot, get_group_joints(problem.robot, 'base'))
ik_fn = get_ik_fn(problem)
found = False
saved_world = WorldSaver()
for grasp, in grasp_gen(problem.movable[0]):
print(grasp.value)
# confs_cmds = ik_ir_fn(problem.arms[0], problem.movable[0], pose, grasp)
# for conf, cmds in confs_cmds:
# found = True
cmds = ik_fn(problem.arms[0], problem.movable[0], pose, grasp,
base_conf)
if cmds is not None:
cmds = cmds[0]
found = True
if found:
break
if not found:
raise Exception('No grasp found')
saved_world.restore()
for cmd in cmds.commands:
print(cmd)
control_commands(cmds.commands)
print('Quit?')
wait_for_user()
disconnect()
def main(display='control'): # control | execute | step
connect(use_gui=True)
disable_real_time()
# KUKA_IIWA_URDF | DRAKE_IIWA_URDF
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True)
# floor = load_model('models/short_floor.urdf')
floor = p.loadURDF("plane.urdf")
block = load_model(
"models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera()
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_user()
disconnect()
def plan(robot, block, fixed, teleport):
grasp_gen = get_grasp_gen(robot, 'top', TOOL_FRAME)
ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION)
free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport, self_collisions=ENABLE_SELF_COLLISION)
holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION)
movable_joints = get_track_arm_joints(robot) if USE_IKFAST else get_movable_joints(robot)
pose0 = BodyPose(block)
conf0 = BodyConf(robot, joints=movable_joints)
saved_world = WorldSaver()
for grasp, in grasp_gen(block):
saved_world.restore()
result1 = ik_fn(block, pose0, grasp)
if result1 is None:
continue
conf1, path2 = result1
pose0.assign()
result2 = free_motion_fn(conf0, conf1)
if result2 is None:
continue
path1, = result2
result3 = holding_motion_fn(conf1, conf0, block, grasp)
if result3 is None:
continue
path3, = result3
return Command(path1.body_paths + path2.body_paths + path3.body_paths)
return None
def plan(robot, block, fixed, teleport):
grasp_gen = get_grasp_gen(robot, 'top')
ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport)
free_motion_fn = get_free_motion_gen(robot,
fixed=([block] + fixed),
teleport=teleport)
holding_motion_fn = get_holding_motion_gen(robot,
fixed=fixed,
teleport=teleport)
pose0 = BodyPose(block)
conf0 = BodyConf(robot)
saved_world = WorldSaver()
for grasp, in grasp_gen(block):
saved_world.restore()
result1 = ik_fn(block, pose0, grasp)
if result1 is None:
continue
conf1, path2 = result1
pose0.assign()
result2 = free_motion_fn(conf0, conf1)
if result2 is None:
continue
path1, = result2
result3 = holding_motion_fn(conf1, conf0, block, grasp)
if result3 is None:
continue
path3, = result3
return Command(path1.body_paths + path2.body_paths + path3.body_paths)
return None
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
draw_global_system()
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera(distance=2)
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
wait_if_gui('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_if_gui()
disconnect()
def main(display='execute'): # control | execute | step
# One of the arm's gantry carriage is fixed when the other is moving.
connect(use_gui=True)
set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0))
draw_pose(unit_pose(), length=1.0)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True)
# floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
#link = link_from_name(robot, 'gantry_base_link')
#print(get_aabb(robot, link))
block_x = -0.2
#block_y = 1 if ARM == 'right' else 13.5
#block_x = 10
block_y = 5.
# set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3)))
# set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor))))
set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5)))
# set_default_camera()
dump_world()
#print(get_camera())
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor],
if (command is None) or (display is None):
print('Unable to find a plan! Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
print('{}?'.format(display))
wait_for_interrupt()
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
def create_problems(args):
task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS}
problems = []
for num in range(N_TRIALS):
for task_name in TASK_NAMES:
print('Trial: {} / {} | Task: {}'.format(num, N_TRIALS, task_name))
random.seed(hash((0, task_name, num, time.time())))
numpy.random.seed(
wrap_numpy_seed(hash((1, task_name, num, time.time()))))
world = World(use_gui=False) # SERIAL
task_fn = task_fn_from_name[task_name]
task = task_fn(world, fixed=args.fixed)
task.world = None
if not SERIALIZE_TASK:
task = task_name
saver = WorldSaver()
problems.append({
'task': task,
'trial': num,
'saver': saver,
#'seeds': [get_random_seed(), get_numpy_seed()],
#'seeds': [random.getstate(), numpy.random.get_state()],
})
#print(world.body_from_name) # TODO: does not remain the same
#wait_for_user()
#world.reset()
#if has_gui():
# wait_for_user()
world.destroy()
return problems
def sample_state(self, **kwargs):
pose_from_name = self.sample(**kwargs)
world_saver = WorldSaver()
attachments = []
for pose in pose_from_name.values():
attachments.extend(pose.confs)
if self.grasped is not None:
attachments.append(self.grasped.get_attachment())
return State(self.world, savers=[world_saver], attachments=attachments)
def draw(self, **kwargs):
with LockRenderer(True):
remove_handles(self.handles)
self.handles = []
with WorldSaver():
for name, pose_dist in self.pose_dists.items():
self.handles.extend(
pose_dist.draw(color=self.color_from_name[name],
**kwargs))
def create_observable_belief(world, **kwargs):
with WorldSaver():
belief = Belief(world,
pose_dists={
name: create_observable_pose_dist(world, name)
for name in world.movable
},
**kwargs)
belief.task = world.task
return belief
def _update_initial(self):
# TODO: store initial poses as well?
self.initial_saver = WorldSaver()
self.goal_bq = FConf(self.robot, self.base_joints)
self.goal_aq = FConf(self.robot, self.arm_joints)
if are_confs_close(self.goal_aq, self.carry_conf):
self.goal_aq = self.carry_conf
self.goal_gq = FConf(self.robot, self.gripper_joints)
self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq]
set_all_static()
def create_surface_belief(world, surface_dists, **kwargs):
with WorldSaver():
belief = Belief(world,
pose_dists={
name:
create_surface_pose_dist(world, name, surface_dist)
for name, surface_dist in surface_dists.items()
},
**kwargs)
belief.task = world.task
return belief
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
floor_x = 2
set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
# set_default_camera()
dump_world()
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
print('Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
def create_state(world):
# TODO: support initially holding
# TODO: would be better to explicitly keep the state around
# world.initial_saver.restore()
world_saver = WorldSaver()
attachments = []
for obj_name in world.movable:
surface_name = world.get_supporting(obj_name)
if surface_name is not None:
attachments.append(
create_surface_attachment(world, obj_name, surface_name))
return State(world, savers=[world_saver], attachments=attachments)
def step_plan(world, plan, attachments={}, **kwargs):
if plan is None:
return False
attachments = dict(attachments)
with ClientSaver(world.perception.client):
with WorldSaver():
for name, args in plan:
if name in ['cook']:
continue
control = args[-1]
for command in control['commands']:
step_command(world, command, attachments, **kwargs)
wait_for_user('Finished!')
return True
def main():
parser = argparse.ArgumentParser()
# choreo_brick_demo | choreo_eth-trees_demo
parser.add_argument('-p',
'--problem',
default='choreo_brick_demo',
help='The name of the problem to solve')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions with obstacles')
parser.add_argument('-t',
'--teleport',
action='store_true',
help='Teleports instead of computing motions')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
robot, brick_from_index, obstacle_from_name = load_pick_and_place(
args.problem)
np.set_printoptions(precision=2)
pr = cProfile.Profile()
pr.enable()
with WorldSaver():
pddlstream_problem = get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=not args.cfree,
teleport=args.teleport)
with LockRenderer():
solution = solve_focused(pddlstream_problem,
planner='ff-wastar1',
max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
print_solution(solution)
plan, _, _ = solution
if plan is None:
return
if not has_gui():
connect(use_gui=True)
_ = load_pick_and_place(args.problem)
step_plan(plan, time_step=(np.inf if args.teleport else 0.01))
def plan(robot, block, fixed, teleport):
grasp_gen = get_grasp_gen(robot, 'bottom', get_tool_frame(ARM)) #'top'
ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION)
free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport,
self_collisions=ENABLE_SELF_COLLISION)
holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport,
self_collisions=ENABLE_SELF_COLLISION)
#arm_joints = get_torso_arm_joints(robot, ARM)
arm_joints = joints_from_names(robot, get_arm_joint_names(ARM))
pose0 = BodyPose(block)
conf0 = BodyConf(robot, joints=arm_joints)
saved_world = WorldSaver()
for grasp, in grasp_gen(block):
saved_world.restore()
result1 = ik_fn(block, pose0, grasp)
if result1 is None:
print('ik fn fails!')
continue
conf1, path2 = result1
pose0.assign()
result2 = free_motion_fn(conf0, conf1)
if result2 is None:
print("free motion plan fails!")
continue
path1, = result2
result3 = holding_motion_fn(conf1, conf0, block, grasp)
if result3 is None:
print('holding motion fails!')
continue
path3, = result3
return Command(path1.body_paths +
path2.body_paths +
path3.body_paths)
return None
def update(self, detections, n_samples=25):
start_time = time.time()
self.observations.append(detections)
# Processing detected first
# Could simply sample from the set of worlds and update
# Would need to sample many worlds with name at different poses
# Instead, let the moving object take on different poses
with LockRenderer():
with WorldSaver():
if REPAIR_DETECTIONS:
detections = fix_detections(
self, detections) # TODO: skip if in sim
detections = relative_detections(self, detections)
order = [name for name in detections] # Detected
order.extend(set(self.pose_dists) - set(order)) # Not detected
for name in order:
self.pose_dists[name] = self.pose_dists[name].update(
self, detections, n_samples=n_samples)
self.update_state()
print('Update time: {:.3f} sec for {} objects and {} samples'.format(
elapsed_time(start_time), len(order), n_samples))
return self
def sample_task(skill, **kwargs):
# TODO: extract and just control robot gripper
with HideOutput():
sim_world = ROSWorld(sim_only=True, visualize=kwargs['visualize'])
# Only used by pb_controller, for ros_controller it's assumed always True
sim_world.controller.execute_motor_control = True
collector = SKILL_COLLECTORS[skill]
while True: # TODO: timeout if too many failures in a row
sim_world.controller.set_gravity()
task, feature, evaluation_fn = collector.collect_fn(
sim_world, **kwargs['collect_kwargs'])
# evaluation_fn is the score function for the skill
if task is not None:
task.skill = skill
feature['simulation'] = True
break
sim_world.reset(keep_robot=True)
saver = WorldSaver()
if kwargs['visualize'] and kwargs['visualize'] != 'No_name':
draw_names(sim_world)
draw_forward_reachability(sim_world, task.arms)
return sim_world, collector, task, feature, evaluation_fn, saver
def __init__(self,
robot_name=FRANKA_CARTER,
use_gui=True,
full_kitchen=False):
self.task = None
self.interface = None
self.client = connect(use_gui=use_gui)
set_real_time(False)
#set_caching(False) # Seems to make things worse
disable_gravity()
add_data_path()
set_camera_pose(camera_point=[2, -1.5, 1])
if DEBUG:
draw_pose(Pose(), length=3)
#self.kitchen_yaml = load_yaml(KITCHEN_YAML)
with HideOutput(enable=True):
self.kitchen = load_pybullet(KITCHEN_PATH,
fixed_base=True,
cylinder=True)
self.floor = load_pybullet('plane.urdf', fixed_base=True)
z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2]
point = np.array(get_point(self.kitchen)) - np.array([0, 0, z])
set_point(self.floor, point)
self.robot_name = robot_name
if self.robot_name == FRANKA_CARTER:
urdf_path, yaml_path = FRANKA_CARTER_PATH, None
#urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML
#elif self.robot_name == EVE:
# urdf_path, yaml_path = EVE_PATH, None
else:
raise ValueError(self.robot_name)
self.robot_yaml = yaml_path if yaml_path is None else load_yaml(
yaml_path)
with HideOutput(enable=True):
self.robot = load_pybullet(urdf_path)
#dump_body(self.robot)
#chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link'))
#wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link'))
#wait_for_user()
set_point(self.robot, Point(z=stable_z(self.robot, self.floor)))
self.set_initial_conf()
self.gripper = create_gripper(self.robot)
self.environment_bodies = {}
if full_kitchen:
self._initialize_environment()
self._initialize_ik(urdf_path)
self.initial_saver = WorldSaver()
self.body_from_name = {}
# self.path_from_name = {}
self.names_from_type = {}
self.custom_limits = {}
self.base_limits_handles = []
self.cameras = {}
self.disabled_collisions = set()
if self.robot_name == FRANKA_CARTER:
self.disabled_collisions.update(
tuple(link_from_name(self.robot, link) for link in pair)
for pair in DISABLED_FRANKA_COLLISIONS)
self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf)
#self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left'))
self.calibrate_conf = FConf(
self.robot, self.arm_joints,
self.default_conf) # Must differ from carry_conf
self.special_confs = [self.carry_conf] #, self.calibrate_conf]
self.open_gq = FConf(self.robot, self.gripper_joints,
get_max_limits(self.robot, self.gripper_joints))
self.closed_gq = FConf(self.robot, self.gripper_joints,
get_min_limits(self.robot, self.gripper_joints))
self.gripper_confs = [self.open_gq, self.closed_gq]
self.open_kitchen_confs = {
joint: FConf(self.kitchen, [joint], [self.open_conf(joint)])
for joint in self.kitchen_joints
}
self.closed_kitchen_confs = {
joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)])
for joint in self.kitchen_joints
}
self._update_custom_limits()
self._update_initial()
def plan_extrusion(args_list, viewer=False, verify=False, verbose=False, watch=False):
results = []
if not args_list:
return results
# TODO: setCollisionFilterGroupMask
if not verbose:
sys.stdout = open(os.devnull, 'w')
problems = {args.problem for args in args_list}
assert len(problems) == 1
[problem] = problems
# TODO: change dir for pddlstream
extrusion_path = get_extrusion_path(problem)
#extrusion_path = rotate_problem(extrusion_path)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path, verbose=True)
elements = sorted(element_from_id.values())
#node_points = transform_model(problem, elements, node_points, ground_nodes)
connect(use_gui=viewer, shadows=SHADOWS, color=BACKGROUND_COLOR)
with LockRenderer(lock=True):
#draw_pose(unit_pose(), length=1.)
obstacles, robot = load_world()
#draw_model(elements, node_points, ground_nodes)
#wait_for_user()
color = apply_alpha(BLACK, alpha=0) # 0, 1
#color = None
element_bodies = dict(zip(elements, create_elements_bodies(node_points, elements, color=color)))
set_extrusion_camera(node_points)
#if viewer:
# label_nodes(node_points)
saver = WorldSaver()
#visualize_stiffness(extrusion_path)
#debug_elements(robot, node_points, node_order, elements)
initial_position = point_from_pose(get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
checker = None
plan = None
for args in args_list:
if args.stiffness and (checker is None):
checker = create_stiffness_checker(extrusion_path, verbose=False)
saver.restore()
#initial_conf = get_joint_positions(robot, get_movable_joints(robot))
with LockRenderer(lock=not viewer):
start_time = time.time()
plan, data = None, {}
with timeout(args.max_time):
with Profiler(num=10, cumulative=False):
plan, data = solve_extrusion(robot, obstacles, element_from_id, node_points, element_bodies,
extrusion_path, ground_nodes, args, checker=checker)
runtime = elapsed_time(start_time)
sequence = recover_directed_sequence(plan)
if verify:
max_translation, max_rotation = compute_plan_deformation(extrusion_path, recover_sequence(plan))
valid = check_plan(extrusion_path, sequence)
print('Valid:', valid)
safe = validate_trajectories(element_bodies, obstacles, plan)
print('Safe:', safe)
data.update({
'safe': safe,
'valid': valid,
'max_translation': max_translation,
'max_rotation': max_rotation,
})
data.update({
'runtime': runtime,
'num_elements': len(elements),
'ee_distance': compute_sequence_distance(node_points, sequence, start=initial_position, end=initial_position),
'print_distance': get_print_distance(plan, teleport=True),
'distance': get_print_distance(plan, teleport=False),
'sequence': sequence,
'parameters': get_global_parameters(),
'problem': args.problem,
'algorithm': args.algorithm,
'heuristic': args.bias,
'plan_extrusions': not args.disable,
'use_collisions': not args.cfree,
'use_stiffness': args.stiffness,
})
print(data)
#plan_path = '{}_solution.json'.format(args.problem)
#with open(plan_path, 'w') as f:
# json.dump(plan_data, f, indent=2, sort_keys=True)
results.append((args, data))
reset_simulation()
disconnect()
if watch and (plan is not None):
args = args_list[-1]
animate = not (args.disable or args.ee_only)
connect(use_gui=True, shadows=SHADOWS, color=BACKGROUND_COLOR) # TODO: avoid reconnecting
obstacles, robot = load_world()
display_trajectories(node_points, ground_nodes, plan, #time_step=None, video=True,
animate=animate)
reset_simulation()
disconnect()
if not verbose:
sys.stdout.close()
return results
def stripstream(robot1,
obstacles,
node_points,
element_bodies,
ground_nodes,
dual=True,
serialize=False,
hierarchy=False,
**kwargs):
robots = mirror_robot(robot1, node_points) if dual else [robot1]
elements = set(element_bodies)
initial_confs = {
ROBOT_TEMPLATE.format(i): Conf(robot)
for i, robot in enumerate(robots)
}
saver = WorldSaver()
layer_from_n = compute_layer_from_vertex(elements, node_points,
ground_nodes)
#layer_from_n = cluster_vertices(elements, node_points, ground_nodes) # TODO: increase resolution for small structures
# TODO: compute directions from first, layer from second
max_layer = max(layer_from_n.values())
print('Max layer: {}'.format(max_layer))
data = {}
if serialize:
plan, certificate = solve_serialized(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs=initial_confs,
**kwargs)
else:
plan, certificate = solve_joint(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs=initial_confs,
**kwargs)
if plan is None:
return None, data
if hierarchy:
print(SEPARATOR)
static_facts = extract_static_facts(plan, certificate, initial_confs)
partial_orders = compute_total_orders(plan)
plan, certificate = solve_joint(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs=initial_confs,
can_print=False,
can_transit=True,
additional_init=static_facts,
additional_orders=partial_orders,
**kwargs)
if plan is None:
return None, data
if plan and not isinstance(plan[0], DurativeAction):
time_from_start = 0.
retimed_plan = []
for name, args in plan:
command = args[-1]
command.retime(start_time=time_from_start)
retimed_plan.append(
DurativeAction(name, args, time_from_start, command.duration))
time_from_start += command.duration
plan = retimed_plan
plan = reverse_plan(plan)
print('\nLength: {} | Makespan: {:.3f}'.format(len(plan),
compute_duration(plan)))
# TODO: retime using the TFD duration
# TODO: attempt to resolve once without any optimistic facts to see if a solution exists
# TODO: choose a better initial config
# TODO: decompose into layers hierarchically
#planned_elements = [args[2] for name, args, _, _ in sorted(plan, key=lambda a: get_end(a))] # TODO: remove approach
#if not check_plan(extrusion_path, planned_elements):
# return None, data
if has_gui():
saver.restore()
#label_nodes(node_points)
# commands = [action.args[-1] for action in reversed(plan) if action.name == 'print']
# trajectories = flatten_commands(commands)
# elements = recover_sequence(trajectories)
# draw_ordered(elements, node_points)
# wait_if_gui('Continue?')
#simulate_printing(node_points, trajectories)
#display_trajectories(node_points, ground_nodes, trajectories)
simulate_parallel(robots, plan)
return None, data
def solve_serialized(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
trajectories=[],
post_process=False,
collisions=True,
disable=False,
max_time=INF,
**kwargs):
start_time = time.time()
saver = WorldSaver()
elements = set(element_bodies)
elements_from_layers = compute_elements_from_layer(elements, layer_from_n)
layers = sorted(elements_from_layers.keys())
print('Layers:', layers)
full_plan = []
makespan = 0.
removed = set()
for layer in reversed(layers):
print(SEPARATOR)
print('Layer: {}'.format(layer))
saver.restore()
remaining = elements_from_layers[layer]
printed = elements - remaining - removed
draw_model(remaining, node_points, ground_nodes, color=GREEN)
draw_model(printed, node_points, ground_nodes, color=RED)
problem = get_pddlstream(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
printed=printed,
removed=removed,
return_home=False,
trajectories=trajectories,
collisions=collisions,
disable=disable,
**kwargs)
layer_plan, certificate = solve_pddlstream(problem,
node_points,
element_bodies,
max_time=max_time -
elapsed_time(start_time))
remove_all_debug()
if layer_plan is None:
return None
if post_process:
print(SEPARATOR)
# Allows the planner to continue to check collisions
problem.init[:] = certificate.all_facts
#static_facts = extract_static_facts(layer_plan, ...)
#problem.init.extend(('Order',) + pair for pair in compute_total_orders(layer_plan))
for fact in [('print', ), ('move', )]:
if fact in problem.init:
problem.init.remove(fact)
new_layer_plan, _ = solve_pddlstream(problem,
node_points,
element_bodies,
planner=POSTPROCESS_PLANNER,
max_time=max_time -
elapsed_time(start_time))
if (new_layer_plan
is not None) and (compute_duration(new_layer_plan) <
compute_duration(layer_plan)):
layer_plan = new_layer_plan
user_input('{:.3f}->{:.3f}'.format(
compute_duration(layer_plan),
compute_duration(new_layer_plan)))
# TODO: replan in a cost sensitive way
layer_plan = apply_start(layer_plan, makespan)
duration = compute_duration(layer_plan)
makespan += duration
print(
'\nLength: {} | Start: {:.3f} | End: {:.3f} | Duration: {:.3f} | Makespan: {:.3f}'
.format(len(layer_plan), compute_start(layer_plan),
compute_end(layer_plan), duration, makespan))
full_plan.extend(layer_plan)
removed.update(remaining)
print(SEPARATOR)
print_plan(full_plan)
return full_plan, None
def run_loop(args, ros_world, policy):
items = wait_until_observation(args.problem, ros_world)
if items is None:
ros_world.controller.speak("Observation failure")
print('Failed to detect the required objects')
return None
task_fn = get_task_fn(PROBLEMS, 'collect_{}'.format(args.problem))
task, feature = task_fn(args, ros_world)
feature['simulation'] = False
#print('Arms:', task.arms)
#print('Required:', task.required)
init_stackings = add_scales(task, ros_world) if task.use_scales else {}
print('Scale stackings:', str_from_object(init_stackings))
print('Surfaces:', ros_world.perception.get_surfaces())
print('Items:', ros_world.perception.get_items())
add_walls(ros_world)
with ros_world:
remove_all_debug()
draw_names(ros_world)
draw_forward_reachability(ros_world, task.arms)
#########################
init_raw_masses = read_raw_masses(init_stackings)
if init_raw_masses is None:
ros_world.controller.speak("Scale failure")
return None
# Should be robust to material starting in the target
initial_contains = bowls_holding_material(task.init)
print('Initial contains:', initial_contains)
#bowl_masses = {bowl: MODEL_MASSES[get_type(bowl)] if bowl in initial_contains else init_raw_masses[bowl]
# for bowl in init_stackings.values()}
bowl_masses = {
bowl: MODEL_MASSES[get_type(bowl)]
for bowl in init_stackings.values()
}
print('Material:', args.material)
init_masses = estimate_masses(init_raw_masses, bowl_masses, args.material)
init_total_mass = sum(init_masses.values())
print('Total mass:', init_total_mass)
if POUR and (init_total_mass < 1e-3):
ros_world.controller.speak("Material failure")
print('Initial total mass is {:.3f}'.format(init_total_mass))
return None
# for bowl, mass in init_masses.items():
# if bowl in initial_contains:
# if mass < 0:
# return None
# else:
# if 0 <= mass:
# return None
#########################
policy_name = policy if isinstance(policy, str) else ''
print('Feature:', feature)
collector = SKILL_COLLECTORS[args.problem]
parameter_fns, parameter, prediction = get_parameter_fns(
ros_world, collector, policy, feature,
valid=True) # valid=not args.active)
print('Policy:', policy_name)
print('Parameter:', parameter)
print('Prediction:', prediction)
result = {
SKILL: args.problem,
'date': datetime.datetime.now().strftime(DATE_FORMAT),
'simulated': False,
'material': args.material, # TODO: pass to feature
'dataset': 'train' if args.train else 'test',
'feature': feature,
'parameter': parameter,
'score': None,
}
plan = None
if test_validity(result, ros_world, collector, feature, parameter,
prediction):
planning_world = PlanningWorld(task, visualize=args.visualize_planning)
with ClientSaver(planning_world.client):
#set_caching(False)
planning_world.load(ros_world)
print(planning_world)
start_time = time.time()
saver = WorldSaver()
plan = None
while (plan is None) and (elapsed_time(start_time) <
45): # Doesn't typically timeout
plan = plan_actions(planning_world,
parameter_fns=parameter_fns,
collisions=True,
max_time=30)
saver.restore()
planning_world.stop()
result.update({
'success': plan is not None,
'plan-time':
elapsed_time(start_time), # TODO: should be elapsed time
})
if plan is None:
print('Failed to find a plan')
ros_world.controller.speak("Planning failure")
return result
#########################
#if not review_plan(task, ros_world, plan):
# return
start_time = time.time()
ros_world.controller.speak("Executing")
success = execute_plan(ros_world,
plan,
joint_speed=0.5,
default_sleep=0.25)
print('Finished! Success:', success)
result['execution'] = success
result['execution-time'] = elapsed_time(start_time)
if not success:
ros_world.controller.speak("Execution failure")
return None
#########################
# TODO: could read scales while returning home
final_raw_masses = read_raw_masses(init_stackings)
if final_raw_masses is None:
ros_world.controller.speak("Scale failure")
return None
result['score'] = {}
result['score'].update(
score_masses(args, task, ros_world, init_total_mass, bowl_masses,
final_raw_masses))
result['score'].update(score_poses(
args.problem, task, ros_world)) # TODO: fail if fail to detect
result['score'].update({
'initial_{}_mass'.format(name): mass
for name, mass in init_raw_masses.items()
})
result['score'].update({
'final_{}_mass'.format(name): mass
for name, mass in final_raw_masses.items()
})
ros_world.controller.speak("Done")
return result
def solve_pddlstream(belief,
problem,
args,
skeleton=None,
replan_actions=set(),
max_time=INF,
max_memory=MAX_MEMORY,
max_cost=INF):
set_cost_scale(COST_SCALE)
reset_globals()
stream_info = get_stream_info()
#print(set(stream_map) - set(stream_info))
skeletons = create_ordered_skeleton(skeleton)
max_cost = min(max_cost, COST_BOUND)
print('Max cost: {:.3f} | Max runtime: {:.3f}'.format(max_cost, max_time))
constraints = PlanConstraints(skeletons=skeletons,
max_cost=max_cost,
exact=True)
success_cost = 0 if args.anytime else INF
planner = 'ff-astar' if args.anytime else 'ff-wastar2'
search_sample_ratio = 0.5 # 0.5
max_planner_time = 10
# TODO: max number of samples per iteration flag
# TODO: don't greedily expand samples with too high of a complexity if out of time
pr = cProfile.Profile()
pr.enable()
saver = WorldSaver()
sim_state = belief.sample_state()
sim_state.assign()
wait_for_duration(0.1)
with LockRenderer(lock=not args.visualize):
# TODO: option to only consider costs during local optimization
# effort_weight = 0 if args.anytime else 1
effort_weight = 1e-3 if args.anytime else 1
#effort_weight = 0
#effort_weight = None
solution = solve_focused(
problem,
constraints=constraints,
stream_info=stream_info,
replan_actions=replan_actions,
initial_complexity=5,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=max_time,
max_memory=max_memory,
verbose=True,
debug=False,
unit_efforts=True,
effort_weight=effort_weight,
max_effort=INF,
# bind=True, max_skeletons=None,
search_sample_ratio=search_sample_ratio)
saver.restore()
# print([(s.cost, s.time) for s in SOLUTIONS])
# print(SOLUTIONS)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime
return solution
def main(execute='execute'):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
args = parser.parse_args()
#display = args.display
display = True
connect(use_gui=args.viewer)
robot, block = load_world()
#robot2 = clone_body(robot)
#block2 = clone_body(block)
#dump_world()
saved_world = WorldSaver()
dump_world()
ss_problem = ss_from_problem(robot,
movable=[block],
teleport=False,
movable_collisions=True)
#ss_problem = ss_problem.debug_problem()
#print(ss_problem)
t0 = time.time()
plan, evaluations = dual_focused(ss_problem, verbose=True)
# plan, evaluations = incremental(ss_problem, verbose=True)
print_plan(plan, evaluations)
print(time.time() - t0)
if (not display) or (plan is None):
disconnect()
return
paths = []
for action, params in plan:
if action.name == 'place':
paths += params[-1].reverse().body_paths
elif action.name in ['move_free', 'move_holding', 'pick']:
paths += params[-1].body_paths
print(paths)
command = Command(paths)
if not args.viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
saved_world.restore()
user_input('Execute?')
if execute == 'control':
command.control()
elif execute == 'execute':
command.refine(num_steps=10).execute(time_step=0.001)
elif execute == 'step':
command.step()
else:
raise ValueError(execute)
#dt = 1. / 240 # Bullet default
#p.setTimeStep(dt)
wait_for_interrupt()
disconnect()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
def main():
np.set_printoptions(precision=N_DIGITS, suppress=True,
threshold=3) # , edgeitems=1) #, linewidth=1000)
parser = argparse.ArgumentParser()
parser.add_argument(
'-a',
'--algorithm',
default='rrt_connect', # choices=[],
help='The motion planning algorithm to use.')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument(
'--holonomic',
action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false', help='')
parser.add_argument('-r', '--reversible', action='store_true', help='')
parser.add_argument(
'-s',
'--seed',
default=None,
type=int, # None | 1
help='The random seed to use.')
parser.add_argument('-n',
'--num',
default=10,
type=int,
help='The number of obstacles.')
parser.add_argument('-o', '--orientation', action='store_true', help='')
parser.add_argument('-v', '--viewer', action='store_false', help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
#set_aabb_buffer(buffer=1e-3)
#set_separating_axis_collisions()
#seed = 0
#seed = time.time()
seed = args.seed
if seed is None:
seed = random.randint(0, 10**3 - 1)
print('Seed:', seed)
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
#print('Random seed:', get_random_seed(), random.random())
#print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1(n_obstacles=args.num)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits(base_limits)
# draw_pose(get_link_pose(robot, base_link), length=0.5)
start_conf = get_joint_positions(robot, base_joints)
for conf in [start_conf, goal_conf]:
draw_waypoint(conf)
#resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
plan_joints = base_joints[:2] if not args.orientation else base_joints
d = len(plan_joints)
holonomic = args.holonomic or (d != 3)
resolutions = 1. * DEFAULT_RESOLUTION * np.ones(
d) # TODO: default resolutions, velocities, accelerations fns
#weights = np.reciprocal(resolutions)
weights = np.array([1, 1, 1e-3])[:d]
cost_fn = get_acceleration_fn(robot,
plan_joints,
max_velocities=MAX_VELOCITIES[:d],
max_accelerations=MAX_ACCELERATIONS[:d])
# TODO: check that taking shortest turning direction (reversible affecting?)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
#set_all_static() # Doesn't seem to affect
#test_aabb(robot)
#test_caching(robot, obstacles)
#return
#########################
# TODO: filter if straight-line path is feasible
saver = WorldSaver()
wait_for_duration(duration=1e-2)
start_time = time.time()
with LockRenderer(lock=args.lock):
with Profiler(field='cumtime', num=25): # tottime | cumtime | None
# TODO: draw the search tree
path = plan_base_joint_motion(
robot,
plan_joints,
goal_conf[:d],
holonomic=holonomic,
reversible=args.reversible,
obstacles=obstacles,
self_collisions=False,
custom_limits=custom_limits,
use_aabb=True,
cache=True,
max_distance=MIN_PROXIMITY,
resolutions=resolutions,
weights=weights, # TODO: use KDTrees
circular={
2: UNBOUNDED_LIMITS if holonomic else CIRCULAR_LIMITS
},
cost_fn=cost_fn,
algorithm=args.algorithm,
verbose=True,
restarts=5,
max_iterations=50,
smooth=None if holonomic else 100,
smooth_time=1, # None | 100 | INF
)
saver.restore()
# TODO: draw ROADMAPS
#wait_for_duration(duration=1e-3)
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = compute_cost(robot,
base_joints,
path,
resolutions=resolutions[:len(plan_joints)])
print(
'Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
wait_if_gui()
disconnect()
return
# for i, conf in enumerate(path):
# set_joint_positions(robot, plan_joints, conf)
# wait_if_gui('{}/{}) Continue?'.format(i + 1, len(path)))
path = extract_full_path(robot, plan_joints, path, base_joints)
with LockRenderer():
draw_last_roadmap(robot, base_joints)
# for i, conf in enumerate(path):
# set_joint_positions(robot, base_joints, conf)
# wait_if_gui('{}/{}) Continue?'.format(i+1, len(path)))
iterate_path(
robot,
base_joints,
path,
step_size=2e-2,
smooth=holonomic,
custom_limits=custom_limits,
resolutions=DEFAULT_RESOLUTION * np.ones(3), # resolutions
obstacles=obstacles,
self_collisions=False,
max_distance=MIN_PROXIMITY)
disconnect()
def iterate_path(robot,
joints,
path,
simulate=False,
step_size=None,
resolutions=None,
smooth=False,
**kwargs): # 1e-2 | None
if path is None:
return
saver = WorldSaver()
path = adjust_path(robot, joints, path)
with LockRenderer():
handles = draw_path(path)
waypoints = path
#waypoints = waypoints_from_path(path)
#curve = interpolate_path(robot, joints, waypoints, k=1, velocity_fraction=1) # TODO: no velocities in the URDF
if not smooth:
curve = retime_path(robot,
joints,
path,
max_velocities=MAX_VELOCITIES,
max_accelerations=MAX_ACCELERATIONS)
else:
curve = smooth_path(robot,
joints,
path,
resolutions=resolutions,
max_velocities=MAX_VELOCITIES,
max_accelerations=MAX_ACCELERATIONS,
**kwargs)
path = discretize_curve(robot, joints, curve, resolutions=resolutions)
print('Steps: {} | Start: {:.3f} | End: {:.3f} | Knots: {}'.format(
len(path), curve.x[0], curve.x[-1], len(curve.x)))
with LockRenderer():
handles = draw_path(path)
if False:
# TODO: handle circular joints
#curve_collision_fn = lambda *args, **kwargs: False
curve_collision_fn = get_curve_collision_fn(robot,
joints,
resolutions=resolutions,
**kwargs)
with LockRenderer():
with Profiler():
curve = smooth_curve(curve,
MAX_VELOCITIES,
MAX_ACCELERATIONS,
curve_collision_fn,
max_time=5) #, curve_collision_fn=[])
saver.restore()
path = [conf for t, conf in sample_curve(curve, time_step=step_size)]
print('Steps: {} | Start: {:.3f} | End: {:.3f} | Knots: {}'.format(
len(path), curve.x[0], curve.x[-1], len(curve.x)))
with LockRenderer():
handles = draw_path(path)
if simulate:
simulate_curve(robot, joints, curve)
else:
path = [conf for t, conf in sample_curve(curve, time_step=step_size)]
step_curve(robot, joints, path, step_size=step_size)