From f26f958dc7b7694a707a40d3edaa67064b33bfaa Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Sun, 22 Nov 2020 15:57:53 +0100 Subject: [PATCH] fixed errors in pid and mpc controller --- remote_control/control_commander.py | 60 +++++++++++++++++++++-------- remote_control/mpc_controller.py | 8 ++-- remote_control/pid_controller.py | 3 +- 3 files changed, 50 insertions(+), 21 deletions(-) diff --git a/remote_control/control_commander.py b/remote_control/control_commander.py index caea8ff..073ba1e 100644 --- a/remote_control/control_commander.py +++ b/remote_control/control_commander.py @@ -8,17 +8,35 @@ from event_listener import EventListener class CommanderBase: - def __init__(self): - self.robots = [] - self.robots = [ControlledRobot(12, '10.10.11.91'), ControlledRobot(13, '10.10.11.90')] #, Robot(14, '192.168.1.14')] - # self.robots = [ControlledRobot(marker_id=13, ip='10.10.11.90'), - # ControlledRobot(marker_id=14, ip='10.10.11.89')] + valid_controller_types = {'pid': PIDController, + 'mpc': MPCController} - for r in self.robots: + def __init__(self, id_ip_dict, controller_type='pid'): + """ + + :param id_ip_dict: dictionary containing robot marker id and ip of the robot, e.g. { 12: '10.10.11.91' } + :param controller_type: string 'pid', 'mpc'; or dictionary with robot id and controller type string, e.g. { 12: 'mpc', 13: 'pid'} + """ + self.robots = {} + for id, ip in id_ip_dict.items(): + self.robots[id] = ControlledRobot(id, ip) + + for id, r in self.robots.items(): r.connect() - self.robots[0].attach_controller(PIDController()) - self.robots[1].attach_controller(MPCController()) + if type(controller_type) == dict: + for id, ctype in controller_type.items(): + if ctype in CommanderBase.valid_controller_types: + self.robots[id].attach_controller(CommanderBase.valid_controller_types[ctype]()) + else: + raise Exception(f"invalid controller type {ctype} specified for robot {id}. " + f"valid controller types are {list(CommanderBase.valid_controller_types.keys())}") + elif controller_type in CommanderBase.valid_controller_types: + for id, r in self.robots.items(): + r.attach_controller(CommanderBase.valid_controller_types[controller_type]()) + else: + raise Exception(f"invalid controller type {controller_type} specified. valid controller types are " + f"{list(CommanderBase.valid_controller_types.keys())}") self.event_listener = EventListener(event_server=('127.0.0.1', 42424)) @@ -27,14 +45,14 @@ class CommanderBase: self.running = False def run(self): - unconnected_robots = list(filter(lambda r: not r.connected, self.robots)) + unconnected_robots = list(filter(lambda r: not r.connected, self.robots.values())) if len(unconnected_robots) > 0: print(f"warning: could not connect to the following robots: {unconnected_robots}") return all_detected = False while not all_detected: - undetected_robots = list(filter(lambda r: None in r.get_measurement(), self.robots)) + undetected_robots = list(filter(lambda r: None in r.get_measurement(), self.robots.values())) all_detected = len(undetected_robots) == 0 if not all_detected: print(f"warning: no measurements available for the following robots: {undetected_robots}") @@ -53,23 +71,25 @@ class CommanderBase: if event[0] == 'click': target = event[1] target_pos = np.array([target['x'], target['y'], target['angle']]) - self.robots[self.current_robot_index].move_to_pos(target_pos) + controlled_robot_id = list(self.robots.keys())[self.current_robot_index] + self.robots[controlled_robot_id].move_to_pos(target_pos) elif event[0] == 'key': key = event[1] if key == 32: # arrow up self.controlling = not self.controlling if not self.controlling: print("disable control") - for r in self.robots: + for _, r in self.robots.items(): r.stop_control() else: print("enable control") - for r in self.robots: + for _, r in self.robots.items(): r.start_control() elif key == 9: # TAB # switch controlled robot self.current_robot_index = (self.current_robot_index + 1) % len(self.robots) - robot = self.robots[self.current_robot_index] + controlled_robot_id = list(self.robots.keys())[self.current_robot_index] + robot = self.robots[controlled_robot_id] print(f"controlled robot: {robot.id}") elif key == 113 or key == 27: # q or ESCAPE print("quit!") @@ -79,5 +99,15 @@ class CommanderBase: if __name__ == '__main__': - rc = CommanderBase() + id_ip_dict = { + #11: '10.10.11.88', + #12: '10.10.11.91', + #13: '10.10.11.90', + 14: '10.10.11.89', + } + + # controller_type = {12: 'mpc', 13: 'pid'} + controller_type = 'mpc' + + rc = CommanderBase(id_ip_dict, controller_type=controller_type) rc.run() diff --git a/remote_control/mpc_controller.py b/remote_control/mpc_controller.py index 44d1dec..89974d2 100644 --- a/remote_control/mpc_controller.py +++ b/remote_control/mpc_controller.py @@ -18,21 +18,21 @@ class MPCController(ControllerBase): # integrator self.omega_max = 5.0 - self.control_scaling = 0.4 + self.control_scaling = 0.5 def set_target_position(self, target_pos): super(MPCController, self).set_target_position(target_pos) self.t = time.time() def compute_control(self, state): - x_pred = state + x_pred = np.array(state[1:]) 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 error_pos > 0.075 or error_ang > 0.35: - if error_pos > 0.05: + if error_pos > 0.05 or error_ang > 0.2: + #if error_pos > 0.05: # solve mpc open loop problem res = self.ols.solve(x_pred, self.target_pos) diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py index 28417e1..6884472 100644 --- a/remote_control/pid_controller.py +++ b/remote_control/pid_controller.py @@ -112,6 +112,5 @@ class PIDController(ControllerBase): 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) + super(PIDController, self).apply_control(control)