fixed errors in pid and mpc controller

remote_control/control_commander.py

@@ -8,17 +8,35 @@ from event_listener import EventListener
 
 
 class CommanderBase:
-    def __init__(self):
+    valid_controller_types = {'pid': PIDController,
-        self.robots = []
+                              'mpc': MPCController}
-        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')]
 
-        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())
+        if type(controller_type) == dict:
-        self.robots[1].attach_controller(MPCController())
+            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()

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 or error_ang > 0.2:
-        if error_pos > 0.05:
+        #if error_pos > 0.05:
             # solve mpc open loop problem
             res = self.ols.solve(x_pred, self.target_pos)
 

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)