def test(arm1, conf1, arm2, conf2):
# TODO: don't let the arms get too close
if not collisions or (arm1 == arm2):
return False
arm1_joints = get_arm_joints(robot, arm1)
set_joint_positions(robot, arm1_joints, conf1)
arm2_joints = get_arm_joints(robot, arm2)
set_joint_positions(robot, arm2_joints, conf2)
return link_pairs_collision(robot,
get_moving_links(robot,
arm1_joints), robot,
get_moving_links(robot, arm2_joints))
def plan_workspace_motion(robot,
arm,
tool_path,
collision_buffer=COLLISION_BUFFER,
**kwargs):
_, resolutions = get_weights_resolutions(robot, arm)
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
arm_joints = get_arm_joints(robot, arm)
arm_waypoints = plan_cartesian_motion(robot,
arm_joints[0],
tool_link,
tool_path,
pos_tolerance=5e-3)
if arm_waypoints is None:
return None
arm_waypoints = [[conf[j] for j in movable_from_joints(robot, arm_joints)]
for conf in arm_waypoints]
set_joint_positions(robot, arm_joints, arm_waypoints[0])
return plan_waypoints_joint_motion(
robot,
arm_joints,
arm_waypoints[1:],
resolutions=resolutions,
max_distance=collision_buffer,
custom_limits=get_pr2_safety_limits(robot),
**kwargs)
def get_weights_resolutions(robot,
arm,
weights_regularize=0.005,
resolution_scale=0.001):
arm_joints = get_arm_joints(robot, arm)
weights = np.array(
[JOINT_WEIGHTS[i] for i in movable_from_joints(robot, arm_joints)])
weights += weights_regularize * np.ones(weights.shape)
resolutions = np.divide(resolution_scale * np.ones(weights.shape), weights)
return weights, resolutions
def get_pairwise_arm_links(robot, arms):
disabled_collisions = set()
arm_links = {}
for arm in arms:
arm_links[arm] = sorted(
get_moving_links(robot, get_arm_joints(robot, arm)))
#print(arm, [get_link_name(world.robot, link) for link in arm_links[arm]])
for arm1, arm2 in combinations(arm_links, 2):
disabled_collisions.update(product(arm_links[arm1], arm_links[arm2]))
return disabled_collisions
def test(arm1, control, arm2, conf2):
if not collisions or (arm1 == arm2):
return False
attachments = control['context'].assign()
arm1_links = get_moving_links(robot, get_arm_joints(robot, arm1))
arm2_joints = get_arm_joints(robot, arm2)
arm2_links = get_moving_links(robot, arm2_joints)
set_joint_positions(robot, arm2_joints, conf2)
for command in control['commands']:
for _ in command.iterate(world,
attachments): # TODO: randomize sequence
for attachment in attachments.values():
attachment.assign()
if link_pairs_collision(robot, arm2_links,
attachment.child):
return True
if link_pairs_collision(robot, arm1_links, robot, arm2_links):
return True
return False
def test(arm, control, obj, pose):
if not collisions or (obj in control['objects']):
return False
attachments = control['context'].assign()
body = world.bodies[obj]
set_pose(body, pose)
arm_links = get_moving_links(robot, get_arm_joints(robot, arm))
for command in control['commands']:
for _ in command.iterate(world,
attachments): # TODO: randomize sequence
for attachment in attachments.values():
attachment.assign()
if body_pair_collision(attachment.child,
body,
collision_buffer=collision_buffer):
return True
if link_pairs_collision(robot, arm_links, body):
return True
return False
def solve_inverse_kinematics(robot,
arm,
world_pose,
obstacles=[],
collision_buffer=COLLISION_BUFFER,
max_attempts=25):
arm_joints = get_arm_joints(robot, arm)
custom_limits = get_pr2_safety_limits(robot) # TODO: self-collisions
collision_fn = get_collision_fn(robot,
arm_joints,
obstacles=obstacles,
attachments=[],
max_distance=collision_buffer,
self_collisions=False,
disabled_collisions=set(),
custom_limits=custom_limits)
base_pose = get_link_pose(robot,
link_from_name(robot,
BASE_FRAME)) # != get_pose(robot)
target_point, target_quat = multiply(invert(base_pose), world_pose)
[torso_position] = get_group_conf(robot, 'torso')
upper_joint = joint_from_name(robot, UPPER_JOINTS[arm])
lower, upper = get_custom_limits(robot, [upper_joint],
custom_limits,
circular_limits=CIRCULAR_LIMITS)
upper_limits = list(zip(lower, upper))[0]
# TODO(caelan): just attempt one IK sample for each generator to restart quickly
for i, arm_conf in enumerate(
islice(
get_arm_ik_generator(get_arm_prefix(arm),
list(target_point),
list(target_quat),
torso_position,
upper_limits=upper_limits),
max_attempts)):
if not collision_fn(arm_conf):
#print('Attempts:', i)
return Conf(arm_conf)
return None
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, 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
def iterate(self, world, attachments):
joints = get_arm_joints(world.robot, self.arm)
# gripper_joints = get_gripper_joints(robot, arm)
for positions in self.path:
set_joint_positions(world.robot, joints, positions)
yield positions
def ss_from_problem(problem,
bound='shared',
remote=False,
teleport=False,
movable_collisions=False):
robot = problem.robot
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
initial_atoms = [
CanMove(),
IsBConf(initial_bq),
AtBConf(initial_bq),
initialize(TotalCost(), 0),
]
for name in problem.arms:
joints = get_arm_joints(robot, name)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
initial_atoms += [
IsArm(name),
HandEmpty(name),
IsAConf(name, conf),
AtAConf(name, conf)
]
for body in problem.movable:
pose = Pose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose),
POSE(pose)
]
for surface in problem.surfaces:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
initial_atoms += map(Washer, problem.sinks)
initial_atoms += map(Stove, problem.stoves)
initial_atoms += [IsConnected(*pair) for pair in problem.buttons]
initial_atoms += [IsButton(body) for body, _ in problem.buttons]
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
initial_atoms += [IsBConf(goal_conf)]
goal_literals += [AtBConf(goal_conf)]
goal_literals += [Holding(*pair) for pair in problem.goal_holding]
goal_literals += [On(*pair) for pair in problem.goal_on]
goal_literals += map(Cleaned, problem.goal_cleaned)
goal_literals += map(Cooked, problem.goal_cooked)
#GraspedCollision = Predicate([A, G, AT], domain=[IsArm(A), POSE(G), IsArmTraj(AT)],
# fn=lambda a, p, at: False, bound=False)
PlacedCollision = Predicate([AT, P],
domain=[IsArmTraj(AT), POSE(P)],
fn=lambda at, p: False,
bound=False)
actions = [
Action(name='pick',
param=[A, O, P, G, BQ, AT],
pre=[
IsKin(A, O, P, G, BQ, AT),
HandEmpty(A),
AtPose(O, P),
AtBConf(BQ), ~Unsafe(AT)
],
eff=[
HasGrasp(A, O, G),
CanMove(), ~HandEmpty(A), ~AtPose(O, P),
Increase(TotalCost(), 1)
]),
Action(name='place',
param=[A, O, P, G, BQ, AT],
pre=[
IsKin(A, O, P, G, BQ, AT),
HasGrasp(A, O, G),
AtBConf(BQ), ~Unsafe(AT)
],
eff=[
HandEmpty(A),
CanMove(),
AtPose(O, P), ~HasGrasp(A, O, G),
Increase(TotalCost(), 1)
]),
Action(
name='move',
param=[BQ, BQ2, BT],
pre=[IsMotion(BQ, BQ2, BT),
CanMove(),
AtBConf(BQ), ~Unsafe(BT)],
eff=[
AtBConf(BQ2), ~CanMove(), ~AtBConf(BQ),
Increase(TotalCost(), 1)
]),
# TODO: should the robot be allowed to have different carry configs or a defined one?
#Action(name='move_arm', param=[A, BQ, BQ2],
# pre=[IsAConf(A, BQ), IsAConf(A, BQ),
# CanMove(), AtBConf(BQ)],
# eff=[AtAConf(BQ2), ~AtAConf(BQ),
# Increase(TotalCost(), 1)]),
# Why would one want to move to the pick configuration for something anywhere but the goal?
# Stream that generates arm motions as long as one is a shared configuration
# Maybe I should make a base
]
if remote:
actions += [
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Washer(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)]),
]
else:
actions += [
Action(name='press_clean',
param=[O, O2, A, B, BQ, AT],
pre=[
Stackable(O, O2),
Washer(O2),
IsConnected(B, O2),
IsPress(A, B, BQ, AT), ~Cooked(O),
On(O, O2),
HandEmpty(A),
AtBConf(BQ)
],
eff=[Cleaned(O), CanMove()]),
Action(name='press_cook',
param=[O, O2, A, B, BQ, AT],
pre=[
Stackable(O, O2),
Stove(O2),
IsConnected(B, O2),
IsPress(A, B, BQ, AT),
Cleaned(O),
On(O, O2),
HandEmpty(A),
AtBConf(BQ)
],
eff=[Cooked(O), ~Cleaned(O),
CanMove()]),
]
axioms = [
Axiom(param=[A, O, G],
pre=[IsArm(A), IsGrasp(O, G),
HasGrasp(A, O, G)],
eff=Holding(A, 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=[O, P, AT],
pre=[IsPose(O, P),
PlacedCollision(AT, P),
AtPose(O, P)],
eff=Unsafe(AT)),
]
streams = [
FnStream(name='motion',
inp=[BQ, BQ2],
domain=[IsBConf(BQ), IsBConf(BQ2)],
fn=get_motion_gen(problem, teleport=teleport),
out=[BT],
graph=[IsMotion(BQ, BQ2, BT),
IsBaseTraj(BT)],
bound=bound),
ListStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(problem),
out=[G],
graph=[IsGrasp(O, G), GRASP(G)],
bound=bound),
# TODO: test_support
Stream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(problem),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2),
POSE(P)],
bound=bound),
GenStream(
name='ik_ir',
inp=[A, O, P, G],
domain=[IsArm(A), IsPose(O, P),
IsGrasp(O, G)],
fn=get_ik_ir_gen(problem, teleport=teleport),
out=[BQ, AT],
graph=[IsKin(A, O, P, G, BQ, AT),
IsBConf(BQ),
IsArmTraj(AT)],
bound=bound),
GenStream(name='press',
inp=[A, B],
domain=[IsArm(A), IsButton(B)],
fn=get_press_gen(problem, teleport=teleport),
out=[BQ, AT],
graph=[IsPress(A, B, BQ, AT),
IsBConf(BQ),
IsArmTraj(AT)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
def get_initial_and_goal(world):
# TODO: add an object representing the world
task = world.task
constant_map = {
'@{}'.format(material): material
for material in SIM_MATERIALS
}
initial_atoms = list(task.init) + [
('IsPose', world.world_name, world.world_pose),
('AtPose', world.world_name, world.world_pose),
] + [('Material', material) for material in SIM_MATERIALS]
goal_literals = list(task.goal)
for arm in ARMS:
arm_joints = get_arm_joints(world.robot, arm)
arm_conf = Conf(get_joint_positions(world.robot, arm_joints))
constant_map['@{}'.format(arm)] = arm
constant_map['@{}-conf'.format(arm)] = arm_conf
initial_atoms.extend([
('IsConf', arm, arm_conf),
('AtConf', arm, arm_conf),
])
if arm not in world.arms:
continue
initial_atoms.extend([
('IsArm', arm),
('CanMove', arm), # Prevents double moves
])
if arm not in world.initial_grasps:
initial_atoms.append(('Empty', arm))
if task.reset_arms:
goal_literals.append(
('AtConf', arm,
arm_conf)) # Be careful of end orientation constraints
if arm in task.empty_arms:
goal_literals.append(('Empty', arm))
for name, pose in world.initial_poses.items():
initial_atoms.extend([
('IsPose', name, pose),
('AtPose', name, pose),
# TODO: detect which frame is supporting
('IsSupported', name, pose, world.world_name, world.world_pose, TOP
),
])
if in_type_group(name, task.reset_items):
goal_literals.append(('AtPose', name, pose))
for arm, grasp in world.initial_grasps.items():
name = grasp.obj_name
initial_atoms.extend([
('IsGrasp', name, grasp),
('AtGrasp', arm, name, grasp),
])
for name in world.bodies:
initial_atoms.append(('IsType', name, get_type(name)))
for item in world.items:
initial_atoms.extend(get_item_facts(world, item))
for surface in world.bodies: # world.surfaces
if in_type_group(surface, task.pressable):
initial_atoms.append(('Pressable', surface))
if is_obj_type(surface, 'stove'):
initial_atoms.append(('Stove', surface, TOP))
print('Constants:', constant_map)
print('Initial:', initial_atoms)
print('Goal:', goal_literals)
return constant_map, initial_atoms, And(*goal_literals)
