Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

added option to dynamically initialize grid and robots and added support for arbitrary grid orientation w.r.t. camera

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-10-18 18:03:33 +02:00
parent 056d91da52
commit e93ae65e0f
4 changed files with 158 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
remote_control/mpc_controller.py
View File

@ -17,7 +17,7 @@ class MPCController:
# integrator # integrator
self.omega_max = 5.0 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): def move_to_pos(self, target_pos, robot, near_target_counter=5):
near_target = 0 near_target = 0
@ -59,42 +59,45 @@ class MPCController:
x_pred = self.get_measurement(robot.id) x_pred = self.get_measurement(robot.id)
error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2]) if x_pred is not None:
angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2])
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1]) angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data
#print("error pos = ", error_pos) error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
#print("error_pos = {}, error_ang = {}".format(error_pos, error_ang)) #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.075 or error_ang > 0.35:
if error_pos > 0.05 or error_ang > 0.2: if error_pos > 0.05 or error_ang > 0.1:
# solve mpc open loop problem # solve mpc open loop problem
res = self.ols.solve(x_pred, target_pos) res = self.ols.solve(x_pred, target_pos)
#us1 = res[0] #us1 = res[0]
#us2 = res[1] #us2 = res[1]
us1 = res[0] * self.control_scaling us1 = res[0] * self.control_scaling
us2 = res[1] * self.control_scaling us2 = res[1] * self.control_scaling
#print("u = {}", (us1, us2)) #print("u = {}", (us1, us2))
tmpc_end = time.time() tmpc_end = time.time()
#print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start)) #print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
dt_mpc = time.time() - self.t dt_mpc = time.time() - self.t
if dt_mpc < self.dt: # wait until next control can be applied if dt_mpc < self.dt: # wait until next control can be applied
#print("sleeping for {} seconds...".format(self.dt - dt_mpc)) #print("sleeping for {} seconds...".format(self.dt - dt_mpc))
time.sleep(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: else:
us1 = [0] * self.mstep print("robot not detected yet!")
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 interactive_control(self, robots): def interactive_control(self, robots):
controlled_robot_number = 0 controlled_robot_number = 0
@ -169,4 +172,4 @@ class MPCController:
self.t = time.time() # save time the most recent control was applied self.t = time.time() # save time the most recent control was applied
def get_measurement(self, robot_id): def get_measurement(self, robot_id):
return np.array(self.estimator.get_robot_state_estimate(robot_id)) return self.estimator.get_robot_state_estimate(robot_id)

13
remote_control/opencv_viewer_example.py
View File

@ -6,8 +6,6 @@ from shapely.geometry import LineString
from queue import Queue from queue import Queue
class ArucoEstimator: class ArucoEstimator:
grid_columns = 10
grid_rows = 8
corner_marker_ids = { corner_marker_ids = {
'a': 0, 'a': 0,
'b': 1, 'b': 1,
@ -24,7 +22,10 @@ class ArucoEstimator:
'd': {'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=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: if robot_marker_ids is None:
robot_marker_ids = [] robot_marker_ids = []
self.robot_marker_ids = robot_marker_ids self.robot_marker_ids = robot_marker_ids
@ -248,7 +249,7 @@ class ArucoEstimator:
elif orientation == 'v': elif orientation == 'v':
angle = x_frac * angle_ad + (1 - x_frac) * angle_bc angle = x_frac * angle_ad + (1 - x_frac) * angle_bc
elif orientation == '^': 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)) return np.array((point_of_intersection[0], point_of_intersection[1], angle))
else: else:
@ -315,11 +316,13 @@ class ArucoEstimator:
def get_robot_state_estimate(self, id): def get_robot_state_estimate(self, id):
if id in self.robot_marker_estimates: if id in self.robot_marker_estimates:
if self.robot_marker_estimates[id] is not None: if self.robot_marker_estimates[id] is not None:
return self.robot_marker_estimates[id] return np.array(self.robot_marker_estimates[id])
else: else:
print(f"error: no estimate available for robot {id}") print(f"error: no estimate available for robot {id}")
return None
else: else:
print(f"error: invalid robot id {id}") print(f"error: invalid robot id {id}")
return None
def draw_robot_pos(self, frame, corners, ids): def draw_robot_pos(self, frame, corners, ids):
# draws information about the robot positions onto the given frame # draws information about the robot positions onto the given frame

176
remote_control/roborally.py
View File

@ -4,10 +4,12 @@ import threading
import time import time
from mpc_controller import MPCController from mpc_controller import MPCController
from robot import Robot
import opencv_viewer_example import opencv_viewer_example
MSGLEN = 64 MSGLEN = 64
def myreceive(sock): def myreceive(sock):
chunks = [] chunks = []
bytes_recd = 0 bytes_recd = 0
@ -24,7 +26,7 @@ def myreceive(sock):
return b''.join(chunks) return b''.join(chunks)
class Robot: class RoboRallyRobot(Robot):
# dictionary mapping the current orientation and a turn command to the resulting orientation # dictionary mapping the current orientation and a turn command to the resulting orientation
resulting_orientation = { resulting_orientation = {
'^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'}, '^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'},
@ -36,94 +38,65 @@ class Robot:
# dictionary mapping an orientation to its opposite # dictionary mapping an orientation to its opposite
opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'} opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'}
def __init__(self, id, ip, x=0, y=0, orientation='>'): def __init__(self, id, ip, x, y, orientation):
self.x = x super().__init__(id, ip)
self.y = y
self.orientation = orientation
self.id = id self.grid_x = x
self.grid_y = y
self.pos = None self.grid_orientation = orientation
self.euler = None
self.ip = ip
self.socket = socket.socket()
# currently active control
self.u1 = 0.0
self.u2 = 0.0
def get_neighbor_coordinates(self, direction): def get_neighbor_coordinates(self, direction):
# get the coordinates of the neighboring tile in the given direction # get the coordinates of the neighboring tile in the given direction
if direction == '^': if direction == '^':
return (self.x, self.y - 1) return self.grid_x, self.grid_y - 1
elif direction == '>': elif direction == '>':
return (self.x + 1, self.y) return self.grid_x + 1, self.grid_y
elif direction == 'v': elif direction == 'v':
return (self.x, self.y + 1) return self.grid_x, self.grid_y + 1
elif direction == '<': elif direction == '<':
return (self.x - 1, self.y) return self.grid_x - 1, self.grid_y
else: else:
print("error: unknown direction") print("error: unknown direction")
sys.exit(1) sys.exit(1)
def move(self, type): def move(self, move_type):
if type == 'forward': if move_type == 'forward':
target_tile = self.get_neighbor_coordinates(self.orientation) target_tile = self.get_neighbor_coordinates(self.grid_orientation)
self.x = target_tile[0] self.grid_x = target_tile[0]
self.y = target_tile[1] self.grid_y = target_tile[1]
elif type == 'backward': elif move_type == 'backward':
opposite_orientation = Robot.opposites[self.orientation] opposite_orientation = RoboRallyRobot.opposites[self.grid_orientation]
target_tile = self.get_neighbor_coordinates(opposite_orientation) target_tile = self.get_neighbor_coordinates(opposite_orientation)
self.x = target_tile[0] self.grid_x = target_tile[0]
self.y = target_tile[1] self.grid_y = target_tile[1]
elif 'turn' in type: elif 'turn' in move_type:
self.orientation = Robot.resulting_orientation[self.orientation][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: else:
print("error: invalid move") print("error: invalid move")
sys.exit(1) 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): def __str__(self):
return self.__repr__() return self.__repr__()
def __repr__(self): 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: class RemoteController:
valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'nop', 'get position',
'set position', 'initialize_robot', 'initialize_grid']
def __init__(self): 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)), self.robots = []
# Robot(13, '192.168.1.13', (6, -3, np.pi)), Robot(14, '192.168.1.14', (6, -2, np.pi))] self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')]
#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.robot_ids = {} self.robot_ids = {}
for r in self.robots: for r in self.robots:
self.robot_ids[r.id] = r 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 # socket for movement commands
self.comm_socket = socket.socket() self.comm_socket = socket.socket()
self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
@ -143,6 +116,14 @@ class RemoteController:
self.controller = MPCController(self.estimator) 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): def run(self):
print("waiting until all markers are detected...") print("waiting until all markers are detected...")
while not self.estimator.all_corners_detected(): while not self.estimator.all_corners_detected():
@ -165,10 +146,28 @@ class RemoteController:
cmd = cmd.strip().decode() cmd = cmd.strip().decode()
if len(inputs) > 1: 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, <grid columns>, <grid rows>')
except IndexError:
print("could not initialize grid!")
clientsocket.sendall(b'"could not initialize grid!\n'
b'expected: initialize_robot, <grid columns>, <grid rows>')
else: # robot command
try: try:
robot_id = int(inputs[1]) robot_id = int(inputs[1])
except ValueError: except ValueError:
robot_id = None
print("could not read robot id!") print("could not read robot id!")
clientsocket.sendall(b'Could not read robot id!\n') clientsocket.sendall(b'Could not read robot id!\n')
@ -178,7 +177,8 @@ class RemoteController:
elif cmd == b'set position': elif cmd == b'set position':
try: try:
pos_data = ",".join(inputs[2:]) 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 self.robot_ids[robot_id].grid_pos = new_grid_pos
clientsocket.sendall(b'OK\n') clientsocket.sendall(b'OK\n')
except IndexError as e: except IndexError as e:
@ -188,17 +188,58 @@ class RemoteController:
self.robot_ids[robot_id].grid_pos))) self.robot_ids[robot_id].grid_pos)))
except ValueError as e: except ValueError as e:
print("could not set grid position!") 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: else:
self.grid_control(robot_id, cmd) self.grid_control(robot_id, cmd)
clientsocket.sendall(b'OK\n') 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, <id>, <ip>, <x>, <y>, <orientation>\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, <id>, <ip>, <x>, <y>, <orientation>\n'.encode())
else: else:
print("invalid robot id!") print("invalid robot id!")
clientsocket.sendall(b'Invalid robot id!\n') clientsocket.sendall(b'Invalid robot id!\n')
else: else:
clientsocket.sendall(b'Invalid command!\n') clientsocket.sendall(b'Invalid command!\n')
else: # len(inputs) <= 1 else: # len(inputs) <= 1
if b'quit' in inputs[0]: if b'quit' in inputs[0]:
clientsocket.close() clientsocket.close()
self.comm_socket.close() self.comm_socket.close()
@ -214,20 +255,21 @@ class RemoteController:
clientsocket.close() clientsocket.close()
def grid_control(self, robot_id, cmd): 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) print("robot grid pos before move: ", robot)
robot.move(cmd) robot.move(cmd)
print("robot grid pos after move: ", robot) 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) self.controller.move_to_pos(target, robot)
def main(args): def main(args):
rc = RemoteController() rc = RemoteController()
rc.run() rc.run()
if __name__ == '__main__': if __name__ == '__main__':
main(sys.argv) main(sys.argv)

6
remote_control/robot.py
View File

@ -15,21 +15,23 @@ class Robot:
self.u1 = 0.0 self.u1 = 0.0
self.u2 = 0.0 self.u2 = 0.0
self.connected = False
def connect(self): def connect(self):
# connect to robot # connect to robot
try: try:
print("connecting to robot {} with ip {} ...".format(self.id, self.ip)) print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
self.socket.connect((self.ip, 1234)) # connect to robot self.socket.connect((self.ip, 1234)) # connect to robot
print("connected!") print("connected!")
self.connected = True
except socket.error: except socket.error:
print("could not connect to robot {} with ip {}".format(self.id, self.ip)) 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): def send_cmd(self, u1=0.0, u2=0.0):
if self.socket: if self.socket:
try: try:
self.socket.send(f'({u1},{u2})\n'.encode()) self.socket.send(f'({u1},{u2})\n'.encode())
except BrokenPipeError: 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 pass