def test(b1, p1, g1, b2, p2): if not collisions or (b1 == b2): return True p2.assign() for _ in iterate_approach_path(problem.robot, 'left', gripper, p1, g1, body=b1): if pairwise_collision(b1, b2) or pairwise_collision(gripper, b2): return False return True
def gen(rover, objective): base_joints = get_base_joints(rover) target_point = get_point(objective) base_generator = visible_base_generator(rover, target_point, base_range) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) attempts = 0 while True: if max_attempts <= attempts: attempts = 0 yield None attempts += 1 base_conf = next(base_generator) if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in obstacles): continue link_pose = get_link_pose(rover, link_from_name(rover, KINECT_FRAME)) if use_cone: mesh, _ = get_detection_cone(rover, objective, camera_link=KINECT_FRAME, depth=max_range) if mesh is None: continue cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) local_pose = Pose() else: distance = get_distance(point_from_pose(link_pose), target_point) if max_range < distance: continue cone = create_cylinder(radius=0.01, height=distance, color=(0, 0, 1, 0.5)) local_pose = Pose(Point(z=distance / 2.)) set_pose(cone, multiply(link_pose, local_pose)) # TODO: colors corresponding to scanned object if any( pairwise_collision(cone, b) for b in obstacles if b != objective and not is_placement(objective, b)): # TODO: ensure that this works for the rover remove_body(cone) continue if not reachable_test(rover, bq): continue print('Visibility attempts:', attempts) y = Ray(cone, rover, objective) yield Output(bq, y)
def gen(o, p): # default_conf = arm_conf(a, g.carry) # joints = get_arm_joints(robot, a) # TODO: check collisions with fixed links target_point = point_from_pose(p.value) base_generator = visible_base_generator(robot, target_point, base_range) while True: for _ in range(max_attempts): set_pose(o, p.value) base_conf = next(base_generator) #set_base_values(robot, base_conf) set_joint_positions(robot, base_joints, base_conf) if any(pairwise_collision(robot, b) for b in fixed): continue head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible continue #bq = Pose(robot, get_pose(robot)) bq = Conf(robot, base_joints, base_conf) hq = Conf(robot, head_joints, head_conf) yield (bq, hq) break else: yield None
def test(traj1, traj2): if not problem.collisions: return True if (robot1 is None) or (robot2 is None): return True if traj1 is traj2: return False if not aabb_overlap(get_turtle_traj_aabb(traj1), get_turtle_traj_aabb(traj2)): return True # TODO: could also use radius to prune # TODO: prune if start and end conf collide for conf1 in randomize(traj1.iterate()): if not aabb_overlap(get_turtle_traj_aabb(conf1), get_turtle_traj_aabb(traj2)): continue set_base_conf(robot1, conf1.values) for conf2 in randomize(traj2.iterate()): if not aabb_overlap(get_turtle_traj_aabb(conf1), get_turtle_traj_aabb(conf2)): continue set_base_conf(robot2, conf2.values) if pairwise_collision(robot1, robot2): return False return True
def test(ray, rover, conf): if not collisions or (rover == ray.start) or (rover == ray.end): return True conf.assign() collision = pairwise_collision(ray.body, rover) #if collision: # wait_for_user() return not collision
def test(c, b2, p2): if not collisions: return True state = c.assign() if b2 in state.attachments: return True p2.assign() for _ in c.apply(state): state.assign() for b1 in state.attachments: if pairwise_collision(b1, b2): #wait_for_user() return False if pairwise_collision(problem.robot, b2): return False # TODO: just check collisions with moving links return True
def test(c, a, b1, g, b2, p2): raise NotImplementedError() if not collisions or (b1 == b2): return True state = c.assign() if (b1 in state.attachments) or (b2 in state.attachments): return True p2.assign() grasp_attachment = g.get_attachment(problem.robot, a) for _ in c.apply(state): state.assign() grasp_attachment.assign() if pairwise_collision(b1, b2): return False if pairwise_collision(problem.robot, b2): return False return True
def fn(robot, conf, body, grasp): conf.assign() link = link_from_name(robot, BASE_LINK) world_from_body = multiply(get_link_pose(robot, link), grasp.value) pose = Pose(body, world_from_body) pose.assign() if any(pairwise_collision(body, obst) for obst in problem.obstacles): return None return (pose, )
def fn(robot, body, pose, grasp): joints = get_base_joints(robot) robot_pose = multiply(pose.value, invert(grasp.value)) base_values = base_values_from_pose(robot_pose) conf = Conf(robot, joints, base_values) conf.assign() if any(pairwise_collision(robot, obst) for obst in problem.obstacles): return None return (conf, )
def fn(robot, body, pose, grasp): # TODO: reverse into the pick or place for differential drive joints = get_base_joints(robot) robot_pose = multiply(pose.value, invert(grasp.value)) base_values = base_values_from_pose(robot_pose) conf = Conf(robot, joints, base_values) conf.assign() if any(pairwise_collision(robot, obst) for obst in problem.obstacles): return None return (conf, )
def test(b1, p1, g1, b2, p2): if not collisions or (b1 == b2): return True p2.assign() grasp_pose = multiply(p1.value, invert(g1.value)) approach_pose = multiply(p1.value, invert(g1.approach), g1.value) for obj_pose in interpolate_poses(grasp_pose, approach_pose): set_pose(b1, obj_pose) if pairwise_collision(b1, b2): return False return True
def check_trajectory_collision(robot, trajectory, bodies): # TODO: each new addition makes collision checking more expensive #offset = 4 movable_joints = get_movable_joints(robot) #for q in trajectory[offset:-offset]: collisions = [False for _ in range(len(bodies))] # TODO: batch collision detection for q in trajectory: set_joint_positions(robot, movable_joints, q) for i, body in enumerate(bodies): if not collisions[i]: collisions[i] |= pairwise_collision(robot, body) return collisions
def gen(rock): base_joints = get_group_joints(rover, 'base') x, y, _ = get_point(rock) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) while True: theta = np.random.uniform(-np.pi, np.pi) base_conf = [x, y, theta] if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in fixed): continue yield (bq, )
def gen(o, p): if o in task.rooms: # TODO: predicate instead return # default_conf = arm_conf(a, g.carry) # joints = get_arm_joints(robot, a) # TODO: check collisions with fixed links target_point = point_from_pose(p.value) base_generator = visible_base_generator(robot, target_point, base_range) while True: set_pose(o, p.value) # p.assign() bq = Conf(robot, base_joints, next(base_generator)) # bq = Pose(robot, get_pose(robot)) bq.assign() if any(pairwise_collision(robot, b) for b in fixed): yield None else: yield (bq, )
def test(r, t, b2, p2): if not problem.collisions: return True p2.assign() state = State() for _ in t.apply(state): state.assign() #for b1 in state.attachments: # if pairwise_collision(b1, b2): # return False if pairwise_collision(r, b2): set_renderer(True) color = get_visual_data(b2)[0].rgbaColor handles = add_line(point_from_conf(t.path[0].values, z=0.02), point_from_conf(t.path[-1].values, z=0.02), color=color) wait_for_user() set_renderer(False) return False return True
def sample_placements(body_surfaces, obstacles=None, min_distances={}): if obstacles is None: obstacles = [ body for body in get_bodies() if body not in body_surfaces ] obstacles = list(obstacles) # TODO: max attempts here for body, surface in body_surfaces.items(): min_distance = min_distances.get(body, 0.01) while True: pose = sample_placement(body, surface) if pose is None: return False if not any( pairwise_collision(body, obst, max_distance=min_distance) for obst in obstacles if obst not in [body, surface]): obstacles.append(body) break return True
def gen(rover, rock): base_joints = get_base_joints(rover) x, y, _ = get_point(rock) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) while True: for _ in range(max_attempts): theta = np.random.uniform(-np.pi, np.pi) base_conf = [x, y, theta] if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in obstacles): continue if not reachable_test(rover, bq): continue yield Output(bq) break else: yield None
def gen(objective): base_joints = get_group_joints(robot, 'base') head_joints = get_group_joints(robot, 'head') target_point = get_point(objective) base_generator = visible_base_generator(robot, target_point, base_range) lower_limits, upper_limits = get_custom_limits(robot, base_joints, custom_limits) while True: base_conf = next(base_generator) if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(robot, base_joints, base_conf) bq.assign() if any(pairwise_collision(robot, b) for b in fixed): continue head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible continue hq = Conf(robot, head_joints, head_conf) y = None yield (bq, hq, y)
def test(r, q, b2, p2): if not problem.collisions: return True q.assign() p2.assign() return not pairwise_collision(r, b2)
def test(b1, p1, b2, p2): if not collisions or (b1 == b2): return True p1.assign() p2.assign() return not pairwise_collision(b1, b2, **kwargs) #, max_distance=0.001)