From d0c17c0b9108459a6baa7702e63a199df8ab65ff Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Wed, 25 Aug 2021 22:53:09 +0200 Subject: [PATCH] got mpc controller running again --- remote_control/controller.py | 3 ++- remote_control/mpc_test.py | 48 +++++++++++++++++++++++++++--------- remote_control/robot.py | 28 ++++++++++++++++++--- 3 files changed, 62 insertions(+), 17 deletions(-) diff --git a/remote_control/controller.py b/remote_control/controller.py index 55809f2..414b997 100644 --- a/remote_control/controller.py +++ b/remote_control/controller.py @@ -21,7 +21,8 @@ class ControllerBase: if self.controlling: self.apply_control(control) - self.apply_control((0.0, 0.0)) # stop robot + if self.robot is not None: + self.robot.send_cmd(u1=0, u2=0) # stop robot def set_target_position(self, target_pos): self.target_pos = target_pos diff --git a/remote_control/mpc_test.py b/remote_control/mpc_test.py index 19a2b81..1ff7c60 100644 --- a/remote_control/mpc_test.py +++ b/remote_control/mpc_test.py @@ -3,17 +3,32 @@ import socket import threading import time -from robot import Robot +from robot import Robot, ControlledRobot from mpc_controller import MPCController -from aruco_estimator import ArucoEstimator +# HOST, PORT = "localhost", 42424 +# +# robot_id = 12 +# +# sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +# sock.connect((HOST, PORT)) +# sock.sendall(f"{robot_id}\n".encode()) # request data for robot with given id +# #sock.sendall(f"events\n".encode()) # request events +# receiving = True +# while receiving: +# received = str(sock.recv(1024), "utf-8") +# print("Received: {}".format(received)) +# receiving = len(received) > 0 class RemoteController: def __init__(self): self.robots = [] - #self.robots = [Robot(11, '192.168.1.11'), Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')] + #self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')] + #self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')] + #self.robots = [Robot(14, '10.10.11.89')] + self.robots = [ControlledRobot(13, '192.168.1.13')] self.robot_ids = {} for r in self.robots: @@ -32,18 +47,17 @@ class RemoteController: self.t = time.time() # start thread for marker position detection - self.estimator = ArucoEstimator(self.robot_ids.keys()) - self.estimator_thread = threading.Thread(target=self.estimator.run_tracking) - self.estimator_thread.start() - - self.controller = MPCController(self.estimator) + self.controller = MPCController() + self.robots[0].attach_controller(self.controller) 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("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) + #self.controller.interactive_control(robots=self.robots) + self.controller.start() + pass def main(args): @@ -53,3 +67,13 @@ def main(args): if __name__ == '__main__': main(sys.argv) + +# to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams +# marker positions and GUI events via socket +# next, launch this program (mpc_test.py) in debug mode and set a breakpoint after self.controller.start() +# you can now set the target position for the controlled robot via move_to_pos(), e.g. +# self.robots[0].move_to_pos([0.0,0,0], True) + +# TODO +# figure out how to get clicked positions from measurement server and drive robot there +# also document and improve program structure \ No newline at end of file diff --git a/remote_control/robot.py b/remote_control/robot.py index f6c41b4..ad13ad8 100644 --- a/remote_control/robot.py +++ b/remote_control/robot.py @@ -1,7 +1,9 @@ import socket import threading import json - +import numpy as np +import time +import math class Robot: def __init__(self, marker_id, ip, measurement_server=('127.0.0.1', 42424)): @@ -123,8 +125,26 @@ class ControlledRobot(Robot): self.controller = controller self.controller.attach_robot(self) - def move_to_pos(self, target_pos): + def move_to_pos(self, target_pos, blocking=False): if self.controller is not None: - self.controller.set_target_position(target_pos) + if not blocking: + self.controller.set_target_position(target_pos) + else: # only return after the robot has reached the target + self.stop_control() + self.controller.set_target_position(target_pos) + self.start_control() + + close_to_target = False + while not close_to_target: + current_pos = np.array([self.x, self.y, self.angle]) + v = target_pos - current_pos + angles_unwrapped = np.unwrap([current_pos[2], target_pos[2]]) # unwrap angle to avoid jump in data + e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference + e_pos = np.linalg.norm(v[0:2]) + print(f"e_pos = {e_pos}, e_ang = {e_angle}") + close_to_target = e_pos < 0.05 and abs(e_angle) < 0.1 + time.sleep(0.1) + print("target reached!") + self.stop_control() else: - raise Exception("Error: Cannot move to position: there is not controller attached to the robot!") + raise Exception("Error: Cannot move to position: there is not controller attached to the robot!") \ No newline at end of file