def fn(bq, *args): #, aq):
# TODO: include if holding anything?
bq.assign()
aq = world.carry_conf
#aq.assign() # TODO: could sample aq instead achieve it by move actions
#world.open_gripper()
robot_saver = BodySaver(world.robot)
cmd = Sequence(
State(world, savers=[robot_saver]),
commands=[
#Trajectory(world, world.robot, world.arm_joints, approach_path),
# TODO: calibrate command
],
name='calibrate')
return (cmd, )
def fn(gq1, gq2):
#if gq1 == gq2:
# return None
if teleport:
path = [gq1.values, gq2.values]
else:
extend_fn = get_extend_fn(gq2.body,
gq2.joints,
resolutions=resolutions)
path = [gq1.values] + list(extend_fn(gq1.values, gq2.values))
cmd = Sequence(State(world),
commands=[
Trajectory(world, gq2.body, gq2.joints, path),
],
name='gripper')
return (cmd, )
def fn(bq, aq1, aq2, fluents=[]):
#if aq1 == aq2:
# return None
bq.assign()
aq1.assign()
attachments, obstacles = parse_fluents(world, fluents)
obstacles.update(world.static_obstacles)
if not collisions:
obstacles = set()
robot_saver = BodySaver(world.robot)
if teleport:
path = [aq1.values, aq2.values]
else:
path = plan_joint_motion(
world.robot,
aq2.joints,
aq2.values,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions,
restarts=2,
iterations=50,
smooth=50)
if path is None:
print('Failed to find an arm motion plan for {}->{}'.format(
aq1, aq2))
if PAUSE_MOTION_FAILURES:
set_renderer(enable=True)
print(fluents)
for bq in [aq1, aq2]:
bq.assign()
wait_for_user()
set_renderer(enable=False)
return None
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
Trajectory(world, world.robot, world.arm_joints,
path),
],
name='arm')
return (cmd, )
def gen(bowl_name, wp, cup_name, grasp, bq):
# https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/plan_tools/samplers/pour.py
if bowl_name == cup_name:
return
#attachment = get_grasp_attachment(world, arm, grasp)
bowl_body = world.get_body(bowl_name)
#cup_body = world.get_body(cup_name)
obstacles = (world.static_obstacles
| {bowl_body}) if collisions else set()
cup_path_bowl = pour_path_from_parameter(world, bowl_name, cup_name)
while True:
for _ in range(max_attempts):
bowl_pose = wp.get_world_from_body()
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
rotate_cup = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
cup_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl,
rotate_cup) for cup_pose_bowl in cup_path_bowl
]
#visualize_cartesian_path(cup_body, cup_path)
#if cartesian_path_collision(cup_body, cup_path, obstacles + [bowl_body]):
# continue
tool_path = [
multiply(p, invert(grasp.grasp_pose)) for p in cup_path
]
# TODO: extra collision test for visibility
# TODO: orientation constraint while moving
bq.assign()
grasp.set_gripper()
world.carry_conf.assign()
arm_path = plan_workspace(world,
tool_path,
obstacles,
randomize=True) # tilt to upright
if arm_path is None:
continue
assert MOVE_ARM
aq = FConf(world.robot, world.arm_joints, arm_path[-1])
robot_saver = BodySaver(world.robot)
obj_type = type_from_name(cup_name)
duration = 5.0 if obj_type in [MUSTARD] else 1.0
objects = [bowl_name, cup_name]
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
ApproachTrajectory(objects, world,
world.robot,
world.arm_joints,
arm_path[::-1]),
Wait(world, duration=duration),
ApproachTrajectory(objects, world,
world.robot,
world.arm_joints,
arm_path),
],
name='pour')
yield (
aq,
cmd,
)
break
else:
yield None
def plan_pull(world,
door_joint,
door_plan,
base_conf,
randomize=True,
collisions=True,
teleport=False,
**kwargs):
door_path, handle_path, handle_plan, tool_path = door_plan
handle_link, handle_grasp, handle_pregrasp = handle_plan
door_obstacles = get_descendant_obstacles(
world.kitchen, door_joint) # if collisions else set()
obstacles = (world.static_obstacles | door_obstacles
) # if collisions else set()
base_conf.assign()
world.open_gripper()
world.carry_conf.assign()
robot_saver = BodySaver(world.robot) # TODO: door_saver?
if not is_pull_safe(world, door_joint, door_plan):
return
arm_path = plan_workspace(world,
tool_path,
world.static_obstacles,
randomize=randomize,
teleport=collisions)
if arm_path is None:
return
approach_paths = []
for index in [0, -1]:
set_joint_positions(world.kitchen, [door_joint], door_path[index])
set_joint_positions(world.robot, world.arm_joints, arm_path[index])
tool_pose = multiply(handle_path[index], invert(handle_pregrasp))
approach_path = plan_approach(world,
tool_pose,
obstacles=obstacles,
teleport=teleport,
**kwargs)
if approach_path is None:
return
approach_paths.append(approach_path)
if MOVE_ARM:
aq1 = FConf(world.robot, world.arm_joints, approach_paths[0][0])
aq2 = FConf(world.robot, world.arm_joints, approach_paths[-1][0])
else:
aq1 = world.carry_conf
aq2 = aq1
set_joint_positions(world.kitchen, [door_joint], door_path[0])
set_joint_positions(world.robot, world.arm_joints, arm_path[0])
grasp_width = close_until_collision(world.robot,
world.gripper_joints,
bodies=[(world.kitchen, [handle_link])
])
gripper_motion_fn = get_gripper_motion_gen(world,
teleport=teleport,
collisions=collisions,
**kwargs)
gripper_conf = FConf(world.robot, world.gripper_joints,
[grasp_width] * len(world.gripper_joints))
finger_cmd, = gripper_motion_fn(world.open_gq, gripper_conf)
objects = []
commands = [
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
approach_paths[0]),
DoorTrajectory(world, world.robot, world.arm_joints, arm_path,
world.kitchen, [door_joint], door_path),
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
reversed(approach_paths[-1])),
]
door_path, _, _, _ = door_plan
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
if pull:
commands.insert(1, finger_cmd.commands[0])
commands.insert(3, finger_cmd.commands[0].reverse())
cmd = Sequence(State(world, savers=[robot_saver]), commands, name='pull')
yield (
aq1,
aq2,
cmd,
)
def plan_pick(world,
obj_name,
pose,
grasp,
base_conf,
obstacles,
randomize=True,
**kwargs):
# TODO: check if within database convex hull
# TODO: flag to check if initially in collision
obj_body = world.get_body(obj_name)
pose.assign()
base_conf.assign()
world.open_gripper()
robot_saver = BodySaver(world.robot)
obj_saver = BodySaver(obj_body)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
world_from_body = pose.get_world_from_body()
gripper_pose = multiply(world_from_body, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
full_grasp_conf = world.solve_inverse_kinematics(gripper_pose)
if full_grasp_conf is None:
if PRINT_FAILURES: print('Grasp kinematic failure')
return
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
#robot_obstacle = get_descendant_obstacles(world.robot, child_link_from_joint(world.arm_joints[0]))
#robot_obstacle = world.robot
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Grasp collision failure')
#set_renderer(enable=True)
#wait_for_user()
#set_renderer(enable=False)
return
approach_pose = multiply(world_from_body, invert(grasp.pregrasp_pose))
approach_path = plan_approach(
world,
approach_pose, # attachments=[grasp.get_attachment()],
obstacles=obstacles,
**kwargs)
if approach_path is None:
if PRINT_FAILURES: print('Approach plan failure')
return
if MOVE_ARM:
aq = FConf(world.robot, world.arm_joints, approach_path[0])
else:
aq = world.carry_conf
gripper_motion_fn = get_gripper_motion_gen(world, **kwargs)
finger_cmd, = gripper_motion_fn(world.open_gq, grasp.get_gripper_conf())
attachment = create_surface_attachment(world, obj_name, pose.support)
objects = [obj_name]
cmd = Sequence(State(world,
savers=[robot_saver, obj_saver],
attachments=[attachment]),
commands=[
ApproachTrajectory(objects, world, world.robot,
world.arm_joints, approach_path),
finger_cmd.commands[0],
Detach(world, attachment.parent, attachment.parent_link,
attachment.child),
AttachGripper(world, obj_body, grasp=grasp),
ApproachTrajectory(objects, world, world.robot,
world.arm_joints,
reversed(approach_path)),
],
name='pick')
yield (
aq,
cmd,
)
def plan_press(world,
knob_name,
pose,
grasp,
base_conf,
obstacles,
randomize=True,
**kwargs):
base_conf.assign()
world.close_gripper()
robot_saver = BodySaver(world.robot)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
gripper_pose = multiply(pose, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
#set_joint_positions(world.gripper, get_movable_joints(world.gripper), world.closed_gq.values)
#set_tool_pose(world, gripper_pose)
full_grasp_conf = world.solve_inverse_kinematics(gripper_pose)
#wait_for_user()
if full_grasp_conf is None:
# if PRINT_FAILURES: print('Grasp kinematic failure')
return
robot_obstacle = (world.robot,
frozenset(get_moving_links(world.robot,
world.arm_joints)))
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
#if PRINT_FAILURES: print('Grasp collision failure')
return
approach_pose = multiply(pose, invert(grasp.pregrasp_pose))
approach_path = plan_approach(world,
approach_pose,
obstacles=obstacles,
**kwargs)
if approach_path is None:
return
aq = FConf(world.robot, world.arm_joints,
approach_path[0]) if MOVE_ARM else world.carry_conf
#gripper_motion_fn = get_gripper_motion_gen(world, **kwargs)
#finger_cmd, = gripper_motion_fn(world.open_gq, world.closed_gq)
objects = []
cmd = Sequence(
State(world, savers=[robot_saver]),
commands=[
#finger_cmd.commands[0],
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
approach_path),
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
reversed(approach_path)),
#finger_cmd.commands[0].reverse(),
Wait(world, duration=1.0),
],
name='press')
yield (
aq,
cmd,
)
def fn(bq1, bq2, aq, fluents=[]):
#if bq1 == bq2:
# return None
aq.assign()
attachments, obstacles = parse_fluents(world, fluents)
obstacles.update(world.static_obstacles)
if not collisions:
obstacles = set()
start_path, end_path = [], []
if hasattr(bq1, 'nearby_bq'):
bq1.assign()
start_path = plan_nonholonomic_motion(
world.robot,
bq2.joints,
bq1.nearby_bq.values,
attachments=attachments,
obstacles=obstacles,
custom_limits=world.custom_limits,
reversible=True,
self_collisions=False,
restarts=-1)
if start_path is None:
print('Failed to find nearby base conf!')
return
bq1 = bq1.nearby_bq
if hasattr(bq2, 'nearby_bq'):
bq2.nearby_bq.assign()
end_path = plan_nonholonomic_motion(
world.robot,
bq2.joints,
bq2.values,
attachments=attachments,
obstacles=obstacles,
custom_limits=world.custom_limits,
reversible=True,
self_collisions=False,
restarts=-1)
if end_path is None:
print('Failed to find nearby base conf!')
return
bq2 = bq2.nearby_bq
bq1.assign()
robot_saver = BodySaver(world.robot)
if (bq1 == bq2) or teleport_base or teleport:
path = [bq1.values, bq2.values]
else:
# It's important that the extend function is reversible to avoid getting trapped
path = plan_nonholonomic_motion(world.robot,
bq2.joints,
bq2.values,
attachments=attachments,
obstacles=obstacles,
custom_limits=world.custom_limits,
reversible=True,
self_collisions=False,
restarts=restarts,
iterations=iterations,
smooth=smooth)
if path is None:
print('Failed to find an arm motion plan for {}->{}'.format(
bq1, bq2))
if PAUSE_MOTION_FAILURES:
set_renderer(enable=True)
print(fluents)
for bq in [bq1, bq2]:
bq.assign()
wait_for_user()
set_renderer(enable=False)
return None
# TODO: could actually plan with all joints as long as we return to the same config
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
Trajectory(world, world.robot, world.base_joints,
path)
for path in [start_path, path, end_path] if path
],
name='base')
return (cmd, )