def init(cls) -> None: PowerMgmt.register_management_change_callback(cls._set_power_save_timeouts) cls._set_power_save_timeouts(PowerMgmt.get_plan()) # to be set defaults Planner.plan(cls._check_time_to_power_save, True) Logging.add_logger(BleLogger()) cls._start_ble() #to be allocated big blocks in the beginning it should prevent memory fragmentation
def plan(): map_ = load_map(MAP_PATH) coordinates = load_coordinates(COORDINATE_PATH) image = captureImage() if image is None: return jsonify({"error": "image is None"}) is_empty_map = getEmptySpaces(image, coordinates) print(is_empty_map) for tile in map_: if not is_empty_map.get((tile.y, tile.x), True): tile.is_temporarily_blocked = True for tile in map_: print("~~~ R%dC%d %s" % (tile.y, tile.x, tile.is_temporarily_blocked)) robot, cars = parse_args(request.args) p = Planner() plan = p.plan(tiles=map_, cars=cars, robot=robot) return jsonify({ "plan": plan, "is_empty": {str(k): bool(v) for k, v in is_empty_map.items()} })
def plan_view(): map_ = load_map(MAP_PATH) coordinates = load_coordinates(COORDINATE_PATH) image = captureImage() if image is None: return jsonify({ "error": "image is None" }) is_empty_map = getEmptySpaces(image, coordinates, THRESHOLD) for tile in map_: if not is_empty_map.get((tile.row, tile.column), True): tile.is_temporarily_blocked = True robot, cars = parse_args(request.args) p = Planner() plan = p.plan( tiles=map_, cars=cars, robot=robot ) return jsonify({ "plan": plan, "is_empty": {f"R{k[0]}C{k[1]}": bool(v) for k, v in is_empty_map.items()} })
def main(): p = Planner() plan = p.plan( None, cars=[Car(0, 3, Car.CarStatus.AWAITING_PARKING)], robot=Robot(2, 0) ) print("PLAN") for step in plan: print(f"\t{step}")
class PlanningScene(object): """ Planning scene containing the environment configuration, planner and trajectory. """ def __init__(self, robot, cfg): self.trajectory = Trajectory(cfg.timesteps) self.env = Environment(robot, cfg) self.planner = Planner(self.env, self.trajectory) def update_planner(self): self.planner.update() def reset(self): """ Resets scene for the next plan """ self.planner = Planner(self.env, self.trajectory) def plan(self): """ Run an optimization step """ plan = self.planner.plan(self.trajectory) return plan
def log(cls, level, message): Planner.plan(cls._log_to_ble, bytes([level]) + message)
def process_input(cls, key_name: str = None, scan_code: bytes = None): logging.info("key:%s", key_name) for callback in cls.callbacks: if callback.trigger in (key_name, scan_code): Planner.plan(callback.callback_type, *callback.args, **callback.kwargs)
def test_enumeration_planner(): tree = RigidBodyTree() AddModelInstanceFromUrdfFile("resources/cartpole_disturbance.urdf", FloatingBaseType.kFixed, None, tree) AddModelInstanceFromUrdfFile("resources/wall.urdf", FloatingBaseType.kFixed, None, tree) plant = RigidBodyPlant(tree) context = plant.CreateDefaultContext() #d = 1. d = 2. #d = 5. #d = 10. #d = 0. ''' input_weight = 1. goal_weight = 30. goal_x = 1. def running_cost(x, u, t): return input_weight * (u[0]**2 + u[1]**2) + goal_weight * (x[0] - goal_x)**2 def final_cost(x, u, t): return 0. ''' input_weight = 1. pole_upright_weight = 5. #goal_weight = 50. goal_weight = 30. goal_x = 1. def running_cost(x, u, t): return input_weight * (u[0]**2 + u[1]**2) + pole_upright_weight * x[1]**2 def final_cost(x, u, t): return goal_weight * (x[0] - goal_x)**2 def final_state_constraint(x): pole_length = 0.5 wall_x = 2. return x[0] + pole_length * sym.sin(x[1]) - wall_x signed_dist_funcs = init_signed_dist_funcs() print("Disturbance is {} N".format(d)) planner = Planner(plant, context, running_cost, final_cost, signed_dist_funcs, final_state_constraint) initial_state = (0., 0., 0., 0.) #tmin = 0.5 tmin = 4. T = 8. #dt = 0.1 #dc = 0.1 dt = 0.5 dc = 0.35 x_traj_nc, x_traj_c = planner.plan(initial_state, tmin, T, dt, dc, d) vis = PlanarRigidBodyVisualizer(tree, xlim=[-2.5, 2.5], ylim=[-1, 2.5]) ani = vis.animate(x_traj_nc, repeat=True) ani.save('disturbance_{}N.mp4'.format(d)) plt.show()
self.rotated = pygame.transform.rotozoom(self.img, degrees(self.theta), 1) self.rect = self.rotated.get_rect(center=(self.x, self.y)) planner = Planner() navigator = Navigator() _map = build_map() start = (50, 480) goal = (50, 330) current_position = (start[0], start[1], 0) planner.update_map(_map) planner.update_position(current_position) planner.update_goal((goal[0], goal[1])) planner.plan() print(planner.planned) positioner = Position() positioner.update_plan(planner.planned) pygame.init() dims = (500, 500) running = True environment = Envir(dims) robot = Robot(start, r"D:\Robot\navigation_upgrade\test\robot.png", 0.01 * M2P) dt = 0 lasttime = pygame.time.get_ticks() while running: for event in pygame.event.get():
import numpy as np import matplotlib.pyplot as plt from motion_domain_2d import motion_domain_2d from planner import Planner env = MotionDomain2d() params_path = 'mcts_parameters.yaml' planner = Planner(env, params_path) rng = Planner.rmcts.rng state, action = None, 0 r_cum = 0 while planner.risk_handler.get_residual_steps() > 0 and action is not None: # env.plot(curr_state) action = Planner.plan(state) state, r = dynamics(state, action, rng) r_cum += r result = 'Failure' if action is None else 'Success' print(result + ', total reward = {}'.format(r_sum))
import cv2 import yaml from analyzer import Analyzer from planner import Planner from helper_functions import newest_frame # author: Hendrik Werner s4549775 with open("config.yml") as conf_file: conf = yaml.safe_load(conf_file) analyzer = Analyzer( conf["ball"], conf["blue_car"], conf["red_car"], ) planner = Planner( analyzer, conf["car_length"], ) webcam = cv2.VideoCapture(conf["capture_device"]) while cv2.waitKey(1) != 27: success, image = newest_frame(webcam) if success: imageHSV = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) cv2.imshow("Webcam", image) planner.plan(imageHSV)
from maze import Maze m = Maze("safety-gym-maze-1") m.plot() from planner import Planner p = Planner() p.plan("safety-gym-maze-1")
#! /usr/bin/env python3 # -*- coding: utf-8 -*- from generator import Generator from planner import Planner if __name__ == '__main__': planner = Planner() generator = Generator() while True: hints = input("Type in hints >> ") keywords = planner.plan(hints) print("Keywords: " + ' '.join(keywords)) poem = generator.generate(keywords) print("Poem generated:") for sentence in poem: print(sentence)
lf.step() to_right = False # Depo arrival. if p.state[1] == "D" and len(p.ignorations) == 0: print("Depo arrival") dn.start() while True: dn.search() if bd.brickAhead(True): break dn.exit() # Go on. p.plan("D" + bd.result) lf.left = p.left # Detect bricks. elif p.state[1] in ["L", "R"]: if bd.brickAhead(False): # Plan destination. p.plan(p.state[1] + bd.result) # Turn around. lf.turn(1 if p.left else - 1, 250) # Set side. lf.left = p.left # Last mile detection.