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 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, '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 ss_from_problem(robot,
movable=[],
bound='shared',
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
print('Robot:', robot)
print('Movable:', movable)
print('Fixed:', fixed)
conf = BodyConf(robot, get_configuration(robot))
initial_atoms = [
HandEmpty(),
CanMove(),
IsConf(conf),
AtConf(conf),
initialize(TotalCost(), 0),
]
for body in movable:
pose = BodyPose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose)
]
for surface in fixed:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
initial_atoms.append(Sink(body))
if 'stove' in name:
initial_atoms.append(Stove(body))
body = movable[0]
goal_literals = [
AtConf(conf),
#Holding(body),
#On(body, fixed[0]),
#On(body, fixed[2]),
#Cleaned(body),
Cooked(body),
]
PlacedCollision = Predicate([T, O, P],
domain=[IsTraj(T), IsPose(O, P)],
fn=get_movable_collision_test(),
bound=False)
actions = [
Action(name='pick',
param=[O, P, G, Q, T],
pre=[
IsKin(O, P, G, Q, T),
HandEmpty(),
AtPose(O, P),
AtConf(Q), ~Unsafe(T)
],
eff=[HasGrasp(O, G),
CanMove(), ~HandEmpty(), ~AtPose(O, P)]),
Action(
name='place',
param=[O, P, G, Q, T],
pre=[IsKin(O, P, G, Q, T),
HasGrasp(O, G),
AtConf(Q), ~Unsafe(T)],
eff=[HandEmpty(),
CanMove(),
AtPose(O, P), ~HasGrasp(O, G)]),
# Can just do move if we check holding collisions
Action(name='move_free',
param=[Q, Q2, T],
pre=[
IsFreeMotion(Q, Q2, T),
HandEmpty(),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(name='move_holding',
param=[Q, Q2, O, G, T],
pre=[
IsHoldingMotion(Q, Q2, O, G, T),
HasGrasp(O, G),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Sink(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2), Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
axioms = [
Axiom(param=[O, G],
pre=[IsGrasp(O, G), HasGrasp(O, G)],
eff=Holding(O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[T, O, P],
pre=[IsPose(O, P),
PlacedCollision(T, O, P),
AtPose(O, P)],
eff=Unsafe(T)),
]
streams = [
GenStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(robot, grasp_name),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
# TODO: test_support
GenStream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(fixed=fixed),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2)],
bound=bound),
FnStream(name='inverse_kin',
inp=[O, P, G],
domain=[IsPose(O, P), IsGrasp(O, G)],
fn=get_ik_fn(robot, fixed=fixed, teleport=teleport),
out=[Q, T],
graph=[IsKin(O, P, G, Q, T),
IsConf(Q), IsTraj(T)],
bound=bound),
FnStream(name='free_motion',
inp=[Q, Q2],
domain=[IsConf(Q), IsConf(Q2)],
fn=get_free_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsFreeMotion(Q, Q2, T),
IsTraj(T)],
bound=bound),
FnStream(name='holding_motion',
inp=[Q, Q2, O, G],
domain=[IsConf(Q), IsConf(Q2),
IsGrasp(O, G)],
fn=get_holding_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsHoldingMotion(Q, Q2, O, G, T),
IsTraj(T)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
