diff --git a/remote_control/mpc_controller.py b/remote_control/mpc_controller.py new file mode 100644 index 0000000..dd36f5d --- /dev/null +++ b/remote_control/mpc_controller.py @@ -0,0 +1,102 @@ +import numpy as np +import time + +from casadi_opt import OpenLoopSolver + +class MPCController: + def __init__(self, estimator): + self.t = time.time() + + self.estimator = estimator + self.controlling = False + + self.mstep = 2 + self.ols = OpenLoopSolver(N=20, T=1.0) + self.ols.setup() + self.dt = self.ols.T / self.ols.N + + # integrator + self.omega_max = 5.0 + self.control_scaling = 0.2 + + 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) + if event[0] == 'click': + pass + elif event[0] == 'key': + key = event[1] + + if key == 84: # arrow up + self.controlling = True + self.t = time.time() + elif key == 82: # arrow down + self.controlling = False + robot.send_cmd() + elif key == 48: # 0 + target_pos = np.array([0.0,0.0,0.0]) + 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 == 113: + print("quit!") + self.controlling = False + robot.send_cmd() + return + elif key == 27: # escape + print("quit!") + self.controlling = False + robot.send_cmd() + self.anim_stopped = True + return + + 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 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) + + #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) + 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 + + def get_measurement(self, robot_id): + return np.array(self.estimator.get_robot_state_estimate(robot_id)) diff --git a/remote_control/opencv_viewer_example.py b/remote_control/opencv_viewer_example.py index 6c503a8..225b8fa 100644 --- a/remote_control/opencv_viewer_example.py +++ b/remote_control/opencv_viewer_example.py @@ -3,6 +3,7 @@ import numpy as np import cv2 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"] @@ -42,6 +43,8 @@ class ArucoEstimator: 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 # Configure depth and color streams self.pipeline = rs.pipeline() @@ -68,9 +71,18 @@ class ArucoEstimator: self.cv_camera = cv2.VideoCapture(0) self.pipeline = None + def mouse_callback(self, event, x, y, flags, param): + if event == 1: + print(event) + self.event_queue.put(('click', (x,y))) + def run_tracking(self): + cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE) + cv2.setMouseCallback('RoboRally', self.mouse_callback) + try: - while True: + running = True + while running: if self.pipeline: frames = self.pipeline.wait_for_frames() color_frame = frames.get_color_frame() @@ -113,11 +125,16 @@ class ArucoEstimator: tvec=np.array([1.2, 0.42, 0])) # Show images - cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) - cv2.imshow('RealSense', frame) - cv2.waitKey(1) + cv2.imshow('RoboRally', frame) + key = cv2.waitKey(1) + if key > 0: + self.event_queue.put(('key', key)) + print('key = ', key) + if key == ord('q'): + running = False finally: + cv2.destroyAllWindows() if self.pipeline is not None: # Stop streaming self.pipeline.stop() @@ -304,3 +321,5 @@ class ArucoEstimator: if __name__ == "__main__": estimator = ArucoEstimator() + + estimator.run_tracking() diff --git a/remote_control/roborally.py b/remote_control/roborally.py index b200232..5bd3470 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -1,36 +1,9 @@ -# startup: -# roscore -> start ros -# rosparam set cv_camera/device_id 0 -> set appropriate camera device -# rosrun cv_camera cv_camera_node -> start the camera -# roslaunch aruco_detect aruco_detect.launch camera:=cv_camera image:=image_raw dictionary:=16 transport:= fiducial_len:=0.1 # aruco marker detection -# python fiducial_to_2d_pos_angle.py -> compute position and angle of markers in 2d plane - import sys -import rospy -import pygame -import numpy as np import socket -import scipy.integrate -import copy - import threading -from copy import deepcopy - -import matplotlib.pyplot as plt -import matplotlib.animation as anim -import matplotlib.patches as patch - -from shapely.geometry import Polygon - import time -from casadi_opt import OpenLoopSolver - -#from marker_pos_angle.msg import id_pos_angle - -from collections import OrderedDict - -from argparse import ArgumentParser +from mpc_controller import MPCController import opencv_viewer_example @@ -76,12 +49,6 @@ class Robot: self.ip = ip self.socket = socket.socket() - # variables for measurements - self.tms_0 = None - self.xm_0 = None - self.tms = None - self.xms = None - # currently active control self.u1 = 0.0 self.u2 = 0.0 @@ -126,36 +93,23 @@ class Robot: 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}" -def f_ode(t, x, u): - # dynamical model of the two-wheeled robot - # TODO: find exact values for these parameters - r = 0.03 - R = 0.05 - d = 0.02 - - theta = x[2] - - omega_r = u[0] - omega_l = u[1] - - dx = np.zeros(3) - - dx[0] = (r/2.0 * np.cos(theta) - r*d/(2.0*R) * np.sin(theta)) * omega_r \ - + (r/2.0 * np.cos(theta) + r*d/(2.0 * R) * np.sin(theta)) * omega_l - dx[1] = (r/2.0 * np.sin(theta) + r*d/(2.0*R) * np.cos(theta)) * omega_r \ - + (r/2 * np.sin(theta) - r*d/(2.0*R) * np.cos(theta)) * omega_l - dx[2] = -r/(2.0*R) * (omega_r - omega_l) - - return dx class RemoteController: - def __init__(self, id, ip): + 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))] @@ -182,38 +136,14 @@ class RemoteController: self.t = time.time() - # variables for simulated state - self.x0 = None - self.ts = np.array([]) - self.xs = [] - - # variable for mpc open loop - self.ol_x = None - self.ol_y = None - - # ROS subscriber for detected markers + # 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() - # pid parameters - self.controlling = False + self.controller = MPCController(self.estimator) - self.mstep = 2 - self.ols = OpenLoopSolver(N=20, T=1.0) - self.ols.setup() - self.dt = self.ols.T / self.ols.N - - self.target = (0.0, 0.0, 0.0) - - # integrator - self.r = scipy.integrate.ode(f_ode) - self.omega_max = 5.0 - self.control_scaling = 0.2 - #self.omega_max = 13.32 - - def controller(self): + def run(self): print("waiting until all markers are detected...") while not self.estimator.all_corners_detected(): pass @@ -260,7 +190,7 @@ class RemoteController: 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))) else: - self.mpc_control(robot_id, cmd) + self.grid_control(robot_id, cmd) clientsocket.sendall(b'OK\n') else: print("invalid robot id!") @@ -268,7 +198,7 @@ class RemoteController: 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() @@ -283,123 +213,21 @@ class RemoteController: connected = False clientsocket.close() - def mpc_control(self, robot_id, cmd): - robot = self.robot_ids[robot_id] # get robot to be controlled + def grid_control(self, robot_id, cmd): + 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) - self.target = self.estimator.get_pos_from_grid_point(robot.x, robot.y, robot.orientation) + target = self.estimator.get_pos_from_grid_point(robot.x, robot.y, robot.orientation) - self.pid = False - self.mpc = True - - near_target = 0 - - while near_target < 5: - # open loop controller - events = pygame.event.get() - - for event in events: - if event.type == pygame.KEYDOWN: - if event.key == pygame.K_UP: - self.controlling = True - self.t = time.time() - elif event.key == pygame.K_DOWN: - self.controlling = False - if self.robot_ids[robot_id].socket: - self.robot_ids[robot_id].socket.send('(0.0,0.0)\n') - elif event.key == pygame.K_0: - self.target = np.array([0,0,0]) - elif event.key == pygame.K_PLUS: - self.control_scaling += 0.1 - self.control_scaling = min(self.control_scaling, 1.0) - print("control scaling = ", self.control_scaling) - elif event.key == pygame.K_MINUS: - self.control_scaling -= 0.1 - self.control_scaling = max(self.control_scaling, 0.1) - print("control scaling = ", self.control_scaling) - elif event.key == pygame.K_ESCAPE: - print("quit!") - self.controlling = False - if self.robot_ids[robot_id].socket: - self.robot_ids[robot_id].socket.send('(0.0,0.0)\n') - self.anim_stopped = True - return - elif event.key == pygame.QUIT: - print("quit!") - self.controlling = False - if self.robot_ids[robot_id].socket: - self.robot_ids[robot_id].socket.send('(0.0,0.0)\n') - self.anim_stopped = True - return - - if self.mpc: - x_pred = self.get_measurement(robot_id) - - tmpc_start = time.time() - - error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2]) - angles_unwrapped = np.unwrap([x_pred[2], self.target[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, self.target) - - #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) - 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] - if self.robot_ids[robot_id].socket: - #self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2).encode()) - 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)) + self.controller.move_to_pos(target, robot) def main(args): - parser = ArgumentParser() - parser.add_argument('id', metavar='id', type=str, help='marker id of the controlled robot') - parser.add_argument('ip', metavar='ip', type=str, help='ip address of the controlled robot') - args = parser.parse_args() + rc = RemoteController() - marker_id = int(args.id) - ip = args.ip - - rc = RemoteController(marker_id, ip) - - pygame.init() - - screenheight = 480 - screenwidth = 640 - pygame.display.set_mode([screenwidth, screenheight]) - - rc.controller() + rc.run() if __name__ == '__main__': main(sys.argv)