def test_drake_base_motion(pr2, base_start, base_goal, obstacles=[]):
# TODO: combine this with test_arm_motion
"""
Drake's PR2 URDF has explicit base joints
"""
disabled_collisions = get_disabled_collisions(pr2)
base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']]
set_joint_positions(pr2, base_joints, base_start)
base_joints = base_joints[:2]
base_goal = base_goal[:len(base_joints)]
wait_for_user('Plan Base?')
base_path = plan_joint_motion(pr2,
base_joints,
base_goal,
obstacles=obstacles,
disabled_collisions=disabled_collisions)
if base_path is None:
print('Unable to find a base path')
return
print(len(base_path))
for bq in base_path:
set_joint_positions(pr2, base_joints, bq)
if SLEEP is None:
wait_for_user('Continue?')
else:
time.sleep(SLEEP)
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
user_input('Plan Arm?')
arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions)
if arm_path is None:
print('Unable to find an arm path')
return
print(len(arm_path))
for q in arm_path:
set_joint_positions(pr2, left_joints, q)
#raw_input('Continue?')
time.sleep(0.01)
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
wait_if_gui('Plan Arm?')
with LockRenderer(lock=False):
arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions)
if arm_path is None:
print('Unable to find an arm path')
return
print(len(arm_path))
for q in arm_path:
set_joint_positions(pr2, left_joints, q)
#wait_if_gui('Continue?')
wait_for_duration(0.01)
def test_drake_base_motion(pr2, base_start, base_goal):
# TODO: combine this with test_arm_motion
"""
Drake's PR2 URDF has explicit base joints
"""
disabled_collisions = get_disabled_collisions(pr2)
base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']]
set_joint_positions(pr2, base_joints, base_start)
user_input('Plan Base?')
base_path = plan_joint_motion(pr2, base_joints, base_goal, disabled_collisions=disabled_collisions)
if base_path is None:
print('Unable to find a base path')
return
print(len(base_path))
for bq in base_path:
set_joint_positions(pr2, base_joints, bq)
user_input('Continue?')
def get_push_gen_fn(world, max_samples=25, max_attempts=10, collisions=True, parameter_fns={}, repeat=False):
# TODO(caelan): could also simulate the predicated sample
# TODO(caelan): make final the orientation be aligned with gripper
parameter_fn = parameter_fns.get(get_push_gen_fn, PUSH_PARAMETER_FNS[DESIGNED])
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacles = [world.bodies[obst] for obst in world.get_fixed()] if collisions else []
backoff_distance = 0.03
approach_tform = Pose(point=np.array([-0.1, 0, 0])) # Tool coordinates
push_goal_gen_fn = get_push_goal_gen_fn(world)
surface = world.get_table() # TODO: region?
surface_body = world.bodies[surface]
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
return gen_fn
def get_pour_gen_fn(world,
max_attempts=100,
collisions=True,
parameter_fns={}):
# TODO(caelan): could also simulate the predicated sample
# TODO(caelan): be careful with the collision_buffer
parameter_fn = parameter_fns.get(get_pour_gen_fn,
POUR_PARAMETER_FNS[DESIGNED])
parameter_generator = get_parameter_generator(world, parameter_fn,
is_valid_pour)
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacles = [world.get_body(surface)
for surface in world.get_fixed()] if collisions else []
#world.water_body = load_pybullet(get_body_urdf('red_sphere')) # red_sphere, blue_sphere
world.water_body = create_sphere(radius=0.005, color=(1, 0, 0, 1))
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
return gen_fn
def get_motion_fn(world, collisions=True, teleport=False):
# TODO(caelan): could also also plan w/o arm_confs
disabled_collisions = get_disabled_collisions(world.robot)
custom_limits = get_pr2_safety_limits(world.robot)
smooth = 100
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, )
return gen_fn
def get_scoop_gen_fn(world,
max_attempts=100,
collisions=True,
parameter_fns={}):
parameter_fn = parameter_fns.get(get_scoop_gen_fn,
SCOOP_PARAMETER_FNS[DESIGNED])
parameter_generator = get_parameter_generator(world, parameter_fn,
is_valid_scoop)
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacles = list(map(world.get_body,
world.get_fixed())) if collisions else []
collision_buffer = 0.0
# TODO: sometimes the bowls are fixed but sometimes they are not
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
return gen_fn
def get_stir_gen_fn(world,
max_attempts=25,
collisions=True,
parameter_fns={},
revisit=False):
parameter_fn = parameter_fns.get(get_stir_gen_fn,
STIR_PARAMETER_FNS[DESIGNED])
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacles = [world.bodies[surface]
for surface in world.get_fixed()] if collisions else []
collision_buffer = 0.0
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)
# TODO: continue exploration
return gen_fn
def get_pick_gen_fn(world, max_attempts=25, collisions=True):
# TODO(caelan): check object/end-effector path collisions
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacles = [world.bodies[surface]
for surface in world.get_fixed()] if collisions else []
grasp_gen_fn = get_grasp_gen_fn(world)
#def gen_fn(arm, obj_name, pose, grasp):
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)
# TODO: continue exploration
return gen_fn
def get_press_gen_fn(world, max_attempts=25, collisions=True):
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacle_bodies = [world.bodies[surface]
for surface in world.get_fixed()] if collisions else []
pre_direction = 0.15 * get_unit_vector([-1, 0, 0])
collision_buffer = 0.0 # Because of contact with the table
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)
# TODO: continue exploration
return gen_fn
