def gen_fn(arm, obj_name, pose):
open_width = get_max_limit(world.robot,
get_gripper_joints(world.robot, arm)[0])
obj_body = world.bodies[obj_name]
#grasps = cycle([(grasp,)])
grasps = cycle(grasp_gen_fn(obj_name))
for attempt in range(max_attempts):
try:
grasp, = next(grasps)
except StopIteration:
break
# TODO: if already successful for a grasp, continue
set_pose(obj_body, pose)
set_gripper_position(world.robot, arm, open_width)
robot_saver = BodySaver(
world.robot) # TODO: robot might be at the wrong conf
body_saver = BodySaver(obj_body)
pretool_pose = multiply(pose, invert(grasp.pregrasp_pose))
tool_pose = multiply(pose, invert(grasp.grasp_pose))
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_pose,
obstacles=obstacles)
if grip_conf is None:
continue
#attachment = Attachment(world.robot, tool_link, get_grasp_pose(grasp), world.bodies[obj])
# Attachments cause table collisions
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacles,
attachments=[], #attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pick',
'objects': [obj_name],
'context':
Context(savers=[robot_saver, body_saver], attachments={}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
GripperCommand(arm, grasp.grasp_width,
effort=grasp.effort),
Attach(arm, obj_name, effort=grasp.effort),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (grasp, pre_conf, post_conf, control)
def hold_item(world, arm, name):
try:
grasp, = next(get_grasp_gen_fn(world)(name))
except StopIteration:
return None
set_gripper_position(world.robot, arm, grasp.grasp_width)
attachment = get_grasp_attachment(world, arm, grasp)
attachment.assign()
world.controller.attach(get_arm_prefix(arm), name)
return {arm: grasp}
def gen_fn(arm, bowl_name, bowl_pose, stirrer_name, grasp):
if bowl_name == stirrer_name:
return
bowl_body = world.bodies[bowl_name]
attachment = get_grasp_attachment(world, arm, grasp)
feature = get_stir_feature(world, bowl_name, stirrer_name)
parameter_generator = parameter_fn(world, feature)
if revisit:
parameter_generator = cycle(parameter_generator)
for parameter in parameter_generator:
for _ in range(max_attempts):
set_gripper_position(world.robot, arm, grasp.grasp_width)
set_pose(bowl_body, bowl_pose)
stirrer_path_bowl = sample_stir_trajectory(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
stirrer_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in stirrer_path_bowl
]
#visualize_cartesian_path(world.get_body(stirrer_name), stirrer_path)
arm_path = plan_attachment_motion(
world.robot,
arm,
attachment,
stirrer_path,
obstacles=set(obstacles) - {bowl_body},
self_collisions=collisions,
disabled_collisions=disabled_collisions,
collision_buffer=collision_buffer,
attachment_collisions=False)
if arm_path is None:
continue
pre_conf = Conf(arm_path[0])
post_conf = Conf(arm_path[-1])
control = Control({
'action':
'stir',
'objects': [bowl_name, stirrer_name],
'feature':
feature,
'parameter':
parameter,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={stirrer_name: attachment}),
'commands': [
ArmTrajectory(arm, arm_path),
],
})
yield (pre_conf, post_conf, control)
def gen_fn(arm, button):
gripper_joint = get_gripper_joints(world.robot, arm)[0]
closed_width, open_width = get_joint_limits(world.robot, gripper_joint)
pose = world.initial_poses[button]
body = world.bodies[button]
presses = cycle(get_top_presses(body, tool_pose=TOOL_POSE))
for attempt in range(max_attempts):
try:
press = next(presses)
except StopIteration:
break
set_gripper_position(world.robot, arm, closed_width)
tool_pose = multiply(pose, invert(press))
grip_conf = solve_inverse_kinematics(
world.robot,
arm,
tool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer)
if grip_conf is None:
continue
pretool_pose = multiply(tool_pose, Pose(point=pre_direction))
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer,
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
post_conf = Conf(post_path[-1])
pre_conf = post_conf
pre_path = post_path[::-1]
control = Control({
'action':
'press',
'objects': [],
'context':
Context( # TODO: robot might be at the wrong conf
savers=[BodySaver(world.robot)
], # TODO: start with open instead
attachments={}),
'commands': [
GripperCommand(arm, closed_width),
ArmTrajectory(arm, pre_path),
ArmTrajectory(arm, post_path),
GripperCommand(arm, open_width),
],
})
yield (pre_conf, post_conf, control)
def gen_fn(arm, obj_name, pose1, region):
# TODO: reachability test here
if region is None:
goals = push_goal_gen_fn(obj_name, pose1, surface)
elif isinstance(region, str):
goals = push_goal_gen_fn(obj_name, pose1, surface, region=region)
else:
goals = [(region,)]
if repeat:
goals = cycle(goals)
arm_joints = get_arm_joints(world.robot, arm)
open_width = get_max_limit(world.robot, get_gripper_joints(world.robot, arm)[0])
body = world.bodies[obj_name]
for goal_pos2d, in islice(goals, max_samples):
pose2 = get_end_pose(pose1, goal_pos2d)
body_path = list(interpolate_poses(pose1, pose2))
if cartesian_path_collision(body, body_path, set(obstacles) - {surface_body}) or \
cartesian_path_unsupported(body, body_path, surface_body):
continue
set_pose(body, pose1)
push_direction = np.array(point_from_pose(pose2)) - np.array(point_from_pose(pose1))
backoff_tform = Pose(-backoff_distance * get_unit_vector(push_direction)) # World coordinates
feature = get_push_feature(world, arm, obj_name, pose1, goal_pos2d)
for parameter in parameter_fn(world, feature):
push_contact = next(iter(sample_push_contact(world, feature, parameter, under=False)))
gripper_path = [multiply(pose, invert(multiply(TOOL_POSE, push_contact))) for pose in body_path]
set_gripper_position(world.robot, arm, open_width)
for _ in range(max_attempts):
start_conf = solve_inverse_kinematics(world.robot, arm, gripper_path[0], obstacles=obstacles)
if start_conf is None:
continue
set_pose(body, pose1)
body_saver = BodySaver(body)
#attachment = create_attachment(world.robot, arm, body)
push_path = plan_waypoint_motion(world.robot, arm, gripper_path[-1],
obstacles=obstacles, #attachments=[attachment],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if push_path is None:
continue
pre_backoff_pose = multiply(backoff_tform, gripper_path[0])
pre_approach_pose = multiply(pre_backoff_pose, approach_tform)
set_joint_positions(world.robot, arm_joints, push_path[0])
pre_path = plan_waypoints_motion(world.robot, arm, [pre_backoff_pose, pre_approach_pose],
obstacles=obstacles, attachments=[],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if pre_path is None:
continue
pre_path = pre_path[::-1]
post_backoff_pose = multiply(backoff_tform, gripper_path[-1])
post_approach_pose = multiply(post_backoff_pose, approach_tform)
set_joint_positions(world.robot, arm_joints, push_path[-1])
post_path = plan_waypoints_motion(world.robot, arm, [post_backoff_pose, post_approach_pose],
obstacles=obstacles, attachments=[],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(pre_path[0])
set_joint_positions(world.robot, arm_joints, pre_conf)
robot_saver = BodySaver(world.robot)
post_conf = Conf(post_path[-1])
control = Control({
'action': 'push',
'objects': [obj_name],
'feature': feature,
'parameter': None,
'context': Context(
savers=[robot_saver, body_saver],
attachments={}),
'commands': [
ArmTrajectory(arm, pre_path),
Push(arm, obj_name),
ArmTrajectory(arm, push_path),
Detach(arm, obj_name),
ArmTrajectory(arm, post_path),
],
})
yield (pose2, pre_conf, post_conf, control)
break
def gen_fn(arm, bowl_name, bowl_pose, cup_name, grasp):
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)
feature = get_pour_feature(world, bowl_name, cup_name)
# TODO: this may be called several times with different grasps
for parameter in parameter_generator(feature):
for i in range(max_attempts):
set_pose(bowl_body,
bowl_pose) # Reset because might have changed
set_gripper_position(world.robot, arm, grasp.grasp_width)
cup_path_bowl, times_from_start = pour_path_from_parameter(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
cup_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in cup_path_bowl
]
#if world.visualize:
# visualize_cartesian_path(cup_body, cup_path)
if cartesian_path_collision(cup_body, cup_path,
obstacles + [bowl_body]):
print('Attempt {}: Pour path collision!'.format(i))
continue
tool_waypoints = [
multiply(p, invert(grasp.grasp_pose)) for p in cup_path
]
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_waypoints[0],
obstacles=obstacles)
if grip_conf is None:
continue
if water_robot_collision(world, bowl_body, bowl_pose, cup_body,
cup_path):
print('Attempt {}: Water robot collision'.format(i))
continue
if water_obstacle_collision(world, bowl_body, bowl_pose,
cup_body, cup_path):
print('Attempt {}: Water obstacle collision'.format(i))
continue
post_path = plan_workspace_motion(
world.robot,
arm,
tool_waypoints,
obstacles=obstacles + [bowl_body],
attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pour',
'feature':
feature,
'parameter':
parameter,
'objects': [bowl_name, cup_name],
'times':
times_from_start,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={cup_name: attachment}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
Rest(duration=2.0),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (pre_conf, post_conf, control)
break
else:
yield None
def gen_fn(arm, conf1, conf2, fluents=[]):
arm_confs, object_grasps, object_poses, contains_liquid = parse_fluents(
world, fluents)
for a, q in arm_confs.items():
#print(a, q) # TODO: some optimistic values are getting through
set_joint_positions(world.robot, get_arm_joints(world.robot, a), q)
for name, pose in object_poses.items():
set_pose(world.bodies[name], pose)
obstacle_names = list(world.get_fixed()) + list(object_poses)
obstacles = [world.bodies[name] for name in obstacle_names]
attachments = {}
holding_water = None
water_attachment = None
for arm2, (obj, grasp) in object_grasps.items():
set_gripper_position(world.robot, arm, grasp.grasp_width)
attachment = get_grasp_attachment(world, arm2, grasp)
attachment.assign()
if arm == arm2: # The moving arm
if obj in contains_liquid:
holding_water = obj
water_attachment = attachment
attachments[obj] = attachment
else: # The stationary arm
obstacles.append(world.get_body(obj))
if not collisions:
obstacles = []
# TODO: dynamically adjust step size to be more conservative near longer movements
arm_joints = get_arm_joints(world.robot, arm)
set_joint_positions(world.robot, arm_joints, conf1)
weights, resolutions = get_weights_resolutions(world.robot, arm)
#print(holding_water, attachments, [get_body_name(body) for body in obstacles])
if teleport:
path = [conf1, conf2]
elif holding_water is None:
# TODO(caelan): unify these two methods
path = plan_joint_motion(world.robot,
arm_joints,
conf2,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
restarts=5,
iterations=50,
smooth=smooth)
else:
reference_pose = (unit_point(), get_liquid_quat(holding_water))
path = plan_water_motion(world.robot,
arm_joints,
conf2,
water_attachment,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
reference_pose=reference_pose,
restarts=7,
iterations=75,
smooth=smooth)
if path is None:
return None
control = Control({
'action':
'move-arm',
#'objects': [],
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments=attachments),
'commands': [
ArmTrajectory(arm, path),
],
})
return (control, )
def gen_fn(arm, bowl_name, bowl_pose, spoon_name, grasp):
if bowl_name == spoon_name:
return
bowl_body = world.get_body(bowl_name)
attachment = get_grasp_attachment(world, arm, grasp)
feature = get_scoop_feature(world, bowl_name, spoon_name)
for parameter in parameter_generator(feature):
spoon_path_bowl = sample_scoop_trajectory(world,
feature,
parameter,
collisions=collisions)
if spoon_path_bowl is None:
continue
for _ in range(max_attempts):
set_gripper_position(world.robot, arm, grasp.grasp_width)
set_pose(bowl_body, bowl_pose)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
spoon_path = [
multiply(bowl_pose, invert(rotate_bowl), spoon_pose_bowl)
for spoon_pose_bowl in spoon_path_bowl
]
#visualize_cartesian_path(world.get_body(spoon_name), spoon_path)
# TODO: pass in tool collision pairs here
arm_path = plan_attachment_motion(
world.robot,
arm,
attachment,
spoon_path,
obstacles=set(obstacles) - {bowl_body},
self_collisions=collisions,
disabled_collisions=disabled_collisions,
collision_buffer=collision_buffer,
attachment_collisions=False)
if arm_path is None:
continue
pre_conf = Conf(arm_path[0])
set_joint_positions(world.robot,
get_arm_joints(world.robot, arm), pre_conf)
attachment.assign()
if pairwise_collision(world.robot, bowl_body):
# TODO: ensure no robot/bowl collisions for the full path
continue
robot_saver = BodySaver(world.robot)
post_conf = Conf(arm_path[-1])
control = Control({
'action':
'scoop',
'objects': [bowl_name, spoon_name],
'feature':
feature,
'parameter':
parameter,
'context':
Context(savers=[robot_saver],
attachments={spoon_name: attachment}),
'commands': [
ArmTrajectory(arm, arm_path, dialation=4.0),
],
})
yield (pre_conf, post_conf, control)
break
else:
yield None