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.
