def optimize_angle(robot,
link,
element_pose,
translation,
direction,
reverse,
initial_angles,
collision_fn,
max_error=1e-2):
movable_joints = get_movable_joints(robot)
initial_conf = get_joint_positions(robot, movable_joints)
best_error, best_angle, best_conf = max_error, None, None
for i, angle in enumerate(initial_angles):
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
target_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, target_pose)
# if conf is None:
# continue
#if pairwise_collision(robot, robot):
conf = get_joint_positions(robot, movable_joints)
if not collision_fn(conf):
link_pose = get_link_pose(robot, link)
error = get_distance(point_from_pose(target_pose),
point_from_pose(link_pose))
if error < best_error: # TODO: error a function of direction as well
best_error, best_angle, best_conf = error, angle, conf
# wait_for_interrupt()
if i != len(initial_angles) - 1:
set_joint_positions(robot, movable_joints, initial_conf)
#print(best_error, translation, direction, best_angle)
if best_conf is not None:
set_joint_positions(robot, movable_joints, best_conf)
#wait_for_interrupt()
return best_angle, best_conf
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), scale_cost(1)),
Equal(('PlaceCost',), scale_cost(1)),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = [AND]
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
'MoveCost': move_cost_fn,
'TrajPoseCollision': fn_from_constant(False),
'TrajArmCollision': fn_from_constant(False),
'TrajGraspCollision': fn_from_constant(False),
}
if USE_SYNTHESIZERS:
stream_map['plan-base-motion'] = empty_gen(),
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'world': 'world',
}
world = 'world'
initial_bq = Pose(robot, get_pose(robot))
init = [
('CanMove',),
('BConf', initial_bq), # TODO: could make pose as well...
('AtBConf', initial_bq),
('AtAConf', world, None),
('AtPose', world, world, None),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
#for arm in ARM_JOINT_NAMES:
for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf),
('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', world, body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = ['and']
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
#'plan-base-motion': empty_gen(),
'BTrajCollision': fn_from_constant(False),
}
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_state(state, teleport=False):
task = state.task
robot = task.robot
# TODO: infer open world from task
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'base': 'base',
'left': 'left',
'right': 'right',
'head': 'head',
}
#base_conf = state.poses[robot]
base_conf = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
scan_cost = 1
init = [
('BConf', base_conf),
('AtBConf', base_conf),
Equal(('MoveCost', ), 1),
Equal(('PickCost', ), 1),
Equal(('PlaceCost', ), 1),
Equal(('ScanCost', ), scan_cost),
Equal(('RegisterCost', ), 1),
]
holding_arms = set()
holding_bodies = set()
for attach in state.attachments.values():
holding_arms.add(attach.arm)
holding_bodies.add(attach.body)
init += [('Grasp', attach.body, attach.grasp),
('AtGrasp', attach.arm, attach.body, attach.grasp)]
for arm in ARM_NAMES:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)]
if arm in task.arms:
init += [('Controllable', arm)]
if arm not in holding_arms:
init += [('HandEmpty', arm)]
for body in task.get_bodies():
if body in holding_bodies:
continue
# TODO: no notion whether observable actually corresponds to the correct thing
pose = state.poses[body]
init += [
('Pose', body, pose),
('AtPose', body, pose),
('Observable', pose),
]
init += [('Scannable', body) for body in task.rooms + task.surfaces]
init += [('Registerable', body) for body in task.movable]
init += [('Graspable', body) for body in task.movable]
for body in task.get_bodies():
supports = task.get_supports(body)
if supports is None:
continue
for surface in supports:
p_obs = state.b_on[body].prob(surface)
cost = revisit_mdp_cost(0, scan_cost,
p_obs) # TODO: imperfect observation model
init += [('Stackable', body, surface),
Equal(('LocalizeCost', surface, body), clip_cost(cost))]
#if is_placement(body, surface):
if is_center_stable(body, surface):
if body in holding_bodies:
continue
pose = state.poses[body]
init += [('Supported', body, pose, surface)]
for body in task.get_bodies():
if state.is_localized(body):
init.append(('Localized', body))
else:
init.append(('Uncertain', body))
if body in state.registered:
init.append(('Registered', body))
goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \
[('On', b, s) for b, s in task.goal_on] + \
[('Localized', b) for b in task.goal_localized] + \
[('Registered', b) for b in task.goal_registered])
stream_map = {
'sample-pose':
get_stable_gen(task),
'sample-grasp':
from_list_fn(get_grasp_gen(task)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(task, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(task, teleport=teleport)),
'test-vis-base':
from_test(get_in_range_test(task, VIS_RANGE)),
'test-reg-base':
from_test(get_in_range_test(task, REG_RANGE)),
'sample-vis-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)),
max_attempts=25),
'sample-reg-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)),
max_attempts=25),
'inverse-visibility':
from_fn(get_inverse_visibility_fn(task)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def main(precompute=False):
parser = argparse.ArgumentParser()
# djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
parser.add_argument('-p',
'--problem',
default='simple_frame',
help='The name of the problem to solve')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions with obstacles')
parser.add_argument('-m',
'--motions',
action='store_true',
help='Plans motions between each extrusion')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
args = parser.parse_args()
print('Arguments:', args)
# TODO: setCollisionFilterGroupMask
# TODO: fail if wild stream produces unexpected facts
# TODO: try search at different cost levels (i.e. w/ and w/o abstract)
# TODO: submodule in https://github.mit.edu/yijiangh/assembly-instances
elements, node_points, ground_nodes = load_extrusion(args.problem)
node_order = list(range(len(node_points)))
#np.random.shuffle(node_order)
node_order = sorted(node_order, key=lambda n: node_points[n][2])
elements = sorted(elements,
key=lambda e: min(node_points[n][2] for n in e))
#node_order = node_order[:100]
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [
element for element in elements
if all(n in node_order for n in element)
]
#plan = plan_sequence_test(node_points, elements, ground_nodes)
connect(use_gui=args.viewer)
floor, robot = load_world()
obstacles = [floor]
initial_conf = get_joint_positions(robot, get_movable_joints(robot))
#dump_body(robot)
#if has_gui():
# draw_model(elements, node_points, ground_nodes)
# wait_for_interrupt('Continue?')
#joint_weights = compute_joint_weights(robot, num=1000)
#elements = elements[:50] # 10 | 50 | 100 | 150
#debug_elements(robot, node_points, node_order, elements)
element_bodies = dict(zip(elements, create_elements(node_points,
elements)))
if precompute:
pr = cProfile.Profile()
pr.enable()
trajectories = sample_trajectories(robot, obstacles, node_points,
element_bodies, ground_nodes)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
user_input('Continue?')
else:
trajectories = []
plan = plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories,
collisions=not args.cfree)
if args.motions:
plan = compute_motions(robot, obstacles, element_bodies, initial_conf,
plan)
disconnect()
display_trajectories(ground_nodes, plan)
def main(viewer=True):
# TODO: setCollisionFilterGroupMask
# TODO: only produce collisions rather than collision-free
# TODO: return collisions using wild-stream functionality
# TODO: handle movements between selected edges
# TODO: fail if wild stream produces unexpected facts
# TODO: try search at different cost levels (i.e. w/ and w/o abstract)
# https://github.mit.edu/yijiangh/assembly-instances
#read_minizinc_data(os.path.join(root_directory, 'print_data.txt'))
#return
# djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
elements, node_points, ground_nodes = load_extrusion('voronoi')
node_order = list(range(len(node_points)))
#np.random.shuffle(node_order)
node_order = sorted(node_order, key=lambda n: node_points[n][2])
elements = sorted(elements,
key=lambda e: min(node_points[n][2] for n in e))
#node_order = node_order[:100]
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [
element for element in elements
if all(n in node_order for n in element)
]
print('Nodes: {} | Ground: {} | Elements: {}'.format(
len(node_points), len(ground_nodes), len(elements)))
#plan = plan_sequence_test(node_points, elements, ground_nodes)
connect(use_gui=viewer)
floor, robot = load_world()
obstacles = [floor]
initial_conf = get_joint_positions(robot, get_movable_joints(robot))
#dump_body(robot)
#if has_gui():
# draw_model(elements, node_points, ground_nodes)
# wait_for_interrupt('Continue?')
#joint_weights = compute_joint_weights(robot, num=1000)
#elements = elements[:5]
#elements = elements[:10]
#elements = elements[:25]
#elements = elements[:35]
#elements = elements[:50]
#elements = elements[:75]
#elements = elements[:100]
#elements = elements[:150]
#elements = elements[150:]
# TODO: prune if it collides with any of its supports
# TODO: prioritize choices that don't collide with too many edges
# TODO: accumulate edges along trajectory
#test_grasps(robot, node_points, elements)
#test_print(robot, node_points, elements)
#return
#for element in elements:
# color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0, 0)
# draw_element(node_points, element, color=color)
#wait_for_interrupt('Continue?')
# TODO: topological sort
#node = node_order[40]
#node_neighbors = get_node_neighbors(elements)
#for element in node_neighbors[node]:
# color = (0, 1, 0) if element_supports(element, node, node_points) else (1, 0, 0)
# draw_element(node_points, element, color)
#element = elements[-1]
#draw_element(node_points, element, (0, 1, 0))
#incoming_edges, _ = neighbors_from_orders(get_supported_orders(elements, node_points))
#supporters = []
#retrace_supporters(element, incoming_edges, supporters)
#for e in supporters:
# draw_element(node_points, e, (1, 0, 0))
#wait_for_interrupt('Continue?')
#for name, args in plan:
# n1, e, n2 = args
# draw_element(node_points, e)
# user_input('Continue?')
#test_ik(robot, node_order, node_points)
element_bodies = dict(zip(elements, create_elements(node_points,
elements)))
precompute = False
if precompute:
pr = cProfile.Profile()
pr.enable()
trajectories = sample_trajectories(robot, obstacles, node_points,
element_bodies, ground_nodes)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
user_input('Continue?')
else:
trajectories = []
motions = False
plan = plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories)
if motions:
plan = compute_motions(robot, obstacles, element_bodies, initial_conf,
plan)
disconnect()
display_trajectories(ground_nodes, plan)
def pddlstream_from_problem(problem,
base_limits=None,
collisions=True,
teleport=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'@sink': 'sink',
'@stove': 'stove',
}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), 1),
Equal(('PlaceCost',), 1),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm),
('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body), init=True) # TODO: supported here
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose), ('Stackable', body, None)]
for surface in problem.surfaces:
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body, ty in problem.body_types:
init += [('Type', body, ty)]
bodies_from_type = get_bodies_from_type(problem)
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Conf(robot, get_group_joints(robot, 'base'),
problem.goal_conf)
init += [('BConf', goal_conf)]
goal_literals += [('AtBConf', goal_conf)]
for ty, s in problem.goal_on:
bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(
ty) else [ty]
init += [('Stackable', b, s) for b in bodies]
goal_literals += [('On', ty, s)]
goal_literals += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
goal_formula = []
for literal in goal_literals:
parameters = [a for a in get_args(literal) if is_parameter(a)]
if parameters:
type_literals = [('Type', p, get_parameter_name(p))
for p in parameters]
goal_formula.append(
Exists(parameters, And(literal, *type_literals)))
else:
goal_formula.append(literal)
goal_formula = And(*goal_formula)
custom_limits = {}
if base_limits is not None:
custom_limits.update(get_custom_limits(robot, problem.base_limits))
stream_map = {
'sample-pose':
from_gen_fn(get_stable_gen(problem, collisions=collisions)),
'sample-grasp':
from_list_fn(get_grasp_gen(problem, collisions=collisions)),
#'sample-grasp': from_gen_fn(get_grasp_gen(problem, collisions=collisions)),
'inverse-kinematics':
from_gen_fn(
get_ik_ir_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'plan-base-motion':
from_fn(
get_motion_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'test-cfree-pose-pose':
from_test(get_cfree_pose_pose_test(collisions=collisions)),
'test-cfree-approach-pose':
from_test(get_cfree_approach_pose_test(problem,
collisions=collisions)),
'test-cfree-traj-pose':
from_test(get_cfree_traj_pose_test(robot, collisions=collisions)),
#'test-cfree-traj-grasp-pose': from_test(get_cfree_traj_grasp_pose_test(problem, collisions=collisions)),
#'MoveCost': move_cost_fn,
'Distance':
distance_fn,
}
#stream_map = DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def get_base_conf(robot):
return get_joint_positions(robot, get_base_joints(robot))
