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

restructured controller to work with mpc

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-11-18 21:38:41 +01:00
parent 17637f427b
commit 2f081faee8
3 changed files with 50 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
remote_control/controller.py
View File

@ -18,9 +18,9 @@ class ControllerBase:
control = self.compute_control(state) control = self.compute_control(state)
self.apply_control(control) if self.controlling:
self.apply_control(control)
time.sleep(self.control_rate)
self.apply_control((0.0, 0.0)) # stop robot self.apply_control((0.0, 0.0)) # stop robot
def set_target_position(self, target_pos): def set_target_position(self, target_pos):
@ -42,6 +42,7 @@ class ControllerBase:
else: else:
raise Exception("error: controller cannot apply control!\n" raise Exception("error: controller cannot apply control!\n"
" there is no robot attached to the controller!") " there is no robot attached to the controller!")
time.sleep(self.control_rate)
def attach_robot(self, robot): def attach_robot(self, robot):
self.robot = robot self.robot = robot

195
remote_control/mpc_controller.py
View File

@ -1,175 +1,62 @@
import numpy as np import numpy as np
import time import time
from controller import ControllerBase
from casadi_opt import OpenLoopSolver from casadi_opt import OpenLoopSolver
class MPCController: class MPCController(ControllerBase):
def __init__(self, estimator): def __init__(self):
self.t = time.time() super().__init__()
self.t = None
self.estimator = estimator
self.controlling = False
self.mstep = 2
self.ols = OpenLoopSolver(N=20, T=1.0) self.ols = OpenLoopSolver(N=20, T=1.0)
self.ols.setup() self.ols.setup()
self.dt = self.ols.T / self.ols.N self.control_rate = self.ols.T / self.ols.N
self.mstep = 2
# integrator # integrator
self.omega_max = 5.0 self.omega_max = 5.0
self.control_scaling = 0.4 self.control_scaling = 0.4
def move_to_pos(self, target_pos, robot, near_target_counter=5): def set_target_position(self, target_pos):
near_target = 0 super(MPCController, self).set_target_position(target_pos)
while near_target < near_target_counter: self.t = time.time()
while not self.estimator.event_queue.empty():
event = self.estimator.event_queue.get()
print("event: ", event)
if event[0] == 'click':
pass
elif event[0] == 'key':
key = event[1]
if key == 84: # arrow up def compute_control(self, state):
self.controlling = True x_pred = state
self.t = time.time()
elif key == 82: # arrow down
self.controlling = False
robot.send_cmd()
elif key == 48: # 0
target_pos = np.array([0.0, 0.0, 0.0])
elif key == 43: # +
self.control_scaling += 0.1
self.control_scaling = min(self.control_scaling, 1.0)
print("control scaling = ", self.control_scaling)
elif key == 45: # -
self.control_scaling -= 0.1
self.control_scaling = max(self.control_scaling, 0.1)
print("control scaling = ", self.control_scaling)
elif key == 113:
print("quit!")
self.controlling = False
robot.send_cmd()
return
elif key == 27: # escape
print("quit!")
self.controlling = False
robot.send_cmd()
return
x_pred = self.get_measurement(robot.id) error_pos = np.linalg.norm(x_pred[0:2] - self.target_pos[0:2])
angles_unwrapped = np.unwrap([x_pred[2], self.target_pos[2]]) # unwrap angle to avoid jump in data
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
if x_pred is not None: # if error_pos > 0.075 or error_ang > 0.35:
error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2]) if error_pos > 0.05:
angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data # solve mpc open loop problem
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1]) res = self.ols.solve(x_pred, self.target_pos)
# print("error pos = ", error_pos)
# print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
# if error_pos > 0.075 or error_ang > 0.35: us1 = res[0] * self.control_scaling
if error_pos > 0.05 or error_ang > 0.1: us2 = res[1] * self.control_scaling
# solve mpc open loop problem
res = self.ols.solve(x_pred, target_pos)
# us1 = res[0] dt_mpc = time.time() - self.t
# us2 = res[1] if dt_mpc < self.control_rate: # wait until next control can be applied
us1 = res[0] * self.control_scaling time.sleep(self.control_rate - dt_mpc)
us2 = res[1] * self.control_scaling else:
# print("u = {}", (us1, us2)) us1 = [0] * self.mstep
us2 = [0] * self.mstep
# print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start)) return us1, us2
dt_mpc = time.time() - self.t
if dt_mpc < self.dt: # wait until next control can be applied
# print("sleeping for {} seconds...".format(self.dt - dt_mpc))
time.sleep(self.dt - dt_mpc)
else:
us1 = [0] * self.mstep
us2 = [0] * self.mstep
near_target += 1
# send controls to the robot def apply_control(self, control):
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1 if self.robot is not None:
u1 = us1[i] for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
u2 = us2[i] u1 = control[0][i]
robot.send_cmd(u1, u2) u2 = control[1][i]
if i < self.mstep: self.robot.send_cmd(u1, u2)
time.sleep(self.dt) if i < self.mstep:
self.t = time.time() # save time the most recent control was applied time.sleep(self.control_rate)
else: self.t = time.time() # save time the most recent control was applied
print("robot not detected yet!") else:
raise Exception("error: controller cannot apply control!\n"
def interactive_control(self, robots): " there is no robot attached to the controller!")
controlled_robot_number = 0
robot = robots[controlled_robot_number]
target_pos = np.array([0.0, 0.0, 0.0])
running = True
while running:
# handle events from opencv window
while not self.estimator.event_queue.empty():
event = self.estimator.event_queue.get()
print("event: ", event)
if event[0] == 'click':
target_pos = event[1]
elif event[0] == 'key':
key = event[1]
if key == 32: # arrow up
self.controlling = not self.controlling
if not self.controlling:
print("disable control")
robot.send_cmd() # stop robot
else:
print("enable control")
self.t = time.time()
elif key == 48: # 0
target_pos = np.array([0.0, 0.0, 0.0]) # TODO: use center of board for target pos
elif key == 43: # +
self.control_scaling += 0.1
self.control_scaling = min(self.control_scaling, 1.0)
print("control scaling = ", self.control_scaling)
elif key == 45: # -
self.control_scaling -= 0.1
self.control_scaling = max(self.control_scaling, 0.1)
print("control scaling = ", self.control_scaling)
elif key == 9: # TAB
# switch controlled robot
robot.send_cmd() # stop current robot
controlled_robot_number = (controlled_robot_number + 1) % len(robots)
robot = robots[controlled_robot_number]
print(f"controlled robot: {robot.id}")
elif key == 113 or key == 27: # q or ESCAPE
print("quit!")
self.controlling = False
robot.send_cmd()
return
if self.controlling:
# measure state
x_pred = self.get_measurement(robot.id)
# print(np.linalg.norm(x_pred-target_pos))
# solve mpc open loop problem
res = self.ols.solve(x_pred, target_pos)
us1 = res[0] * self.control_scaling
us2 = res[1] * self.control_scaling
dt_mpc = time.time() - self.t
if dt_mpc < self.dt: # wait until next control can be applied
time.sleep(self.dt - dt_mpc)
# send controls to the robot
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
u1 = us1[i]
u2 = us2[i]
robot.send_cmd(u1, u2)
if i < self.mstep:
time.sleep(self.dt)
self.t = time.time() # save time the most recent control was applied
def get_measurement(self, robot_id):
return self.estimator.get_robot_state_estimate(robot_id)

