diff --git a/remote_control/mpc_controller.py b/remote_control/mpc_controller.py index dd36f5d..aa8435c 100644 --- a/remote_control/mpc_controller.py +++ b/remote_control/mpc_controller.py @@ -22,7 +22,6 @@ class MPCController: def move_to_pos(self, target_pos, robot, near_target_counter=5): near_target = 0 while near_target < near_target_counter: - while not self.estimator.event_queue.empty(): event = self.estimator.event_queue.get() print("event: ", event) @@ -56,7 +55,6 @@ class MPCController: print("quit!") self.controlling = False robot.send_cmd() - self.anim_stopped = True return x_pred = self.get_measurement(robot.id) @@ -98,5 +96,77 @@ class MPCController: time.sleep(self.dt) self.t = time.time() # save time the most recent control was applied + def interactive_control(self, robots): + 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 np.array(self.estimator.get_robot_state_estimate(robot_id)) + return np.array(self.estimator.get_robot_state_estimate(robot_id)) \ No newline at end of file diff --git a/remote_control/mpc_test.py b/remote_control/mpc_test.py new file mode 100644 index 0000000..4dab0be --- /dev/null +++ b/remote_control/mpc_test.py @@ -0,0 +1,53 @@ +import sys +import socket +import threading +import time + +from robot import Robot + +from mpc_controller import MPCController + +import opencv_viewer_example + + +class RemoteController: + def __init__(self): + self.robots = [Robot(12, '192.168.1.12')] + + self.robot_ids = {} + for r in self.robots: + self.robot_ids[r.id] = r + + # socket for movement commands + self.comm_socket = socket.socket() + self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + + for robot in self.robots: + robot.connect() + + self.comm_socket.bind(('', 1337)) + self.comm_socket.listen(5) + + self.t = time.time() + + # start thread for marker position detection + self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys()) + self.estimator_thread = threading.Thread(target=self.estimator.run_tracking) + self.estimator_thread.start() + + self.controller = MPCController(self.estimator) + + def run(self): + print("waiting until all markers are detected...") + while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()): + pass + print("starting control") + self.controller.interactive_control(robots=self.robots) + +def main(args): + rc = RemoteController() + rc.run() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/remote_control/opencv_viewer_example.py b/remote_control/opencv_viewer_example.py index 225b8fa..823c139 100644 --- a/remote_control/opencv_viewer_example.py +++ b/remote_control/opencv_viewer_example.py @@ -5,21 +5,6 @@ from cv2 import aruco from shapely.geometry import LineString from queue import Queue -DS5_product_ids = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07","0B3A"] - - -def find_device_that_supports_advanced_mode(): - ctx = rs.context() - ds5_dev = rs.device() - devices = ctx.query_devices() - for dev in devices: - if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_product_ids: - if dev.supports(rs.camera_info.name): - print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name)) - return dev - raise Exception("No device that supports advanced mode was found") - - class ArucoEstimator: grid_columns = 10 grid_rows = 8 @@ -33,19 +18,21 @@ class ArucoEstimator: angles = [] corner_estimates = { - 'a': (None, 0), # (estimate, n_estimates) - 'b': (None, 0), - 'c': (None, 0), - 'd': (None, 0) + 'a': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, + 'b': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, + 'c': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, + 'd': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, } - def __init__(self, robot_marker_ids=[]): + def __init__(self, robot_marker_ids=None, use_realsense=True): + if robot_marker_ids is None: + robot_marker_ids = [] self.robot_marker_ids = robot_marker_ids self.robot_marker_estimates = dict([(id, None) for id in self.robot_marker_ids]) self.event_queue = Queue() - if False: # check if realsense camera is connected + if use_realsense: # check if realsense camera is connected # Configure depth and color streams self.pipeline = rs.pipeline() config = rs.config() @@ -71,10 +58,33 @@ class ArucoEstimator: self.cv_camera = cv2.VideoCapture(0) self.pipeline = None - def mouse_callback(self, event, x, y, flags, param): + def mouse_callback(self, event, px, py, flags, param): if event == 1: - print(event) - self.event_queue.put(('click', (x,y))) + if self.all_corners_detected(): + # inter/extrapolate from clicked point to marker position + px1 = self.corner_estimates['a']['pixel_coordinate'][0] + px3 = self.corner_estimates['c']['pixel_coordinate'][0] + py1 = self.corner_estimates['a']['pixel_coordinate'][1] + py3 = self.corner_estimates['c']['pixel_coordinate'][1] + + x1 = self.corner_estimates['a']['real_world_estimate'][0] + x3 = self.corner_estimates['c']['real_world_estimate'][0] + y1 = self.corner_estimates['a']['real_world_estimate'][1] + y3 = self.corner_estimates['c']['real_world_estimate'][1] + + alpha = (px - px1) / (px3 - px1) + beta = (py - py1) / (py3 - py1) + + print(f"alpha = {alpha}, beta = {beta}") + + target_x = x1 + alpha * (x3 - x1) + target_y = y1 + beta * (y3 - y1) + target = np.array([target_x, target_y, 0]) + + print(f"target = ({target_x},{target_y})") + self.event_queue.put(('click', target)) + else: + print("not all markers have been detected!") def run_tracking(self): cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE) @@ -102,20 +112,21 @@ class ArucoEstimator: corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids) - if False:#ids is not None: - for id, c in zip(ids, corners): - res = aruco.estimatePoseSingleMarkers(c, 0.10, self.camera_matrix, self.dist_coeffs) + if ids is not None: + for id, corner in zip(ids, corners): + corner_pixel_coord = np.mean(corner[0], axis=0) + res = aruco.estimatePoseSingleMarkers(corner, 0.10, self.camera_matrix, self.dist_coeffs) rvecs = res[0][0][0] tvecs = res[1][0][0] - self.update_estimate(id, rvecs, tvecs) + self.update_estimate(id[0], corner_pixel_coord, rvecs, tvecs) frame = self.draw_grid_lines(frame, corners, ids) frame = self.draw_robot_pos(frame, corners, ids) else: # pretent we detected some markers pass - self.update_estimate(0, rvec=np.array([0,0,0]), tvec=np.array([-1, 1, 0])) + self.update_estimate(0, rvec=np.array([0, 0, 0]), tvec=np.array([-1, 1, 0])) self.update_estimate(1, rvec=np.array([0, 0, 0]), tvec=np.array([1, 1.5, 0])) self.update_estimate(2, rvec=np.array([0, 0, 0]), tvec=np.array([1, -1.5, 0])) self.update_estimate(3, rvec=np.array([0, 0, 0]), tvec=np.array([-1, -1, 0])) @@ -123,6 +134,8 @@ class ArucoEstimator: #self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]), tvec=np.array([-1.0 + random.random() * 2, -1.0 + random.random() * 2, 0])) self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]), tvec=np.array([1.2, 0.42, 0])) + self.update_estimate(13, rvec=np.array([0.0, 0.0, 0.0]), + tvec=np.array([-1.2, -0.42, 0])) # Show images cv2.imshow('RoboRally', frame) @@ -139,21 +152,25 @@ class ArucoEstimator: # Stop streaming self.pipeline.stop() - def update_estimate(self, id, rvec, tvec): + def update_estimate(self, id, pixel_coord, rvec, tvec): # update the marker estimate with new data if id in self.corner_marker_ids.values(): # for corner markers we compute the mean of all measurements s.t. the position stabilizes over time # (we assume the markers do not move) corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys())) # get corresponding corner to the detected marker - old_estimate = self.corner_estimates[corner][0] - n_estimates = self.corner_estimates[corner][1] + old_estimate = self.corner_estimates[corner]['real_world_estimate'] + n_estimates = self.corner_estimates[corner]['n_estimates'] + tvec_proj = tvec[0:2] # projection to get rid of z coordinate tvec_proj = np.array((tvec_proj[0], -tvec_proj[1])) # flip y coordinate if old_estimate is not None: new_estimate = (n_estimates * old_estimate + tvec_proj) / (n_estimates + 1) # weighted update else: new_estimate = tvec_proj # first estimate - self.corner_estimates[corner] = (new_estimate, n_estimates + 1) + self.corner_estimates[corner]['real_world_estimate'] = new_estimate + self.corner_estimates[corner]['n_estimates'] = n_estimates + 1 + self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord + elif id in self.robot_marker_ids: # for robot markers we extract x and y position as well as the angle # here we could also implement a filter @@ -171,7 +188,11 @@ class ArucoEstimator: def all_corners_detected(self): # checks if all corner markers have been detected at least once - return not any([estimate[0] is None for estimate in self.corner_estimates.values()]) + return not any([estimate['n_estimates'] == 0 for estimate in self.corner_estimates.values()]) + + def all_robots_detected(self): + # checks if all robot markers have been detected at least once + return not any([estimate is None for estimate in self.robot_marker_estimates.values()]) def get_pos_from_grid_point(self, x, y, orientation=None): """ @@ -187,10 +208,10 @@ class ArucoEstimator: assert self.all_corners_detected() # compute column line - a = self.corner_estimates['a'][0] - b = self.corner_estimates['b'][0] - c = self.corner_estimates['c'][0] - d = self.corner_estimates['d'][0] + a = self.corner_estimates['a']['real_world_estimate'] + b = self.corner_estimates['b']['real_world_estimate'] + c = self.corner_estimates['c']['real_world_estimate'] + d = self.corner_estimates['d']['real_world_estimate'] x_frac = (x + 0.5) / self.grid_columns y_frac = (y + 0.5) / self.grid_rows @@ -239,8 +260,8 @@ class ArucoEstimator: def print_corner_estimates(self): for key, value in self.corner_estimates.items(): - if value[0] is not None: - print("corner marker {} at pos {}".format(key, value[0])) + if value['n_estimates'] != 0: + print("corner marker {} at pos {}".format(key, value['real_world_estimate'])) print() def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict): @@ -278,7 +299,7 @@ class ArucoEstimator: for x in range(1,self.grid_columns): column_line_top = a + x / self.grid_columns * vab column_line_bottom = d + x / self.grid_columns * vdc - frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(255, 0, 0), + frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(0, 255, 0), thickness=1) vad = d - a @@ -286,7 +307,7 @@ class ArucoEstimator: for y in range(1, self.grid_rows): row_line_top = a + y / self.grid_rows * vad row_line_bottom = b + y / self.grid_rows * vbc - frame = cv2.line(frame, tuple(row_line_top), tuple(row_line_bottom), color=(255, 0, 0), + frame = cv2.line(frame, tuple(row_line_top), tuple(row_line_bottom), color=(0, 255, 0), thickness=1) return frame diff --git a/remote_control/robot.py b/remote_control/robot.py new file mode 100644 index 0000000..6af67cf --- /dev/null +++ b/remote_control/robot.py @@ -0,0 +1,35 @@ +import socket +import sys + + +class Robot: + def __init__(self, marker_id, ip): + self.id = marker_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 + + 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 +