def plan_head_traj(robot, head_conf): # head_conf = get_joint_positions(robot, head_joints) # head_path = [head_conf, hq.values] head_joints = get_group_joints(robot, 'head') head_path = plan_direct_joint_motion(robot, head_joints, head_conf, obstacles=None, self_collisions=False) assert (head_path is not None) return create_trajectory(robot, head_joints, head_path)
def plan_head_traj(task, head_conf): robot = task.robot obstacles = get_fixed_bodies(task) # TODO: movable objects # head_conf = get_joint_positions(robot, head_joints) # head_path = [head_conf, hq.values] head_joints = get_group_joints(robot, 'head') head_path = plan_direct_joint_motion(robot, head_joints, head_conf, obstacles=obstacles, self_collisions=SELF_COLLISIONS) assert (head_path is not None) return create_trajectory(robot, head_joints, head_path)
def test(hq1, hq2): if teleport: ht = Trajectory([hq1, hq2]) else: hq1.assign() path = plan_direct_joint_motion(rover, hq1.joints, hq2.values, self_collisions=False) if path is None: return None ht = create_trajectory(rover, hq1.joints, path) return (ht, )
def test(rover, q1, q2, fluents=[]): if teleport: ht = Trajectory([q1, q2]) return Output(ht) base_link = child_link_from_joint(q1.joints[-1]) q1.assign() attachments = [] movable = set() for fluent in fluents: predicate, args = fluent[0], fluent[1:] if predicate == 'AtGrasp'.lower(): r, b, g = args attachments.append(Attachment(rover, base_link, g.value, b)) elif predicate == 'AtPose'.lower(): b, p = args assert b not in movable p.assign() movable.add(b) # elif predicate == 'AtConf'.lower(): # continue else: raise NotImplementedError(predicate) obstacles = set(problem.fixed) | movable if collisions else [] q1.assign() if holonomic: path = plan_joint_motion(rover, q1.joints, q2.values, custom_limits=custom_limits, attachments=attachments, obstacles=obstacles, self_collisions=False, **kwargs) else: path = plan_nonholonomic_motion(rover, q1.joints, q2.values, reversible=reversible, custom_limits=custom_limits, attachments=attachments, obstacles=obstacles, self_collisions=False, **kwargs) if path is None: return None ht = create_trajectory(rover, q2.joints, path) return Output(ht)
def test(bq1, bq2): if teleport: ht = Trajectory([bq1, bq2]) else: bq1.assign() path = plan_joint_motion(rover, bq1.joints, bq2.values, custom_limits=custom_limits, obstacles=fixed, self_collisions=False) if path is None: return None ht = create_trajectory(rover, bq2.joints, path) return (ht, )
def test(rover, q1, q2): if teleport: ht = Trajectory([q1, q2]) else: q1.assign() path = plan_nonholonomic_motion(rover, q1.joints, q2.values, custom_limits=custom_limits, obstacles=obstacles, self_collisions=False, **kwargs) if path is None: return None ht = create_trajectory(rover, q2.joints, path) return (ht, )
def fn(body, q1, q2): joints = get_base_joints(body) distance_fn = get_base_distance_fn(weights) extend_fn = get_extend_fn(body, joints, resolutions=resolutions) collision_fn = get_collision_fn(body, joints, problem.obstacles, attachments=[], self_collisions=False, disabled_collisions=set(), custom_limits=problem.custom_limits, max_distance=MAX_DISTANCE) # TODO: degree distance = distance_fn(q1.values, q2.values) if max_distance < distance: return None path = [q1.values] + list(extend_fn(q1.values, q2.values)) if any(map(collision_fn, path)): return None handles = add_line(point_from_conf(q1.values), point_from_conf(q2.values)) t = create_trajectory(body, joints, path) return (t,)
def move_look_trajectory(task, trajectory, max_tilt=np.pi / 6): # max_tilt=INF): # TODO: implement a minimum distance instead of max_tilt # TODO: pr2 movement restrictions #base_path = [pose.to_base_conf() for pose in trajectory.path] base_path = trajectory.path if not base_path: return trajectory obstacles = get_fixed_bodies(task) # TODO: movable objects robot = base_path[0].body target_path = get_target_path(trajectory) waypoints = [] index = 0 with BodySaver(robot): #current_conf = base_values_from_pose(get_pose(robot)) for i, conf in enumerate(base_path): # TODO: just do two loops? conf.assign() while index < len(target_path): if i < index: # Don't look at past or current conf target_point = target_path[index] head_conf = inverse_visibility( robot, target_point) # TODO: this is slightly slow #print(index, target_point, head_conf) if (head_conf is not None) and (head_conf[1] < max_tilt): break index += 1 else: head_conf = get_group_conf(robot, 'head') set_group_conf(robot, 'head', head_conf) #print(i, index, conf.values, head_conf) #, get_pose(robot)) waypoints.append(np.concatenate([conf.values, head_conf])) joints = tuple(base_path[0].joints) + tuple(get_group_joints( robot, 'head')) #joints = get_group_joints(robot, 'base') + get_group_joints(robot, 'head') #set_pose(robot, unit_pose()) #set_group_conf(robot, 'base', current_conf) path = plan_waypoints_joint_motion(robot, joints, waypoints, obstacles=obstacles, self_collisions=SELF_COLLISIONS) return create_trajectory(robot, joints, path)
def gen(o, p): if o in task.rooms: #bq = Pose(robot, unit_pose()) #bq = state.poses[robot] bq = Conf(robot, base_joints, np.zeros(len(base_joints))) #hq = Conf(robot, head_joints, np.zeros(len(head_joints))) #ht = create_trajectory(robot, head_joints, plan_pause_scan_path(robot, tilt=tilt)) waypoints = plan_scan_path(task.robot, tilt=tilt) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) ht = create_trajectory(robot, head_joints, path) yield bq, ht.path[0], ht else: for bq, hq in vis_gen(o, p): ht = Trajectory([hq]) yield bq, ht.path[0], ht
def inspect_trajectory(trajectory): if not trajectory.path: return robot = trajectory.path[0].body # TODO: minimum distance of some sort (to prevent from looking at the bottom) # TODO: custom lower limit as well head_waypoints = [] for target_point in get_target_path(trajectory): head_conf = inverse_visibility(robot, target_point) # TODO: could also draw the sequence of inspected points as the head moves if head_conf is None: continue head_waypoints.append(head_conf) head_joints = get_group_joints(robot, 'head') #return create_trajectory(robot, head_joints, head_waypoints) head_path = plan_waypoints_joint_motion(robot, head_joints, head_waypoints, obstacles=None, self_collisions=False) assert (head_path is not None) return create_trajectory(robot, head_joints, head_path)
def fn(o, p, bq): set_pose(o, p.value) # p.assign() bq.assign() if o in task.rooms: waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) if path is None: return None ht = create_trajectory(robot, head_joints, path) hq = ht.path[0] else: target_point = point_from_pose(p.value) head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible return None hq = Conf(robot, head_joints, head_conf) ht = Trajectory([hq]) return (hq, ht)
def linear_trajectory(conf1, conf2): robot = conf1.body joints = conf1.joints extend_fn = get_extend_fn(robot, joints, resolutions=BASE_RESOLUTIONS) path = [conf1.values] + list(extend_fn(conf1.values, conf2.values)) return create_trajectory(robot, joints, path)