Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

general refactoring

simple_control
Simon Pirkelmann 2020-10-24 21:11:22 +02:00
parent 7ded3bee79
commit a0f701fe83
1 changed files with 30 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
remote_control/mpc_controller.py
View File

@ -3,6 +3,7 @@ import time
from casadi_opt import OpenLoopSolver from casadi_opt import OpenLoopSolver
class MPCController: class MPCController:
def __init__(self, estimator): def __init__(self, estimator):
self.t = time.time() self.t = time.time()
@ -30,19 +31,19 @@ class MPCController:
elif event[0] == 'key': elif event[0] == 'key':
key = event[1] key = event[1]
if key == 84: # arrow up if key == 84: # arrow up
self.controlling = True self.controlling = True
self.t = time.time() self.t = time.time()
elif key == 82: # arrow down elif key == 82: # arrow down
self.controlling = False self.controlling = False
robot.send_cmd() robot.send_cmd()
elif key == 48: # 0 elif key == 48: # 0
target_pos = np.array([0.0,0.0,0.0]) target_pos = np.array([0.0, 0.0, 0.0])
elif key == 43: # + elif key == 43: # +
self.control_scaling += 0.1 self.control_scaling += 0.1
self.control_scaling = min(self.control_scaling, 1.0) self.control_scaling = min(self.control_scaling, 1.0)
print("control scaling = ", self.control_scaling) print("control scaling = ", self.control_scaling)
elif key == 45: # - elif key == 45: # -
self.control_scaling -= 0.1 self.control_scaling -= 0.1
self.control_scaling = max(self.control_scaling, 0.1) self.control_scaling = max(self.control_scaling, 0.1)
print("control scaling = ", self.control_scaling) print("control scaling = ", self.control_scaling)
@ -51,7 +52,7 @@ class MPCController:
self.controlling = False self.controlling = False
robot.send_cmd() robot.send_cmd()
return return
elif key == 27: # escape elif key == 27: # escape
print("quit!") print("quit!")
self.controlling = False self.controlling = False
robot.send_cmd() robot.send_cmd()
@ -61,27 +62,26 @@ class MPCController:
if x_pred is not None: if x_pred is not None:
error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2]) error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2])
angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1]) error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
#print("error pos = ", error_pos) # print("error pos = ", error_pos)
#print("error_pos = {}, error_ang = {}".format(error_pos, error_ang)) # print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
#if error_pos > 0.075 or error_ang > 0.35: # if error_pos > 0.075 or error_ang > 0.35:
if error_pos > 0.05 or error_ang > 0.1: if error_pos > 0.05 or error_ang > 0.1:
# solve mpc open loop problem # solve mpc open loop problem
res = self.ols.solve(x_pred, target_pos) res = self.ols.solve(x_pred, target_pos)
#us1 = res[0] # us1 = res[0]
#us2 = res[1] # us2 = res[1]
us1 = res[0] * self.control_scaling us1 = res[0] * self.control_scaling
us2 = res[1] * self.control_scaling us2 = res[1] * self.control_scaling
#print("u = {}", (us1, us2)) # print("u = {}", (us1, us2))
tmpc_end = time.time() # print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
#print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
dt_mpc = time.time() - self.t dt_mpc = time.time() - self.t
if dt_mpc < self.dt: # wait until next control can be applied if dt_mpc < self.dt: # wait until next control can be applied
#print("sleeping for {} seconds...".format(self.dt - dt_mpc)) # print("sleeping for {} seconds...".format(self.dt - dt_mpc))
time.sleep(self.dt - dt_mpc) time.sleep(self.dt - dt_mpc)
else: else:
us1 = [0] * self.mstep us1 = [0] * self.mstep
@ -89,13 +89,13 @@ class MPCController:
near_target += 1 near_target += 1
# send controls to the robot # send controls to the robot
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1 for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
u1 = us1[i] u1 = us1[i]
u2 = us2[i] u2 = us2[i]
robot.send_cmd(u1, u2) robot.send_cmd(u1, u2)
if i < self.mstep: if i < self.mstep:
time.sleep(self.dt) time.sleep(self.dt)
self.t = time.time() # save time the most recent control was applied self.t = time.time() # save time the most recent control was applied
else: else:
print("robot not detected yet!") print("robot not detected yet!")
@ -116,31 +116,31 @@ class MPCController:
elif event[0] == 'key': elif event[0] == 'key':
key = event[1] key = event[1]
if key == 32: # arrow up if key == 32: # arrow up
self.controlling = not self.controlling self.controlling = not self.controlling
if not self.controlling: if not self.controlling:
print("disable control") print("disable control")
robot.send_cmd() # stop robot robot.send_cmd() # stop robot
else: else:
print("enable control") print("enable control")
self.t = time.time() self.t = time.time()
elif key == 48: # 0 elif key == 48: # 0
target_pos = np.array([0.0, 0.0, 0.0]) # TODO: use center of board for target pos target_pos = np.array([0.0, 0.0, 0.0]) # TODO: use center of board for target pos
elif key == 43: # + elif key == 43: # +
self.control_scaling += 0.1 self.control_scaling += 0.1
self.control_scaling = min(self.control_scaling, 1.0) self.control_scaling = min(self.control_scaling, 1.0)
print("control scaling = ", self.control_scaling) print("control scaling = ", self.control_scaling)
elif key == 45: # - elif key == 45: # -
self.control_scaling -= 0.1 self.control_scaling -= 0.1
self.control_scaling = max(self.control_scaling, 0.1) self.control_scaling = max(self.control_scaling, 0.1)
print("control scaling = ", self.control_scaling) print("control scaling = ", self.control_scaling)
elif key == 9: # TAB elif key == 9: # TAB
# switch controlled robot # switch controlled robot
robot.send_cmd() # stop current robot robot.send_cmd() # stop current robot
controlled_robot_number = (controlled_robot_number + 1) % len(robots) controlled_robot_number = (controlled_robot_number + 1) % len(robots)
robot = robots[controlled_robot_number] robot = robots[controlled_robot_number]
print(f"controlled robot: {robot.id}") print(f"controlled robot: {robot.id}")
elif key == 113 or key == 27: # q or ESCAPE elif key == 113 or key == 27: # q or ESCAPE
print("quit!") print("quit!")
self.controlling = False self.controlling = False
robot.send_cmd() robot.send_cmd()
@ -150,7 +150,7 @@ class MPCController:
# measure state # measure state
x_pred = self.get_measurement(robot.id) x_pred = self.get_measurement(robot.id)
#print(np.linalg.norm(x_pred-target_pos)) # print(np.linalg.norm(x_pred-target_pos))
# solve mpc open loop problem # solve mpc open loop problem
res = self.ols.solve(x_pred, target_pos) res = self.ols.solve(x_pred, target_pos)
@ -172,4 +172,4 @@ class MPCController:
self.t = time.time() # save time the most recent control was applied self.t = time.time() # save time the most recent control was applied
def get_measurement(self, robot_id): def get_measurement(self, robot_id):
return self.estimator.get_robot_state_estimate(robot_id) return self.estimator.get_robot_state_estimate(robot_id)