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, )