From 6fb3cd5484e7677554b2bb94071e7c5be5722b32 Mon Sep 17 00:00:00 2001
From: spirkelmann <simon.pirkelmann@gmail.com>
Date: Fri, 4 Sep 2020 16:53:39 +0200
Subject: [PATCH] so-so working version for mpc control

---
 remote_control/roborally.py | 619 ++++++++++++++++++++++++++++++++++++
 1 file changed, 619 insertions(+)
 create mode 100644 remote_control/roborally.py

diff --git a/remote_control/roborally.py b/remote_control/roborally.py
new file mode 100644
index 0000000..6a7f1af
--- /dev/null
+++ b/remote_control/roborally.py
@@ -0,0 +1,619 @@
+# 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
+
+
+
+class Robot:
+    def __init__(self, id, ip):
+        self.pos = None
+        self.orient = None
+
+        self.id = id
+
+        self.pos = None
+        self.euler = None
+
+        self.ip = ip
+
+class Obstacle:
+    def __init__(self, id, radius):
+        self.id = id
+        self.pos = None
+        self.radius = radius
+
+class Track:
+    def __init__(self):
+        # ids in clockwise direction
+        self.inner = OrderedDict()
+        self.inner[0] = None
+        self.inner[1] = None
+        self.inner[2] = None
+        self.inner[3] = None
+
+        self.outer = OrderedDict()
+        self.outer[4] = None
+        self.outer[5] = None
+        self.outer[6] = None
+        self.outer[7] = None
+
+        self.track_complete = False
+
+        self.inner_poly = None
+        self.outer_poly = None
+
+    def set_id(self, d):
+        if not self.track_complete:
+            if d.id in self.inner:
+                print("Detected marker {} at pos {}".format(d.id, (d.x,d.y)))
+                self.inner[d.id] = (d.x, d.y)
+            elif d.id in self.outer:
+                print("Detected marker {} at pos {}".format(d.id, (d.x, d.y)))
+                self.outer[d.id] = (d.x, d.y)
+            else:
+                print("Unknown marker!")
+        else:
+            return
+
+        if not None in self.inner.values() and not None in self.outer.values():
+            print("Track marker positions detected!")
+            self.track_complete = True
+
+            self.inner_poly = Polygon(self.inner.values())
+            self.outer_poly = Polygon(self.outer.values())
+
+    def plot_track(self):
+        if self.track_complete:
+            plt.figure(2)
+            x_in, y_in = self.inner_poly.exterior.xy
+            x_out, y_out = self.outer_poly.exterior.xy
+            plt.plot(x_in, y_in)
+            plt.plot(x_out, y_out)
+        else:
+            print("plot is not complete yet!")
+
+
+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):
+
+        self.anim_stopped = False
+
+        #self.robots = [Robot(14, '192.168.1.103')]
+        #self.robots = [Robot(15, '192.168.1.102')]
+        self.robots = [Robot(id, ip)]
+
+        self.robot_ids = {}
+        for r in self.robots:
+            self.robot_ids[r.id] = r
+
+
+        self.track = Track()
+
+        # connect to robot
+        self.rc_socket = socket.socket()
+        #self.rc_socket = None
+        try:
+            for r in self.robots:
+                self.rc_socket.connect((r.ip, 1234))  # connect to robot
+        except socket.error:
+            print("could not connect to socket")
+            sys.exit(1)
+
+
+        self.t = time.time()
+
+        # variables for simulated state
+        self.x0 = None
+        self.ts = np.array([])
+        self.xs = []
+
+        # variables for measurements
+        self.tms_0 = None
+        self.xm_0 = None
+        self.tms = None
+        self.xms = None
+
+        # variable for mpc open loop
+        self.ol_x = None
+        self.ol_y = None
+
+        self.mutex = threading.Lock()
+
+        # ROS subscriber for detected markers
+        marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
+
+        # pid parameters
+        self.controlling = False
+
+        # currently active control
+        self.u1 = 0.0
+        self.u2 = 0.0
+
+        # animation
+        self.fig = plt.figure()
+        self.ax = self.fig.add_subplot(2,2,1)
+        self.ax2 = self.fig.add_subplot(2, 2, 2)
+        self.ax3 = self.fig.add_subplot(2, 2, 4)
+
+        self.xdata, self.ydata = [], []
+        self.line, = self.ax.plot([],[], color='grey', linestyle=':')
+        self.line_sim, = self.ax.plot([], [])
+        self.line_ol, = self.ax.plot([],[], color='green', linestyle='--')
+        self.dirm, = self.ax.plot([], [])
+        self.dirs, = self.ax.plot([], [])
+
+        self.line_x, = self.ax2.plot([],[])
+        self.line_y, = self.ax3.plot([], [])
+
+        self.track_line_inner, = self.ax.plot([], [])
+        self.track_line_outer, = self.ax.plot([], [])
+
+        self.ax.set_xlabel('x-position')
+        self.ax.set_ylabel('y-position')
+        self.ax.grid()
+
+        self.ax2.set_xlabel('Zeit t')
+        self.ax2.set_ylabel('x-position')
+        self.ax2.grid()
+
+        self.ax3.set_xlabel('Zeit t')
+        self.ax3.set_ylabel('y-position')
+        self.ax3.grid()
+
+        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.omega_max = 13.32
+
+
+    def ani(self):
+
+        print("starting animation")
+        self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
+        plt.ion()
+        plt.show(block=True)
+
+    def ani_init(self):
+        self.ax.set_xlim(-2, 2)
+        self.ax.set_ylim(-2, 2)
+        self.ax.set_aspect('equal', adjustable='box')
+
+        self.ax2.set_ylim(-2, 2)
+        self.ax2.set_xlim(0, 10)
+
+        self.ax3.set_ylim(-2, 2)
+        self.ax3.set_xlim(0, 10)
+
+        self.track_line_inner.set_data(self.track.inner_poly.exterior.xy)
+        self.track_line_outer.set_data(self.track.outer_poly.exterior.xy)
+
+        return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
+               self.track_line_inner, self.track_line_outer, self.line_x,self.line_y,
+
+    def ani_update(self, frame):
+        if self.anim_stopped:
+            self.ani.event_source.stop()
+            sys.exit(0)
+
+        #print("plotting")
+        self.mutex.acquire()
+        try:
+            # copy data for plot from global arrays
+            if self.tms is not None:
+                tm_local = deepcopy(self.tms)
+                xm_local = deepcopy(self.xms)
+
+                if len(tm_local) > 0:
+                    # plot path of the robot
+                    self.line.set_data(xm_local[:,0], xm_local[:,1])
+
+                    # compute and plot direction the robot is facing
+                    a = xm_local[-1, 0]
+                    b = xm_local[-1, 1]
+
+                    a2 = a + np.cos(xm_local[-1, 2]) * 0.2
+                    b2 = b + np.sin(xm_local[-1, 2]) * 0.2
+
+                    self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
+
+                    n_plot = 300
+                    if len(tm_local) > n_plot:
+                        # plot x and y coordinate
+                        self.line_x.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:,0])
+                        self.line_y.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:, 1])
+
+            ts_local = deepcopy(self.ts)
+            xs_local = deepcopy(self.xs)
+
+            if len(ts_local) > 0:
+                # plot simulated path of the robot
+                self.line_sim.set_data(xs_local[:,0], xs_local[:,1])
+
+                # compute and plot direction the robot is facing
+                a = xs_local[-1, 0]
+                b = xs_local[-1, 1]
+
+                a2 = a + np.cos(xs_local[-1, 2]) * 0.2
+                b2 = b + np.sin(xs_local[-1, 2]) * 0.2
+
+                self.dirs.set_data(np.array([a, a2]), np.array([b, b2]))
+
+            ol_x_local = deepcopy(self.ol_x)
+            ol_y_local = deepcopy(self.ol_y)
+
+            if ol_x_local is not None:
+                self.line_ol.set_data(ol_x_local, ol_y_local)
+            else:
+                self.line_ol.set_data([],[])
+
+        finally:
+            self.mutex.release()
+
+        return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\
+               self.line_x, self.line_y, 
+
+    def measurement_callback(self, data):
+        #print(data)
+
+        # detect robots
+        if data.id in self.robot_ids:
+            r = self.robot_ids[data.id]
+
+            r.pos = (data.x, data.y) # only x and y component are important for us
+            r.euler = data.angle
+
+            # save measured position and angle for plotting
+            measurement = np.array([r.pos[0], r.pos[1], r.euler])
+            if self.tms_0 is None:
+                self.tms_0 = time.time()
+                self.xm_0 = measurement
+
+                self.mutex.acquire()
+                try:
+                    self.tms = np.array([0.0])
+                    self.xms = measurement
+                finally:
+                    self.mutex.release()
+            else:
+                self.mutex.acquire()
+                try:
+                    self.tms = np.vstack((self.tms, time.time() - self.tms_0))
+                    self.xms = np.vstack((self.xms, measurement))
+                finally:
+                    self.mutex.release()
+
+        # detect track
+        if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
+            self.track.set_id(data)
+
+    def controller(self):
+        print("starting control")
+
+        targets = {}
+        markers_in = self.track.inner.values()
+        markers_out = self.track.outer.values()
+
+        # find targets:
+        # generate waypoints
+        # lamb = 0.5
+        # j = 0
+        # for i in range(0,4):
+        #     p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
+        #     targets[j] = (p[0],p[1], 0.0)
+        #     j += 1
+        #     if i < 3:
+        #         mean_in = (np.array(markers_in[i]) + np.array(markers_in[i+1])) * 0.5
+        #         mean_out = (np.array(markers_out[i]) + np.array(markers_out[i+1])) * 0.5
+        #         mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
+        #         targets[j] = (mean[0], mean[1], 0.0)
+        #         j += 1
+
+
+        # final connection between first and last marker
+        #mean_in = (np.array(markers_in[3]) + np.array(markers_in[0])) * 0.5
+        #mean_out = (np.array(markers_out[3]) + np.array(markers_out[0])) * 0.5
+        #mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
+        #targets[j] = (mean[0], mean[1], 0.0)
+
+        grid_pos = (0,0, 0)
+        target_pos = np.array((0.0, 0.0, 0.0))
+
+        auto_control = False
+        current_target = 0
+                
+        control_scaling = 0.4
+
+        self.pid = False
+        self.mpc = True
+
+        integ = 0.0
+
+        while True:
+            # open loop controller
+            events = pygame.event.get()
+
+            move = 0
+            turn = 0
+
+            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.rc_socket:
+                            self.rc_socket.send('(0.0,0.0)\n')
+                    elif event.key == pygame.K_0:
+                        grid_pos = (0,0, 0)
+                    elif event.key == pygame.K_w:
+                        move = 1
+                        turn = 0
+
+                    elif event.key == pygame.K_s:
+                        move = -1
+                        turn = 0
+                    elif event.key == pygame.K_a:
+                        turn = 1
+                        move = 0
+                        integ = 0
+                    elif event.key == pygame.K_d:
+                        turn = -1
+                        move = 0
+                        integ = 0
+                    elif event.key == pygame.K_p:
+                        self.pid = True
+                    elif event.key == pygame.K_SPACE:
+                        auto_control = not auto_control
+                        if auto_control:
+                            self.target = targets[current_target]
+                    elif event.key == pygame.K_PLUS:
+                        control_scaling += 0.1
+                        control_scaling = min(control_scaling, 1.0)
+                    elif event.key == pygame.K_MINUS:
+                        control_scaling -= 0.1
+                        control_scaling = max(control_scaling, 0.3)
+                    elif event.key == pygame.K_ESCAPE:
+                        print("quit!")
+                        self.controlling = False
+                        if self.rc_socket:
+                            self.rc_socket.send('(0.0,0.0)\n')
+                        self.anim_stopped = True
+                        return
+                    elif event.key == pygame.QUIT:
+                        print("quit!")
+                        self.controlling = False
+                        if self.rc_socket:
+                            self.rc_socket.send('(0.0,0.0)\n')
+                        self.anim_stopped = True
+                        return
+
+                    # compute new grid position and orientation
+
+                    if move != 0:
+                        new_x = grid_pos[0] + move * np.cos(grid_pos[2])
+                        new_y = grid_pos[1] + move * np.sin(grid_pos[2])
+                        new_angle = grid_pos[2]
+                        grid_pos = (new_x, new_y, new_angle)
+
+                        print(grid_pos)
+                    elif turn != 0:
+                        new_x = grid_pos[0]
+                        new_y = grid_pos[1]
+                        new_angle = grid_pos[2] + turn * np.pi/2
+                        grid_pos = (new_x, new_y, new_angle)
+
+                        print(grid_pos)
+
+                    self.target = np.array((0.25*grid_pos[0], 0.25*grid_pos[1], grid_pos[2]))
+
+            if self.controlling:
+                if self.mpc:
+                    x_pred = self.get_measurement_prediction()
+
+                    tmpc_start = time.time()
+
+                    error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2])
+                    error_ang = np.abs(x_pred[2] - self.target[2])
+                    print("error pos = ", error_pos, " error_ang = ", error_ang)
+
+                    turning = turn != 0
+                    if error_pos > 0.1 or error_ang > 0.4:
+                        # solve mpc open loop problem
+                        res = self.ols.solve(x_pred, self.target, [], turning)
+
+
+                        #us1 = res[0]
+                        #us2 = res[1]
+                        us1 = res[0] * control_scaling
+                        us2 = res[1] * control_scaling
+                        #print("u = {}", (us1, us2))
+
+                        # save open loop trajectories for plotting
+                        self.mutex.acquire()
+                        try:
+                            self.ol_x = res[2]
+                            self.ol_y = res[3]
+                        finally:
+                            self.mutex.release()
+
+                        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
+
+                    # 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.rc_socket:
+                            self.rc_socket.send('({},{})\n'.format(u1, u2))
+                            if i < self.mstep:
+                                time.sleep(self.dt)
+                        self.t = time.time() # save time the most recent control was applied
+                else:
+                    if self.pid:
+                        x_pred = self.get_measurement()
+
+                        # compute angle difference
+                        d_angle = x_pred[2] - self.target[2]
+                        dt = time.time() - self.t
+                        integ += d_angle * dt
+
+                        #print(d_angle)
+
+                        K = 0.2
+                        I = 0.15
+                        pp = d_angle * K
+                        ii = integ * I
+                        u1 = pp + ii
+                        u2 = -u1
+
+                        print("e = {}, dt = {}, P = {}, I = {}, u = {}".format(d_angle, dt, pp, ii, (u1,u2)))
+
+                        self.t = time.time()
+
+                        self.rc_socket.send('({},{})\n'.format(u1, u2))
+                        time.sleep(0.025)
+                        #self.rc_socket.send('({},{})\n'.format(0, 0))
+                        #time.sleep(0.1)
+
+
+
+                        #self.pid = False
+
+
+    def get_measurement_prediction(self):
+        # get measurement
+        self.mutex.acquire()
+        try:
+            window = 3
+            last_measurement = copy.deepcopy(self.xms[-window:])
+            #print("last_measurements = {}".format(last_measurement))
+            #print("mean = {}".format(np.mean(last_measurement, axis=0)))
+            last_measurement = np.mean(last_measurement, axis=0)
+            last_time = copy.deepcopy(self.tms[-1])
+        finally:
+            self.mutex.release()
+
+        # prediction of state at time the mpc will terminate
+        self.r.set_f_params(np.array([self.u1 * self.omega_max, self.u2 * self.omega_max]))
+
+        self.r.set_initial_value(last_measurement, last_time)
+
+        x_pred = self.r.integrate(self.r.t + self.dt)
+
+        return x_pred
+
+    def get_measurement(self):
+        self.mutex.acquire()
+        try:
+            last_measurement = copy.deepcopy(self.xms[-1:])
+        finally:
+            self.mutex.release()
+        return last_measurement[0]
+
+    def pos_getter(self):
+        while True:
+            x_pred = self.get_measurement_prediction()
+
+            print("pos = ", x_pred)
+
+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()
+
+    marker_id = int(args.id)
+    ip = args.ip
+
+
+    rospy.init_node('controller_node', anonymous=True)
+
+    rc = RemoteController(marker_id, ip)
+
+    pygame.init()
+
+    screenheight = 480
+    screenwidth = 640
+    pygame.display.set_mode([screenwidth, screenheight])
+
+ #   print("waiting until track is completely detected")
+ #   while not rc.track.track_complete:
+ #       pass
+
+    #threading.Thread(target=rc.input_handling).start()
+    controller_thread = threading.Thread(target=rc.controller)
+    controller_thread.start()
+
+    #time.sleep(10)
+    #rc.ani()
+
+
+if __name__ == '__main__':
+    main(sys.argv)