From e93ae65e0fa5b7aa4b5a726e0c952796479e7c24 Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Sun, 18 Oct 2020 18:03:33 +0200 Subject: [PATCH] added option to dynamically initialize grid and robots and added support for arbitrary grid orientation w.r.t. camera --- remote_control/mpc_controller.py | 71 +++++----- remote_control/opencv_viewer_example.py | 13 +- remote_control/roborally.py | 176 +++++++++++++++--------- remote_control/robot.py | 6 +- 4 files changed, 158 insertions(+), 108 deletions(-) diff --git a/remote_control/mpc_controller.py b/remote_control/mpc_controller.py index aa8435c..16d163e 100644 --- a/remote_control/mpc_controller.py +++ b/remote_control/mpc_controller.py @@ -17,7 +17,7 @@ class MPCController: # integrator self.omega_max = 5.0 - self.control_scaling = 0.2 + self.control_scaling = 0.4 def move_to_pos(self, target_pos, robot, near_target_counter=5): near_target = 0 @@ -59,42 +59,45 @@ class MPCController: x_pred = self.get_measurement(robot.id) - 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 - error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1]) - #print("error pos = ", error_pos) - #print("error_pos = {}, error_ang = {}".format(error_pos, error_ang)) + if x_pred is not None: + 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 + error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1]) + #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: - if error_pos > 0.05 or error_ang > 0.2: - # solve mpc open loop problem - res = self.ols.solve(x_pred, target_pos) + #if error_pos > 0.075 or error_ang > 0.35: + if error_pos > 0.05 or error_ang > 0.1: + # solve mpc open loop problem + res = self.ols.solve(x_pred, target_pos) - #us1 = res[0] - #us2 = res[1] - us1 = res[0] * self.control_scaling - us2 = res[1] * self.control_scaling - #print("u = {}", (us1, us2)) + #us1 = res[0] + #us2 = res[1] + us1 = res[0] * self.control_scaling + us2 = res[1] * self.control_scaling + #print("u = {}", (us1, us2)) - tmpc_end = time.time() - #print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start)) - 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) + tmpc_end = time.time() + #print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start)) + 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 + 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 else: - us1 = [0] * self.mstep - us2 = [0] * self.mstep - near_target += 1 - - # 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 + print("robot not detected yet!") def interactive_control(self, robots): controlled_robot_number = 0 @@ -169,4 +172,4 @@ class MPCController: self.t = time.time() # save time the most recent control was applied def get_measurement(self, robot_id): - return np.array(self.estimator.get_robot_state_estimate(robot_id)) \ No newline at end of file + return self.estimator.get_robot_state_estimate(robot_id) \ No newline at end of file diff --git a/remote_control/opencv_viewer_example.py b/remote_control/opencv_viewer_example.py index 510e379..aea563d 100644 --- a/remote_control/opencv_viewer_example.py +++ b/remote_control/opencv_viewer_example.py @@ -6,8 +6,6 @@ from shapely.geometry import LineString from queue import Queue class ArucoEstimator: - grid_columns = 10 - grid_rows = 8 corner_marker_ids = { 'a': 0, 'b': 1, @@ -24,7 +22,10 @@ class ArucoEstimator: 'd': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, } - def __init__(self, robot_marker_ids=None, use_realsense=True): + def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8): + self.grid_columns = grid_columns + self.grid_rows = grid_rows + if robot_marker_ids is None: robot_marker_ids = [] self.robot_marker_ids = robot_marker_ids @@ -248,7 +249,7 @@ class ArucoEstimator: elif orientation == 'v': angle = x_frac * angle_ad + (1 - x_frac) * angle_bc elif orientation == '^': - angle = - (x_frac * angle_ad + (1 - x_frac) * angle_bc) + angle = x_frac * angle_ad + (1 - x_frac) * angle_bc + np.pi return np.array((point_of_intersection[0], point_of_intersection[1], angle)) else: @@ -315,11 +316,13 @@ class ArucoEstimator: def get_robot_state_estimate(self, id): if id in self.robot_marker_estimates: if self.robot_marker_estimates[id] is not None: - return self.robot_marker_estimates[id] + return np.array(self.robot_marker_estimates[id]) else: print(f"error: no estimate available for robot {id}") + return None else: print(f"error: invalid robot id {id}") + return None def draw_robot_pos(self, frame, corners, ids): # draws information about the robot positions onto the given frame diff --git a/remote_control/roborally.py b/remote_control/roborally.py index 5bd3470..6ada427 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -4,10 +4,12 @@ import threading import time from mpc_controller import MPCController +from robot import Robot import opencv_viewer_example MSGLEN = 64 + def myreceive(sock): chunks = [] bytes_recd = 0 @@ -24,7 +26,7 @@ def myreceive(sock): return b''.join(chunks) -class Robot: +class RoboRallyRobot(Robot): # dictionary mapping the current orientation and a turn command to the resulting orientation resulting_orientation = { '^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'}, @@ -36,94 +38,65 @@ class Robot: # dictionary mapping an orientation to its opposite opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'} - def __init__(self, id, ip, x=0, y=0, orientation='>'): - self.x = x - self.y = y - self.orientation = orientation + def __init__(self, id, ip, x, y, orientation): + super().__init__(id, ip) - self.id = id - - self.pos = None - self.euler = None - - self.ip = ip - self.socket = socket.socket() - - # currently active control - self.u1 = 0.0 - self.u2 = 0.0 + self.grid_x = x + self.grid_y = y + self.grid_orientation = orientation def get_neighbor_coordinates(self, direction): # get the coordinates of the neighboring tile in the given direction if direction == '^': - return (self.x, self.y - 1) + return self.grid_x, self.grid_y - 1 elif direction == '>': - return (self.x + 1, self.y) + return self.grid_x + 1, self.grid_y elif direction == 'v': - return (self.x, self.y + 1) + return self.grid_x, self.grid_y + 1 elif direction == '<': - return (self.x - 1, self.y) + return self.grid_x - 1, self.grid_y else: print("error: unknown direction") sys.exit(1) - def move(self, type): - if type == 'forward': - target_tile = self.get_neighbor_coordinates(self.orientation) - self.x = target_tile[0] - self.y = target_tile[1] - elif type == 'backward': - opposite_orientation = Robot.opposites[self.orientation] + def move(self, move_type): + if move_type == 'forward': + target_tile = self.get_neighbor_coordinates(self.grid_orientation) + self.grid_x = target_tile[0] + self.grid_y = target_tile[1] + elif move_type == 'backward': + opposite_orientation = RoboRallyRobot.opposites[self.grid_orientation] target_tile = self.get_neighbor_coordinates(opposite_orientation) - self.x = target_tile[0] - self.y = target_tile[1] - elif 'turn' in type: - self.orientation = Robot.resulting_orientation[self.orientation][type] + self.grid_x = target_tile[0] + self.grid_y = target_tile[1] + elif 'turn' in move_type: + self.grid_orientation = RoboRallyRobot.resulting_orientation[self.grid_orientation][move_type] + elif 'nop' in move_type: + pass # nop command -> robot grid position does not change (used e.g. for driving the robot to initial + # position) else: print("error: invalid move") sys.exit(1) - def connect(self): - # connect to robot - try: - print("connecting to robot {} with ip {} ...".format(self.id, self.ip)) - #self.socket.connect((self.ip, 1234)) # connect to robot - print("connected!") - except socket.error: - print("could not connect to robot {} with ip {}".format(self.id, self.ip)) - sys.exit(1) - - def send_cmd(self, u1=0.0, u2=0.0): - if self.socket: - try: - self.socket.send(f'({u1},{u2})\n'.encode()) - except BrokenPipeError: - #print(f"error: connection to robot {self.id} with ip {self.ip} lost") - pass - def __str__(self): return self.__repr__() def __repr__(self): - return f"x: {self.x}, y: {self.y}, orienation: {self.orientation}" + return f"grid x: {self.grid_x}, grid y: {self.grid_y}, grid orientation: {self.grid_orientation}" class RemoteController: + valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'nop', 'get position', + 'set position', 'initialize_robot', 'initialize_grid'] + def __init__(self): -# self.robots = #[Robot(11, '192.168.1.11', (6, -3, np.pi)), Robot(12, '192.168.1.12', (6, -3, np.pi)), - # Robot(13, '192.168.1.13', (6, -3, np.pi)), Robot(14, '192.168.1.14', (6, -2, np.pi))] - #self.robots = [Robot(13, '192.168.1.13', (6, -3, np.pi))] - #self.robots = [] - #self.robots = [Robot(11, '192.168.1.11', (6,-3,0)), Robot(14, '192.168.1.14', (6,3,0))] - #self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')] - self.robots = [Robot(12, '192.168.1.12')] + self.robots = [] + self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')] self.robot_ids = {} for r in self.robots: self.robot_ids[r.id] = r - self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'get position', 'set position'] - # socket for movement commands self.comm_socket = socket.socket() self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) @@ -143,6 +116,14 @@ class RemoteController: self.controller = MPCController(self.estimator) + print("waiting for corner and robot detection..") + while not self.estimator.all_robots_detected() or not self.estimator.all_corners_detected(): + pass + print("everything detected!") + # drive robots to initial position + for robot_id in self.robot_ids: + self.grid_control(robot_id, 'nop') + def run(self): print("waiting until all markers are detected...") while not self.estimator.all_corners_detected(): @@ -165,10 +146,28 @@ class RemoteController: cmd = cmd.strip().decode() if len(inputs) > 1: - if cmd in self.valid_cmds: + if cmd in RemoteController.valid_cmds: + if cmd == 'initialize_grid': + try: + grid_columns = int(inputs[1]) + grid_rows = int(inputs[2]) + + self.estimator.grid_columns = grid_columns + self.estimator.grid_rows = grid_rows + clientsocket.sendall(b'OK\n') + except ValueError: + print("could not initialize grid!") + clientsocket.sendall(b'"could not initialize grid!\n' + b'expected: initialize_robot, , ') + except IndexError: + print("could not initialize grid!") + clientsocket.sendall(b'"could not initialize grid!\n' + b'expected: initialize_robot, , ') + else: # robot command try: robot_id = int(inputs[1]) except ValueError: + robot_id = None print("could not read robot id!") clientsocket.sendall(b'Could not read robot id!\n') @@ -178,7 +177,8 @@ class RemoteController: elif cmd == b'set position': try: pos_data = ",".join(inputs[2:]) - new_grid_pos = tuple(map(lambda x: int(x[1]) if x[0] < 2 else float(x[1]), enumerate(pos_data.strip().strip('()').split(',')))) + new_grid_pos = tuple(map(lambda x: int(x[1]) if x[0] < 2 else float(x[1]), + enumerate(pos_data.strip().strip('()').split(',')))) self.robot_ids[robot_id].grid_pos = new_grid_pos clientsocket.sendall(b'OK\n') except IndexError as e: @@ -188,17 +188,58 @@ class RemoteController: self.robot_ids[robot_id].grid_pos))) except ValueError as e: print("could not set grid position!") - clientsocket.sendall(bytes('could not set grid position! (invalid format)\n'.format(self.robot_ids[robot_id].grid_pos))) + clientsocket.sendall(bytes( + 'could not set grid position! (invalid format)\n'.format( + self.robot_ids[robot_id].grid_pos))) else: self.grid_control(robot_id, cmd) clientsocket.sendall(b'OK\n') + elif cmd == 'initialize_robot': + # add a new robot to the game + try: + id = int(inputs[1]) + ip = inputs[2].decode().strip() + x = int(inputs[3]) + y = int(inputs[4]) + orientation = inputs[5].decode().strip() + + print(f"initializing new robot with id {id} and ip {ip} at pos ({x},{y}) with " + f"orientation '{orientation}'") + new_robot = RoboRallyRobot(id=id, ip=ip, x=x, y=y, orientation=orientation) + new_robot.connect() + if new_robot.connected: + print("created new robot and successfully connected to it!") + # store the new robot in the list of robots + self.robots.append(new_robot) + self.robot_ids[new_robot.id] = new_robot # this also means the estimator + # will track the new robot because + # it got a reference to the list of + # robot ids to keep an eye out for + + while not self.estimator.all_robots_detected(): # wait until the robot gets detected + pass + + # drive the new robot to its starting position + self.grid_control(new_robot.id, 'nop') + + clientsocket.sendall(b'OK\n') + else: + clientsocket.sendall(f"error: could not connect to new robot {new_robot}".encode()) + + except IndexError: + print("could not initialize a new robot") + clientsocket.sendall('could not initialize a new robot: invalid command format\n' + 'expected: initialize_robot, , , , , \n'.encode()) + except ValueError: + print("could not initialize a new robot") + clientsocket.sendall('could not initialize a new robot: invalid command format\n' + 'expected: initialize_robot, , , , , \n'.encode()) else: print("invalid robot id!") clientsocket.sendall(b'Invalid robot id!\n') - else: clientsocket.sendall(b'Invalid command!\n') - else: # len(inputs) <= 1 + else: # len(inputs) <= 1 if b'quit' in inputs[0]: clientsocket.close() self.comm_socket.close() @@ -214,20 +255,21 @@ class RemoteController: clientsocket.close() def grid_control(self, robot_id, cmd): - robot = self.robot_ids[robot_id] # get robot to be controlled + robot = self.robot_ids[robot_id] # get robot to be controlled print("robot grid pos before move: ", robot) robot.move(cmd) print("robot grid pos after move: ", robot) - target = self.estimator.get_pos_from_grid_point(robot.x, robot.y, robot.orientation) + target = self.estimator.get_pos_from_grid_point(robot.grid_x, robot.grid_y, robot.grid_orientation) self.controller.move_to_pos(target, robot) + def main(args): rc = RemoteController() - rc.run() + if __name__ == '__main__': main(sys.argv) diff --git a/remote_control/robot.py b/remote_control/robot.py index 6af67cf..d158147 100644 --- a/remote_control/robot.py +++ b/remote_control/robot.py @@ -15,21 +15,23 @@ class Robot: self.u1 = 0.0 self.u2 = 0.0 + self.connected = False + def connect(self): # connect to robot try: print("connecting to robot {} with ip {} ...".format(self.id, self.ip)) self.socket.connect((self.ip, 1234)) # connect to robot print("connected!") + self.connected = True except socket.error: print("could not connect to robot {} with ip {}".format(self.id, self.ip)) - sys.exit(1) def send_cmd(self, u1=0.0, u2=0.0): if self.socket: try: self.socket.send(f'({u1},{u2})\n'.encode()) except BrokenPipeError: - # print(f"error: connection to robot {self.id} with ip {self.ip} lost") + print(f"error: connection to robot {self.id} with ip {self.ip} lost") pass