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

fixed errors in pid and mpc controller

master
Simon Pirkelmann 2020-11-22 15:57:53 +01:00
parent 0fddd75393
commit f26f958dc7
3 changed files with 50 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
remote_control/control_commander.py
View File

@ -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()

8
remote_control/mpc_controller.py
View File

@ -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)

3
remote_control/pid_controller.py
View File

@ -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)