added option to dynamically initialize grid and robots and added support for arbitrary grid orientation w.r.t. camera
This commit is contained in:
parent
056d91da52
commit
e93ae65e0f
|
|
@ -17,7 +17,7 @@ class MPCController:
|
|||
|
||||
# integrator
|
||||
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):
|
||||
near_target = 0
|
||||
|
|
@ -59,42 +59,45 @@ class MPCController:
|
|||
|
||||
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 x_pred is not None:
|
||||
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)
|
||||
#if error_pos > 0.075 or error_ang > 0.35:
|
||||
if error_pos > 0.05 or error_ang > 0.1:
|
||||
# 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))
|
||||
#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)
|
||||
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
|
||||
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
|
||||
print("robot not detected yet!")
|
||||
|
||||
def interactive_control(self, robots):
|
||||
controlled_robot_number = 0
|
||||
|
|
@ -169,4 +172,4 @@ class MPCController:
|
|||
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 self.estimator.get_robot_state_estimate(robot_id)
|
||||
|
|
@ -6,8 +6,6 @@ from shapely.geometry import LineString
|
|||
from queue import Queue
|
||||
|
||||
class ArucoEstimator:
|
||||
grid_columns = 10
|
||||
grid_rows = 8
|
||||
corner_marker_ids = {
|
||||
'a': 0,
|
||||
'b': 1,
|
||||
|
|
@ -24,7 +22,10 @@ class ArucoEstimator:
|
|||
'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:
|
||||
robot_marker_ids = []
|
||||
self.robot_marker_ids = robot_marker_ids
|
||||
|
|
@ -248,7 +249,7 @@ class ArucoEstimator:
|
|||
elif orientation == 'v':
|
||||
angle = x_frac * angle_ad + (1 - x_frac) * angle_bc
|
||||
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))
|
||||
else:
|
||||
|
|
@ -315,11 +316,13 @@ class ArucoEstimator:
|
|||
def get_robot_state_estimate(self, id):
|
||||
if id in self.robot_marker_estimates:
|
||||
if self.robot_marker_estimates[id] is not None:
|
||||
return self.robot_marker_estimates[id]
|
||||
return np.array(self.robot_marker_estimates[id])
|
||||
else:
|
||||
print(f"error: no estimate available for robot {id}")
|
||||
return None
|
||||
else:
|
||||
print(f"error: invalid robot id {id}")
|
||||
return None
|
||||
|
||||
def draw_robot_pos(self, frame, corners, ids):
|
||||
# draws information about the robot positions onto the given frame
|
||||
|
|
|
|||
|
|
@ -4,10 +4,12 @@ import threading
|
|||
import time
|
||||
|
||||
from mpc_controller import MPCController
|
||||
from robot import Robot
|
||||
|
||||
import opencv_viewer_example
|
||||
|
||||
MSGLEN = 64
|
||||
|
||||
def myreceive(sock):
|
||||
chunks = []
|
||||
bytes_recd = 0
|
||||
|
|
@ -24,7 +26,7 @@ def myreceive(sock):
|
|||
return b''.join(chunks)
|
||||
|
||||
|
||||
class Robot:
|
||||
class RoboRallyRobot(Robot):
|
||||
# dictionary mapping the current orientation and a turn command to the resulting orientation
|
||||
resulting_orientation = {
|
||||
'^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'},
|
||||
|
|
@ -36,94 +38,65 @@ class Robot:
|
|||
# dictionary mapping an orientation to its opposite
|
||||
opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'}
|
||||
|
||||
def __init__(self, id, ip, x=0, y=0, orientation='>'):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.orientation = orientation
|
||||
def __init__(self, id, ip, x, y, orientation):
|
||||
super().__init__(id, ip)
|
||||
|
||||
self.id = 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
|
||||
self.grid_x = x
|
||||
self.grid_y = y
|
||||
self.grid_orientation = orientation
|
||||
|
||||
def get_neighbor_coordinates(self, direction):
|
||||
# get the coordinates of the neighboring tile in the given direction
|
||||
if direction == '^':
|
||||
return (self.x, self.y - 1)
|
||||
return self.grid_x, self.grid_y - 1
|
||||
elif direction == '>':
|
||||
return (self.x + 1, self.y)
|
||||
return self.grid_x + 1, self.grid_y
|
||||
elif direction == 'v':
|
||||
return (self.x, self.y + 1)
|
||||
return self.grid_x, self.grid_y + 1
|
||||
elif direction == '<':
|
||||
return (self.x - 1, self.y)
|
||||
return self.grid_x - 1, self.grid_y
|
||||
else:
|
||||
print("error: unknown direction")
|
||||
sys.exit(1)
|
||||
|
||||
def move(self, type):
|
||||
if type == 'forward':
|
||||
target_tile = self.get_neighbor_coordinates(self.orientation)
|
||||
self.x = target_tile[0]
|
||||
self.y = target_tile[1]
|
||||
elif type == 'backward':
|
||||
opposite_orientation = Robot.opposites[self.orientation]
|
||||
def move(self, move_type):
|
||||
if move_type == 'forward':
|
||||
target_tile = self.get_neighbor_coordinates(self.grid_orientation)
|
||||
self.grid_x = target_tile[0]
|
||||
self.grid_y = target_tile[1]
|
||||
elif move_type == 'backward':
|
||||
opposite_orientation = RoboRallyRobot.opposites[self.grid_orientation]
|
||||
target_tile = self.get_neighbor_coordinates(opposite_orientation)
|
||||
self.x = target_tile[0]
|
||||
self.y = target_tile[1]
|
||||
elif 'turn' in type:
|
||||
self.orientation = Robot.resulting_orientation[self.orientation][type]
|
||||
self.grid_x = target_tile[0]
|
||||
self.grid_y = target_tile[1]
|
||||
elif 'turn' in move_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:
|
||||
print("error: invalid move")
|
||||
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):
|
||||
return self.__repr__()
|
||||
|
||||
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:
|
||||
valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'nop', 'get position',
|
||||
'set position', 'initialize_robot', 'initialize_grid']
|
||||
|
||||
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))]
|
||||
#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.robots = []
|
||||
self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')]
|
||||
|
||||
self.robot_ids = {}
|
||||
for r in self.robots:
|
||||
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
|
||||
self.comm_socket = socket.socket()
|
||||
self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
|
|
@ -143,6 +116,14 @@ class RemoteController:
|
|||
|
||||
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):
|
||||
print("waiting until all markers are detected...")
|
||||
while not self.estimator.all_corners_detected():
|
||||
|
|
@ -165,10 +146,28 @@ class RemoteController:
|
|||
cmd = cmd.strip().decode()
|
||||
|
||||
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:
|
||||
robot_id = int(inputs[1])
|
||||
except ValueError:
|
||||
robot_id = None
|
||||
print("could not read robot id!")
|
||||
clientsocket.sendall(b'Could not read robot id!\n')
|
||||
|
||||
|
|
@ -178,7 +177,8 @@ class RemoteController:
|
|||
elif cmd == b'set position':
|
||||
try:
|
||||
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
|
||||
clientsocket.sendall(b'OK\n')
|
||||
except IndexError as e:
|
||||
|
|
@ -188,17 +188,58 @@ class RemoteController:
|
|||
self.robot_ids[robot_id].grid_pos)))
|
||||
except ValueError as e:
|
||||
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:
|
||||
self.grid_control(robot_id, cmd)
|
||||
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:
|
||||
print("invalid robot id!")
|
||||
clientsocket.sendall(b'Invalid robot id!\n')
|
||||
|
||||
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()
|
||||
|
|
@ -214,20 +255,21 @@ class RemoteController:
|
|||
clientsocket.close()
|
||||
|
||||
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)
|
||||
robot.move(cmd)
|
||||
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)
|
||||
|
||||
|
||||
def main(args):
|
||||
rc = RemoteController()
|
||||
|
||||
rc.run()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main(sys.argv)
|
||||
|
|
|
|||
|
|
@ -15,21 +15,23 @@ class Robot:
|
|||
self.u1 = 0.0
|
||||
self.u2 = 0.0
|
||||
|
||||
self.connected = False
|
||||
|
||||
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!")
|
||||
self.connected = True
|
||||
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")
|
||||
print(f"error: connection to robot {self.id} with ip {self.ip} lost")
|
||||
pass
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user