11
remote_control/pid_controller.py
View File

@ -10,8 +10,6 @@ class PIDController(ControllerBase):
super().__init__() super().__init__()
self.t = None self.t = None
self.controlling = False
self.P_angle = 0.4 self.P_angle = 0.4
self.I_angle = 0.35 self.I_angle = 0.35
self.D_angle = 0.1 self.D_angle = 0.1
@ -46,7 +44,7 @@ class PIDController(ControllerBase):
if self.t is None: if self.t is None:
dt = 0.1 dt = 0.1
else: else:
dt = time.time() - self.t dt = time.time() - self.t # time since last control was applied
if self.mode == 'angle': if self.mode == 'angle':
# compute angle such that robot faces to target point # compute angle such that robot faces to target point
@ -108,9 +106,12 @@ class PIDController(ControllerBase):
self.e_pos_old = e_pos self.e_pos_old = e_pos
self.e_angle_old = e_angle self.e_angle_old = e_angle
else: else:
u1 = 0.0 u1 = 0.0
u2 = 0.0 u2 = 0.0
self.t = time.time() # save time when the most recent control was applied
return u1, u2 return u1, u2
def apply_control(self, control):
super(PIDController, self).apply_control(control)
self.t = time.time() # save time when the most recent control was applied
time.sleep(self.control_rate)