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 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 plan_workspace(world,
tool_path,
obstacles,
randomize=True,
teleport=False):
# Assuming that pairs of fixed things aren't in collision at this point
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
distance_fn = get_distance_fn(world.robot, world.arm_joints)
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()
arm_path = []
for i, tool_pose in enumerate(tool_path):
#set_joint_positions(world.kitchen, [door_joint], door_path[i])
tolerance = INF if i == 0 else NEARBY_PULL
full_arm_conf = world.solve_inverse_kinematics(
tool_pose, nearby_tolerance=tolerance)
if full_arm_conf is None:
# TODO: this fails when teleport=True
if PRINT_FAILURES: print('Workspace kinematic failure')
return None
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Workspace collision failure')
return None
arm_conf = get_joint_positions(world.robot, world.arm_joints)
if arm_path and not teleport:
distance = distance_fn(arm_path[-1], arm_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print(
'Workspace proximity failure (distance={:.5f})'.format(
distance))
return None
arm_path.append(arm_conf)
# wait_for_user()
return arm_path
def bodies(self):
return flatten_links(self.robot, get_moving_links(self.robot, self.robot_joints)) | \
flatten_links(self.world.kitchen, get_moving_links(self.world.kitchen, self.door_joints))
def bodies(self):
# TODO: decompose into dependents and moving?
return flatten_links(self.robot,
get_moving_links(self.robot, self.joints))
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_approach(world,
approach_pose,
attachments=[],
obstacles=set(),
teleport=False,
switches_only=False,
approach_path=not MOVE_ARM,
**kwargs):
# TODO: use velocities in the distance function
distance_fn = get_distance_fn(world.robot, world.arm_joints)
aq = world.carry_conf
grasp_conf = get_joint_positions(world.robot, world.arm_joints)
if switches_only:
return [aq.values, grasp_conf]
# TODO: could extract out collision function
# TODO: track the full approach motion
full_approach_conf = world.solve_inverse_kinematics(
approach_pose, nearby_tolerance=NEARBY_APPROACH)
if full_approach_conf is None: # TODO: | {obj}
if PRINT_FAILURES: print('Pregrasp kinematic failure')
return None
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
#robot_obstacle = world.robot
if any(pairwise_collision(robot_obstacle, b)
for b in obstacles): # TODO: | {obj}
if PRINT_FAILURES: print('Pregrasp collision failure')
return None
approach_conf = get_joint_positions(world.robot, world.arm_joints)
if teleport:
return [aq.values, approach_conf, grasp_conf]
distance = distance_fn(grasp_conf, approach_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print('Pregrasp proximity failure (distance={:.5f})'.format(
distance))
return None
resolutions = ARM_RESOLUTION * np.ones(len(world.arm_joints))
grasp_path = plan_direct_joint_motion(
world.robot,
world.arm_joints,
grasp_conf,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions / 4.)
if grasp_path is None:
if PRINT_FAILURES: print('Pregrasp path failure')
return None
if not approach_path:
return grasp_path
# TODO: plan one with attachment placed and one held
# TODO: can still use this as a witness that the conf is reachable
aq.assign()
approach_path = plan_joint_motion(
world.robot,
world.arm_joints,
approach_conf,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions,
restarts=2,
iterations=25,
smooth=25)
if approach_path is None:
if PRINT_FAILURES: print('Approach path failure')
return None
return approach_path + grasp_path
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,
)
