Compare commits
No commits in common. "67405fbd3f49eebef0682d081c894a03ad8b2795" and "843b30f5d3d1651a2335066988307f470954cc29" have entirely different histories.
67405fbd3f
...
843b30f5d3
|
@ -1,29 +0,0 @@
|
||||||
import machine
|
|
||||||
|
|
||||||
class Motor:
|
|
||||||
def __init__(self, enable, dir1, dir2):
|
|
||||||
self.enable_pin = machine.Pin(enable, machine.Pin.OUT)
|
|
||||||
self.enable_pwm = machine.PWM(self.enable_pin)
|
|
||||||
self.enable_pwm.freq(250)
|
|
||||||
|
|
||||||
self.dir1_pin = machine.Pin(dir1, machine.Pin.OUT)
|
|
||||||
self.dir2_pin = machine.Pin(dir2, machine.Pin.OUT)
|
|
||||||
|
|
||||||
self.direction = 1 # default direction: dir1_pin = HIGH, dir2_pin = LOW
|
|
||||||
self.reverse()
|
|
||||||
|
|
||||||
def reverse(self):
|
|
||||||
self.direction = not self.direction
|
|
||||||
self.dir1_pin.value(self.direction)
|
|
||||||
self.dir2_pin.value(not self.direction)
|
|
||||||
|
|
||||||
def speed(self, value):
|
|
||||||
if value > 0: # forward
|
|
||||||
if not self.direction: # switch direction if necessary
|
|
||||||
self.reverse()
|
|
||||||
else: # backward
|
|
||||||
if self.direction: # switch direction if necessary
|
|
||||||
self.reverse()
|
|
||||||
# apply value as pwm signal
|
|
||||||
self.enable_pwm.duty(int(abs(value)*10.23))
|
|
||||||
|
|
|
@ -2,46 +2,43 @@ import machine
|
||||||
import sys
|
import sys
|
||||||
from machine import I2C, Pin
|
from machine import I2C, Pin
|
||||||
|
|
||||||
i2c = False
|
import d1motor
|
||||||
if i2c:
|
|
||||||
import d1motor
|
|
||||||
else:
|
|
||||||
import l293motor
|
|
||||||
|
|
||||||
import time
|
import utime
|
||||||
import usocket
|
import usocket
|
||||||
|
import uselect
|
||||||
import esp
|
import esp
|
||||||
|
|
||||||
class Robot:
|
class Robot:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
if i2c:
|
print("setting up I2C ...")
|
||||||
print("setting up I2C ...")
|
d1 = Pin(5)
|
||||||
#d1 = Pin(5)
|
d2 = Pin(4)
|
||||||
#d2 = Pin(4)
|
i2c = I2C(scl=d1, sda=d2)
|
||||||
#i2c = I2C(scl=d1, sda=d2)
|
i2c_scan = i2c.scan()
|
||||||
#i2c_scan = i2c.scan()
|
if len(i2c_scan) > 0:
|
||||||
i2c_scan = []
|
i2c_addr = i2c_scan[0]
|
||||||
if len(i2c_scan) > 0:
|
print("i2c scan = {}".format(i2c_addr))
|
||||||
i2c_addr = i2c_scan[0]
|
print("setting up motors ...")
|
||||||
print("i2c scan = {}".format(i2c_addr))
|
self.m1 = d1motor.Motor(0, i2c, address=i2c_addr)
|
||||||
print("setting up motors ...")
|
self.m2 = d1motor.Motor(1, i2c, address=i2c_addr)
|
||||||
self.m1 = d1motor.Motor(0, i2c, address=i2c_addr)
|
self.m1.speed(0)
|
||||||
self.m2 = d1motor.Motor(1, i2c, address=i2c_addr)
|
self.m2.speed(0)
|
||||||
self.m1.speed(0)
|
print("motor setup complete!")
|
||||||
self.m2.speed(0)
|
|
||||||
print("motor setup complete!")
|
|
||||||
else:
|
|
||||||
print("error: no i2c interfaces found!")
|
|
||||||
sys.exit(1)
|
|
||||||
else:
|
else:
|
||||||
print("setting up L293D motor")
|
print("error: no i2c interfaces found!")
|
||||||
self.m1 = l293motor.Motor(14, 13, 12)
|
sys.exit(1)
|
||||||
self.m2 = l293motor.Motor(5, 0, 4)
|
|
||||||
|
|
||||||
ip = my_ip[0]
|
ip = my_ip[0]
|
||||||
# setup socket for remote control
|
# setup socket for remote control
|
||||||
self.addr = usocket.getaddrinfo(ip, 1234)[0][-1]
|
self.addr = usocket.getaddrinfo(ip, 1234)[0][-1]
|
||||||
|
|
||||||
|
self.poller = uselect.poll()
|
||||||
|
self.poller_timeout = 2 # timeout in ms
|
||||||
|
|
||||||
|
self.control_queue = []
|
||||||
|
|
||||||
|
|
||||||
def remote_control(self):
|
def remote_control(self):
|
||||||
while True:
|
while True:
|
||||||
print("setting up socket communication ...")
|
print("setting up socket communication ...")
|
||||||
|
@ -55,51 +52,124 @@ class Robot:
|
||||||
socket_setup_complete = True
|
socket_setup_complete = True
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(e))
|
print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(e))
|
||||||
time.sleep(1.0)
|
utime.sleep(1.0)
|
||||||
|
|
||||||
print("waiting for connections on {} ...".format(self.addr))
|
print("waiting for connections on {} ...".format(self.addr))
|
||||||
socket.listen(1)
|
socket.listen(1)
|
||||||
res = socket.accept() # this blocks until someone connects to the socket
|
res = socket.accept() # this blocks until someone connects to the socket
|
||||||
comm_socket = res[0]
|
comm_socket = res[0]
|
||||||
|
self.poller.register(comm_socket, uselect.POLLIN)
|
||||||
print("connected!")
|
print("connected!")
|
||||||
listening = True
|
listening = True
|
||||||
try:
|
|
||||||
while listening:
|
duration_current = 0
|
||||||
# expected data: '(t, u1, u2)'\n"
|
t_current = utime.ticks_ms()
|
||||||
# where ui = control for motor i
|
duration_next = None
|
||||||
# ui \in [-1.0, 1.0]
|
stopped = True
|
||||||
|
timeouts = 0
|
||||||
|
while listening:
|
||||||
|
elapsed = utime.ticks_ms()
|
||||||
|
remaining = duration_current - (elapsed-t_current)
|
||||||
|
if remaining >= 0:
|
||||||
|
timeouts = 0
|
||||||
|
print("start of loop\n I have {} ms until next control needs to be applied".format(remaining))
|
||||||
|
else:
|
||||||
|
# current control timed out -> applying next control
|
||||||
|
if len(self.control_queue) > 0:
|
||||||
|
print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current))
|
||||||
|
u_next = self.control_queue.pop(0)
|
||||||
|
#print("duration of previous control = {}".format((elapsed - t_current)/1000.0))
|
||||||
|
#print("applying new control (duration, u1, u2) = ({}, {}, {})".format(duration_next, u1_next, u2_next))
|
||||||
|
# if so, apply it
|
||||||
|
self.m1.speed(u_next[0])
|
||||||
|
self.m2.speed(u_next[1])
|
||||||
|
|
||||||
|
t_current = utime.ticks_ms()
|
||||||
|
duration_current = duration_next
|
||||||
|
|
||||||
|
stopped = False
|
||||||
|
elif not stopped:
|
||||||
|
print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current))
|
||||||
|
#print("duration of previous control = {}".format((elapsed - t_current)/1000.0))
|
||||||
|
# no new control available -> shutdown
|
||||||
|
print("no new control available -> stopping")
|
||||||
|
|
||||||
|
self.m1.speed(0)
|
||||||
|
self.m2.speed(0)
|
||||||
|
|
||||||
|
t_current = utime.ticks_ms()
|
||||||
|
duration_current = 0 # as soon as new control will become available we directly want to apply it immediately
|
||||||
|
|
||||||
|
stopped = True
|
||||||
|
|
||||||
|
#elif timeouts < 10:
|
||||||
|
# print("start of loop\n I have {} ms until next control needs to be applied, timeouts = {}".format(remaining, timeouts))
|
||||||
|
# timeouts = timeouts + 1
|
||||||
|
|
||||||
|
trecv_start = utime.ticks_ms()
|
||||||
|
|
||||||
|
# expected data: '(t, u1_0, u2_0, u1_1, u2_1, ...)'\n"
|
||||||
|
# where ui = control for motor i
|
||||||
|
# ui \in [-1.0, 1.0]
|
||||||
|
#print("poller waiting..")
|
||||||
|
poll_res = self.poller.poll(self.poller_timeout) # wait 100 milliseconds for socket data
|
||||||
|
if poll_res:
|
||||||
|
print("new data available")
|
||||||
|
try:
|
||||||
data = comm_socket.readline()
|
data = comm_socket.readline()
|
||||||
data_str = data.decode()
|
data_str = data.decode()
|
||||||
print("Data received: {}".format(data_str))
|
#print("Data received: {}".format(data_str))
|
||||||
print("processing data = {}".format(data_str))
|
#print("processing data = {}".format(data_str))
|
||||||
l = data_str.strip('()\n').split(',')
|
l = data_str.strip('()\n').split(',')
|
||||||
print("l = {}".format(l))
|
#print("l = {}".format(l))
|
||||||
u1 = int(float(l[0])*100)
|
duration_next = int(float(l[0])*1000)
|
||||||
print("u1 = {}".format(u1))
|
#print("duration = {}".format(duration_next))
|
||||||
u2 = int(float(l[1])*100)
|
self.control_queue = []
|
||||||
print("u2 = {}".format(u2))
|
print("putting data into queue")
|
||||||
|
for i in range((len(l)-1)/2):
|
||||||
self.m1.speed(u1)
|
u1_next = int(float(l[2*i+1])*100)
|
||||||
self.m2.speed(u2)
|
print("u1 = {}".format(u1_next))
|
||||||
except ValueError:
|
u2_next = int(float(l[2*i+2])*100)
|
||||||
print("ValueError: Data has wrong format.")
|
print("u2 = {}".format(u2_next))
|
||||||
print("Data received: {}".format(data_str))
|
self.control_queue.append((u1_next, u2_next))
|
||||||
except IndexError:
|
except ValueError:
|
||||||
print("IndexError: Data has wrong format.")
|
print("ValueError: Data has wrong format.")
|
||||||
print("Data received: {}".format(data_str))
|
print("Data received: {}".format(data_str))
|
||||||
except Exception as e:
|
print("Shutting down ...")
|
||||||
print("Some other error occured")
|
self.control_queue = []
|
||||||
print("Exception: {}".format(e))
|
duration_current = 0
|
||||||
finally:
|
listening = False
|
||||||
print("Shutting down ...")
|
comm_socket.close()
|
||||||
u1 = u2 = 0
|
socket.close()
|
||||||
self.m1.speed(u1)
|
del comm_socket
|
||||||
self.m2.speed(u2)
|
del socket
|
||||||
listening = False
|
print("disconnected!")
|
||||||
comm_socket.close()
|
except IndexError:
|
||||||
socket.close()
|
print("IndexError: Data has wrong format.")
|
||||||
del comm_socket
|
print("Data received: {}".format(data_str))
|
||||||
del socket
|
print("Shutting down ...")
|
||||||
|
self.control_queue = []
|
||||||
|
duration_current = 0
|
||||||
|
listening = False
|
||||||
|
comm_socket.close()
|
||||||
|
socket.close()
|
||||||
|
del comm_socket
|
||||||
|
del socket
|
||||||
|
print("disconnected!")
|
||||||
|
except Exception as e:
|
||||||
|
print("Some other error occured")
|
||||||
|
print("Exception: {}".format(e))
|
||||||
|
print("Shutting down ...")
|
||||||
|
self.control_queue = []
|
||||||
|
duration_current = 0
|
||||||
|
listening = False
|
||||||
|
comm_socket.close()
|
||||||
|
socket.close()
|
||||||
|
del comm_socket
|
||||||
|
del socket
|
||||||
|
print("disconnected!")
|
||||||
|
trecv_end = utime.ticks_ms()
|
||||||
|
print("communication (incl. polling) took {} ms".format(trecv_end - trecv_start))
|
||||||
|
|
||||||
wall_e = Robot()
|
wall_e = Robot()
|
||||||
wall_e.remote_control()
|
wall_e.remote_control()
|
||||||
|
|
|
@ -4,15 +4,13 @@ import time
|
||||||
# look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
|
# look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
|
||||||
|
|
||||||
class OpenLoopSolver:
|
class OpenLoopSolver:
|
||||||
def __init__(self, N=20, T=4.0):
|
def __init__(self, N=10, T=2.0):
|
||||||
self.T = T
|
self.T = T
|
||||||
self.N = N
|
self.N = N
|
||||||
|
|
||||||
self.opti_x0 = None
|
self.opti_x0 = None
|
||||||
self.opti_lam_g0 = None
|
self.opti_lam_g0 = None
|
||||||
|
|
||||||
self.use_warmstart = True
|
|
||||||
|
|
||||||
def setup(self):
|
def setup(self):
|
||||||
x = SX.sym('x')
|
x = SX.sym('x')
|
||||||
y = SX.sym('y')
|
y = SX.sym('y')
|
||||||
|
@ -137,9 +135,6 @@ class OpenLoopSolver:
|
||||||
#plt.show()
|
#plt.show()
|
||||||
#return
|
#return
|
||||||
|
|
||||||
|
|
||||||
def solve(self, x0, target, obstacles):
|
|
||||||
tstart = time.time()
|
|
||||||
# alternative solution using multiple shooting (way faster!)
|
# alternative solution using multiple shooting (way faster!)
|
||||||
self.opti = Opti() # Optimization problem
|
self.opti = Opti() # Optimization problem
|
||||||
|
|
||||||
|
@ -148,8 +143,6 @@ class OpenLoopSolver:
|
||||||
self.Q = self.opti.variable(1,self.N+1) # state trajectory
|
self.Q = self.opti.variable(1,self.N+1) # state trajectory
|
||||||
|
|
||||||
self.U = self.opti.variable(2,self.N) # control trajectory (throttle)
|
self.U = self.opti.variable(2,self.N) # control trajectory (throttle)
|
||||||
|
|
||||||
self.slack = self.opti.variable(1,1)
|
|
||||||
#T = self.opti.variable() # final time
|
#T = self.opti.variable() # final time
|
||||||
|
|
||||||
# ---- objective ---------
|
# ---- objective ---------
|
||||||
|
@ -164,6 +157,10 @@ class OpenLoopSolver:
|
||||||
#self.opti.set_initial(speed, 1)
|
#self.opti.set_initial(speed, 1)
|
||||||
#self.opti.set_initial(T, 1)
|
#self.opti.set_initial(T, 1)
|
||||||
|
|
||||||
|
|
||||||
|
def solve(self, x0, target):
|
||||||
|
|
||||||
|
tstart = time.time()
|
||||||
x = SX.sym('x')
|
x = SX.sym('x')
|
||||||
y = SX.sym('y')
|
y = SX.sym('y')
|
||||||
theta = SX.sym('theta')
|
theta = SX.sym('theta')
|
||||||
|
@ -208,12 +205,12 @@ class OpenLoopSolver:
|
||||||
q_next = self.Q[:, k] + dt / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
|
q_next = self.Q[:, k] + dt / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
|
||||||
self.opti.subject_to(self.X[:, k + 1] == x_next) # close the gaps
|
self.opti.subject_to(self.X[:, k + 1] == x_next) # close the gaps
|
||||||
self.opti.subject_to(self.Q[:, k + 1] == q_next) # close the gaps
|
self.opti.subject_to(self.Q[:, k + 1] == q_next) # close the gaps
|
||||||
self.opti.minimize(self.Q[:, self.N] + 1.0e5 * self.slack**2)
|
self.opti.minimize(self.Q[:, self.N])
|
||||||
|
|
||||||
# ---- path constraints -----------
|
# ---- path constraints -----------
|
||||||
# limit = lambda pos: 1-sin(2*pi*pos)/2
|
# limit = lambda pos: 1-sin(2*pi*pos)/2
|
||||||
# self.opti.subject_to(speed<=limit(pos)) # track speed limit
|
# self.opti.subject_to(speed<=limit(pos)) # track speed limit
|
||||||
maxcontrol = 0.95
|
maxcontrol = 0.5
|
||||||
self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol)) # control is limited
|
self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol)) # control is limited
|
||||||
|
|
||||||
# ---- boundary conditions --------
|
# ---- boundary conditions --------
|
||||||
|
@ -230,12 +227,10 @@ class OpenLoopSolver:
|
||||||
# self.opti.subject_to(X[2,:]>=-2) # Time must be positive
|
# self.opti.subject_to(X[2,:]>=-2) # Time must be positive
|
||||||
|
|
||||||
# avoid obstacle
|
# avoid obstacle
|
||||||
for o in obstacles:
|
# r = 0.25
|
||||||
p = obstacles[o].pos
|
# p = (0.5, 0.5)
|
||||||
r = obstacles[o].radius
|
# for k in range(self.N):
|
||||||
if p is not None:
|
# self.opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2)
|
||||||
for k in range(1,self.N):
|
|
||||||
self.opti.subject_to((self.X[0,k]-p[0])**2 + (self.X[1,k]-p[1])**2 + self.slack > r**2)
|
|
||||||
# pass
|
# pass
|
||||||
posx = self.X[0, :]
|
posx = self.X[0, :]
|
||||||
posy = self.X[1, :]
|
posy = self.X[1, :]
|
||||||
|
@ -246,17 +241,13 @@ class OpenLoopSolver:
|
||||||
self.opti.subject_to(angle[0] == x0[2]) # finish line at position 1
|
self.opti.subject_to(angle[0] == x0[2]) # finish line at position 1
|
||||||
tend = time.time()
|
tend = time.time()
|
||||||
|
|
||||||
print("setting up problem took {} seconds".format(tend - tstart))
|
print("setting up problem took {} seconds".format(tend - tstart))
|
||||||
|
|
||||||
tstart = time.time()
|
if self.opti_x0 is not None:
|
||||||
if self.use_warmstart and self.opti_x0 is not None:
|
|
||||||
self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
|
self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
|
||||||
self.opti.set_initial(self.opti.x, self.opti_x0)
|
self.opti.set_initial(self.opti.x, self.opti_x0)
|
||||||
sol = self.opti.solve() # actual solve
|
sol = self.opti.solve() # actual solve
|
||||||
tend = time.time()
|
|
||||||
print("solving the problem took {} seconds".format(tend - tstart))
|
|
||||||
|
|
||||||
tstart = time.time()
|
|
||||||
self.opti_x0 = sol.value(self.opti.x)
|
self.opti_x0 = sol.value(self.opti.x)
|
||||||
self.opti_lam_g0 = sol.value(self.opti.lam_g)
|
self.opti_lam_g0 = sol.value(self.opti.lam_g)
|
||||||
|
|
||||||
|
@ -264,10 +255,8 @@ class OpenLoopSolver:
|
||||||
#u_opt_2 = map(lambda x: float(x), [u_opt[i * 2 + 1] for i in range(0, 60)])
|
#u_opt_2 = map(lambda x: float(x), [u_opt[i * 2 + 1] for i in range(0, 60)])
|
||||||
u_opt_1 = sol.value(self.U[0,:])
|
u_opt_1 = sol.value(self.U[0,:])
|
||||||
u_opt_2 = sol.value(self.U[1,:])
|
u_opt_2 = sol.value(self.U[1,:])
|
||||||
tend = time.time()
|
|
||||||
print("postprocessing took {} seconds".format(tend - tstart))
|
|
||||||
|
|
||||||
return (u_opt_1, u_opt_2, sol.value(posx), sol.value(posy))
|
return (u_opt_1, u_opt_2)
|
||||||
|
|
||||||
#lam_g0 = sol.value(self.opti.lam_g)
|
#lam_g0 = sol.value(self.opti.lam_g)
|
||||||
|
|
||||||
|
|
|
@ -1,150 +0,0 @@
|
||||||
import socket
|
|
||||||
import pygame
|
|
||||||
import pygame.joystick
|
|
||||||
|
|
||||||
|
|
||||||
class robot: # we have 4 arg for this class, because joysticks get the same (value, axis) events
|
|
||||||
def __init__(self, joy, ip, port):
|
|
||||||
self.joy = joy
|
|
||||||
self.ip = ip
|
|
||||||
self.port = port
|
|
||||||
self.robot0_stopped_1 = True
|
|
||||||
self.robot0_stopped_2 = True
|
|
||||||
self.robot0_stopped_3 = True
|
|
||||||
self.robot0_stopped_4 = True
|
|
||||||
self.rc_socket = socket.socket()
|
|
||||||
try:
|
|
||||||
self.rc_socket.connect((self.ip, self.port))
|
|
||||||
except socket.error():
|
|
||||||
print("couldn't connect to socket")
|
|
||||||
|
|
||||||
def joystick_init(self): # Joystick's initialisation
|
|
||||||
self.joystick = pygame.joystick.Joystick(self.joy)
|
|
||||||
self.joystick.init()
|
|
||||||
self.axes = self.joystick.get_numaxes()
|
|
||||||
|
|
||||||
def control(self, event): # the control of two robots with joysticks
|
|
||||||
joy = event.joy
|
|
||||||
value = event.value
|
|
||||||
axis = event.axis
|
|
||||||
|
|
||||||
if joy == self.joy:
|
|
||||||
if axis == 1:
|
|
||||||
if abs(value) > 0.2:
|
|
||||||
u1 = u2 = -value
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_1 = False
|
|
||||||
|
|
||||||
elif not self.robot0_stopped_1:
|
|
||||||
u1 = u2 = 0
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_1 = True
|
|
||||||
elif axis == 3:
|
|
||||||
if abs(value) > 0.2:
|
|
||||||
u1 = value
|
|
||||||
u2 = -value
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_2 = False
|
|
||||||
|
|
||||||
elif not self.robot0_stopped_2:
|
|
||||||
u1 = u2 = 0
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_2 = True
|
|
||||||
elif axis == 2:
|
|
||||||
if value > 0.2:
|
|
||||||
u1 = value/1.9
|
|
||||||
u2 = value/1.2
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_3 = False
|
|
||||||
|
|
||||||
elif not self.robot0_stopped_3:
|
|
||||||
u1 = u2 = 0
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_3 = True
|
|
||||||
elif axis == 5:
|
|
||||||
if value > 0.2:
|
|
||||||
u1 = value/1.2
|
|
||||||
u2 = value/1.9
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_4 = False
|
|
||||||
|
|
||||||
elif not self.robot0_stopped_4:
|
|
||||||
u1 = u2 = 0
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
self.robot0_stopped_4 = True
|
|
||||||
|
|
||||||
def control_keyboard(self, event): # keyboard control for robot1
|
|
||||||
command_received = False
|
|
||||||
if event.key == pygame.K_LEFT:
|
|
||||||
u1 = -1.0
|
|
||||||
u2 = 1.0
|
|
||||||
command_received = True
|
|
||||||
elif event.key == pygame.K_RIGHT:
|
|
||||||
u1 = 1.0
|
|
||||||
u2 = -1.0
|
|
||||||
command_received = True
|
|
||||||
elif event.key == pygame.K_UP:
|
|
||||||
u1 = -1.0
|
|
||||||
u2 = -1.0
|
|
||||||
command_received = True
|
|
||||||
elif event.key == pygame.K_DOWN:
|
|
||||||
u1 = 1.0
|
|
||||||
u2 = 1.0
|
|
||||||
command_received = True
|
|
||||||
if command_received:
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
|
|
||||||
def control_keyboard_2(self, event): # keyboard control for robot2
|
|
||||||
|
|
||||||
command_received = False
|
|
||||||
if event.key == pygame.K_a:
|
|
||||||
u1 = -1.0
|
|
||||||
u2 = 1.0
|
|
||||||
command_received = True
|
|
||||||
elif event.key == pygame.K_d:
|
|
||||||
u1 = 1.0
|
|
||||||
u2 = -1.0
|
|
||||||
command_received = True
|
|
||||||
elif event.key == pygame.K_w:
|
|
||||||
u1 = -1.0
|
|
||||||
u2 = -1.0
|
|
||||||
command_received = True
|
|
||||||
elif event.key == pygame.K_s:
|
|
||||||
u1 = 1.0
|
|
||||||
u2 = 1.0
|
|
||||||
command_received = True
|
|
||||||
if command_received:
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
|
|
||||||
def control_keyboard_stop(self): # stop for both robot
|
|
||||||
|
|
||||||
u1 = 0
|
|
||||||
u2 = 0
|
|
||||||
print("key released, resetting: ({},{})".format(u1, u2))
|
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
|
|
||||||
|
|
||||||
def main():
|
|
||||||
pygame.init()
|
|
||||||
pygame.display.set_mode((640, 480))
|
|
||||||
|
|
||||||
robot_1 = robot(0, '192.168.1.102', 1234)
|
|
||||||
robot_1.joystick_init()
|
|
||||||
robot_2 = robot(1, '192.168.1.103', 1234)
|
|
||||||
robot_2.joystick_init()
|
|
||||||
|
|
||||||
while True:
|
|
||||||
events = pygame.event.get()
|
|
||||||
for event in events:
|
|
||||||
if event.type == pygame.JOYAXISMOTION:
|
|
||||||
robot_1.control(event)
|
|
||||||
robot_2.control(event)
|
|
||||||
elif event.type == pygame.KEYDOWN:
|
|
||||||
robot_1.control_keyboard(event)
|
|
||||||
robot_2.control_keyboard_2(event)
|
|
||||||
elif event.type == pygame.KEYUP:
|
|
||||||
robot_1.control_keyboard_stop()
|
|
||||||
robot_2.control_keyboard_stop()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
|
@ -7,9 +7,9 @@ pygame.display.set_mode((640, 480))
|
||||||
rc_socket = socket.socket()
|
rc_socket = socket.socket()
|
||||||
try:
|
try:
|
||||||
#rc_socket.connect(('192.168.4.1', 1234)) # connect to robot
|
#rc_socket.connect(('192.168.4.1', 1234)) # connect to robot
|
||||||
#rc_socket.connect(('192.168.1.101', 1234)) # connect to robot
|
rc_socket.connect(('192.168.1.101', 1234)) # connect to robot
|
||||||
#rc_socket.connect(('192.168.1.102', 1234)) # connect to robot
|
#rc_socket.connect(('192.168.1.102', 1234)) # connect to robot
|
||||||
rc_socket.connect(('192.168.1.103', 1234)) # connect to robot
|
#rc_socket.connect(('192.168.1.103', 1234)) # connect to robot
|
||||||
except socket.error:
|
except socket.error:
|
||||||
print("could not connect to socket")
|
print("could not connect to socket")
|
||||||
|
|
||||||
|
@ -17,7 +17,7 @@ except socket.error:
|
||||||
while True:
|
while True:
|
||||||
u1 = 0
|
u1 = 0
|
||||||
u2 = 0
|
u2 = 0
|
||||||
vmax = 1.0
|
vmax = 0.5
|
||||||
events = pygame.event.get()
|
events = pygame.event.get()
|
||||||
for event in events:
|
for event in events:
|
||||||
if event.type == pygame.KEYDOWN:
|
if event.type == pygame.KEYDOWN:
|
||||||
|
|
|
@ -18,7 +18,6 @@ from copy import deepcopy
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
import matplotlib.animation as anim
|
import matplotlib.animation as anim
|
||||||
import matplotlib.patches as patch
|
|
||||||
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
|
@ -38,12 +37,6 @@ class Robot:
|
||||||
|
|
||||||
self.ip = ip
|
self.ip = ip
|
||||||
|
|
||||||
class Obstacle:
|
|
||||||
def __init__(self, id, radius):
|
|
||||||
self.id = id
|
|
||||||
self.pos = None
|
|
||||||
self.radius = radius
|
|
||||||
|
|
||||||
def f_ode(t, x, u):
|
def f_ode(t, x, u):
|
||||||
# dynamical model of the two-wheeled robot
|
# dynamical model of the two-wheeled robot
|
||||||
# TODO: find exact values for these parameters
|
# TODO: find exact values for these parameters
|
||||||
|
@ -69,28 +62,19 @@ def f_ode(t, x, u):
|
||||||
class RemoteController:
|
class RemoteController:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
|
||||||
self.robots = [Robot(3, '192.168.1.103')]
|
self.robots = [Robot(5)]
|
||||||
|
|
||||||
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
|
||||||
|
|
||||||
obst = [Obstacle(6, 0.175), Obstacle(5, 0.175), Obstacle(8, 0.175)]
|
|
||||||
|
|
||||||
self.obstacles = {}
|
|
||||||
for r in obst:
|
|
||||||
self.obstacles[r.id] = r
|
|
||||||
|
|
||||||
# connect to robot
|
# connect to robot
|
||||||
self.rc_socket = socket.socket()
|
self.rc_socket = socket.socket()
|
||||||
#self.rc_socket = None
|
|
||||||
try:
|
try:
|
||||||
for r in self.robots:
|
pass
|
||||||
self.rc_socket.connect((r.ip, 1234)) # connect to robot
|
self.rc_socket.connect(('192.168.1.103', 1234)) # connect to robot
|
||||||
except socket.error:
|
except socket.error:
|
||||||
print("could not connect to socket")
|
print("could not connect to socket")
|
||||||
self.rc_socket = None
|
|
||||||
|
|
||||||
|
|
||||||
self.t = time.time()
|
self.t = time.time()
|
||||||
|
|
||||||
|
@ -105,19 +89,23 @@ class RemoteController:
|
||||||
self.tms = None
|
self.tms = None
|
||||||
self.xms = None
|
self.xms = None
|
||||||
|
|
||||||
# variable for mpc open loop
|
|
||||||
self.ol_x = None
|
|
||||||
self.ol_y = None
|
|
||||||
|
|
||||||
self.mutex = threading.Lock()
|
self.mutex = threading.Lock()
|
||||||
|
|
||||||
# ROS subscriber for detected markers
|
|
||||||
marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
|
marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
|
||||||
|
|
||||||
|
|
||||||
# pid parameters
|
# pid parameters
|
||||||
|
self.k = 0
|
||||||
|
self.ii = 0.1
|
||||||
|
self.pp = 0.4
|
||||||
|
|
||||||
|
self.inc = 0.0
|
||||||
|
|
||||||
|
self.alphas = []
|
||||||
|
|
||||||
|
self.speed = 1.0
|
||||||
self.controlling = False
|
self.controlling = False
|
||||||
|
|
||||||
# currently active control
|
|
||||||
self.u1 = 0.0
|
self.u1 = 0.0
|
||||||
self.u2 = 0.0
|
self.u2 = 0.0
|
||||||
|
|
||||||
|
@ -125,34 +113,19 @@ class RemoteController:
|
||||||
self.fig = plt.figure()
|
self.fig = plt.figure()
|
||||||
self.ax = self.fig.add_subplot(1,1,1)
|
self.ax = self.fig.add_subplot(1,1,1)
|
||||||
self.xdata, self.ydata = [], []
|
self.xdata, self.ydata = [], []
|
||||||
self.line, = self.ax.plot([],[], color='grey', linestyle=':')
|
self.line, = self.ax.plot([],[])
|
||||||
self.line_sim, = self.ax.plot([], [])
|
self.line_sim, = self.ax.plot([], [])
|
||||||
self.line_ol, = self.ax.plot([],[], color='green', linestyle='--')
|
|
||||||
self.dirm, = self.ax.plot([], [])
|
self.dirm, = self.ax.plot([], [])
|
||||||
self.dirs, = self.ax.plot([], [])
|
self.dirs, = self.ax.plot([], [])
|
||||||
|
|
||||||
self.circles = []
|
|
||||||
for o in self.obstacles:
|
|
||||||
self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--'))
|
|
||||||
|
|
||||||
for s in self.circles:
|
|
||||||
self.ax.add_artist(s)
|
|
||||||
|
|
||||||
plt.xlabel('x-position')
|
plt.xlabel('x-position')
|
||||||
plt.ylabel('y-position')
|
plt.ylabel('y-position')
|
||||||
plt.grid()
|
plt.grid()
|
||||||
|
|
||||||
self.ols = OpenLoopSolver()
|
self.ols = OpenLoopSolver()
|
||||||
self.ols.setup()
|
self.ols.setup()
|
||||||
self.dt = self.ols.T / self.ols.N
|
|
||||||
|
|
||||||
self.target = (0.0, 0.0, 0.0)
|
self.target = (0.0, 0.0, 0.0)
|
||||||
|
|
||||||
# integrator
|
|
||||||
self.r = scipy.integrate.ode(f_ode)
|
|
||||||
self.omega_max = 5.0
|
|
||||||
|
|
||||||
|
|
||||||
def ani(self):
|
def ani(self):
|
||||||
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
|
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
|
||||||
plt.ion()
|
plt.ion()
|
||||||
|
@ -163,7 +136,7 @@ class RemoteController:
|
||||||
self.ax.set_ylim(-2, 2)
|
self.ax.set_ylim(-2, 2)
|
||||||
self.ax.set_aspect('equal', adjustable='box')
|
self.ax.set_aspect('equal', adjustable='box')
|
||||||
|
|
||||||
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
|
return self.line, self.line_sim, self.dirm, self.dirs,
|
||||||
|
|
||||||
def ani_update(self, frame):
|
def ani_update(self, frame):
|
||||||
#print("plotting")
|
#print("plotting")
|
||||||
|
@ -182,8 +155,8 @@ class RemoteController:
|
||||||
a = xm_local[-1, 0]
|
a = xm_local[-1, 0]
|
||||||
b = xm_local[-1, 1]
|
b = xm_local[-1, 1]
|
||||||
|
|
||||||
a2 = a + np.cos(xm_local[-1, 2]) * 0.2
|
a2 = a + np.cos(xm_local[-1, 2]) * 1.0
|
||||||
b2 = b + np.sin(xm_local[-1, 2]) * 0.2
|
b2 = b + np.sin(xm_local[-1, 2]) * 1.0
|
||||||
|
|
||||||
self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
|
self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
|
||||||
|
|
||||||
|
@ -198,42 +171,26 @@ class RemoteController:
|
||||||
a = xs_local[-1, 0]
|
a = xs_local[-1, 0]
|
||||||
b = xs_local[-1, 1]
|
b = xs_local[-1, 1]
|
||||||
|
|
||||||
a2 = a + np.cos(xs_local[-1, 2]) * 0.2
|
a2 = a + np.cos(xs_local[-1, 2]) * 1.0
|
||||||
b2 = b + np.sin(xs_local[-1, 2]) * 0.2
|
b2 = b + np.sin(xs_local[-1, 2]) * 1.0
|
||||||
|
|
||||||
self.dirs.set_data(np.array([a, a2]), np.array([b, b2]))
|
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([],[])
|
|
||||||
|
|
||||||
i = 0
|
|
||||||
obst_keys = self.obstacles.keys()
|
|
||||||
for s in self.circles:
|
|
||||||
o = self.obstacles[obst_keys[i]]
|
|
||||||
i = i + 1
|
|
||||||
|
|
||||||
if o.pos is not None:
|
|
||||||
s.center = o.pos
|
|
||||||
s.radius = o.radius
|
|
||||||
|
|
||||||
finally:
|
finally:
|
||||||
self.mutex.release()
|
self.mutex.release()
|
||||||
|
|
||||||
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
|
return self.line, self.line_sim, self.dirm, self.dirs,
|
||||||
|
|
||||||
def measurement_callback(self, data):
|
def measurement_callback(self, data):
|
||||||
# detect robots
|
#print("data = {}".format(data))
|
||||||
if data.id in self.robot_ids:
|
if data.id in self.robot_ids:
|
||||||
r = self.robot_ids[data.id]
|
r = self.robot_ids[data.id]
|
||||||
|
|
||||||
r.pos = (data.x, data.y) # only x and y component are important for us
|
r.pos = (data.x, data.y) # only x and y component are important for us
|
||||||
r.euler = data.angle
|
r.euler = data.angle
|
||||||
|
|
||||||
|
#print("r.pos = {}".format(r.pos))
|
||||||
|
#print("r.angle = {}".format(r.euler))
|
||||||
|
|
||||||
# save measured position and angle for plotting
|
# save measured position and angle for plotting
|
||||||
measurement = np.array([r.pos[0], r.pos[1], r.euler])
|
measurement = np.array([r.pos[0], r.pos[1], r.euler])
|
||||||
if self.tms_0 is None:
|
if self.tms_0 is None:
|
||||||
|
@ -254,89 +211,290 @@ class RemoteController:
|
||||||
finally:
|
finally:
|
||||||
self.mutex.release()
|
self.mutex.release()
|
||||||
|
|
||||||
# detect obstacles
|
|
||||||
if data.id in self.obstacles.keys():
|
|
||||||
obst = (data.x, data.y)
|
|
||||||
self.obstacles[data.id].pos = obst
|
|
||||||
|
|
||||||
def controller(self):
|
def controller(self):
|
||||||
|
tgrid = None
|
||||||
|
us1 = None
|
||||||
|
us2 = None
|
||||||
|
u1 = -0.0
|
||||||
|
u2 = 0.0
|
||||||
|
|
||||||
|
r = scipy.integrate.ode(f_ode)
|
||||||
|
|
||||||
|
omega_max = 5.0
|
||||||
|
|
||||||
|
init_pos = None
|
||||||
|
init_time = None
|
||||||
|
final_pos = None
|
||||||
|
final_time = None
|
||||||
|
forward = True
|
||||||
print("starting control")
|
print("starting control")
|
||||||
while True:
|
while True:
|
||||||
|
|
||||||
# open loop controller
|
keyboard_control = False
|
||||||
events = pygame.event.get()
|
keyboard_control_speed_test = False
|
||||||
for event in events:
|
pid = False
|
||||||
if event.type == pygame.KEYDOWN:
|
open_loop_solve = True
|
||||||
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:
|
|
||||||
self.target = (0.0, 0.0, 0.0)
|
|
||||||
elif event.key == pygame.K_1:
|
|
||||||
self.target = (0.5,0.5, -np.pi/2.0)
|
|
||||||
elif event.key == pygame.K_2:
|
|
||||||
self.target = (0.5, -0.5, 0.0)
|
|
||||||
elif event.key == pygame.K_3:
|
|
||||||
self.target = (-0.5,-0.5, np.pi/2.0)
|
|
||||||
elif event.key == pygame.K_4:
|
|
||||||
self.target = (-0.5,0.5, 0.0)
|
|
||||||
|
|
||||||
if self.controlling:
|
if keyboard_control: # keyboard controller
|
||||||
x_pred = self.get_measurement_prediction()
|
events = pygame.event.get()
|
||||||
|
speed = 1.0
|
||||||
|
for event in events:
|
||||||
|
if event.type == pygame.KEYDOWN:
|
||||||
|
if event.key == pygame.K_LEFT:
|
||||||
|
self.u1 = -speed
|
||||||
|
self.u2 = speed
|
||||||
|
#print("turn left: ({},{})".format(u1, u2))
|
||||||
|
elif event.key == pygame.K_RIGHT:
|
||||||
|
self.u1 = speed
|
||||||
|
self.u2 = -speed
|
||||||
|
#print("turn right: ({},{})".format(u1, u2))
|
||||||
|
elif event.key == pygame.K_UP:
|
||||||
|
self.u1 = speed
|
||||||
|
self.u2 = speed
|
||||||
|
#print("forward: ({},{})".format(self.u1, self.u2))
|
||||||
|
elif event.key == pygame.K_DOWN:
|
||||||
|
self.u1 = -speed
|
||||||
|
self.u2 = -speed
|
||||||
|
#print("forward: ({},{})".format(u1, u2))
|
||||||
|
self.rc_socket.send('({},{},{})\n'.format(0.1, self.u1, self.u2))
|
||||||
|
elif event.type == pygame.KEYUP:
|
||||||
|
self.u1 = 0
|
||||||
|
self.u2 = 0
|
||||||
|
#print("key released, resetting: ({},{})".format(u1, u2))
|
||||||
|
self.rc_socket.send('({}, {},{})\n'.format(0.1, self.u1, self.u2))
|
||||||
|
|
||||||
tmpc_start = time.time()
|
tnew = time.time()
|
||||||
|
dt = tnew - self.t
|
||||||
|
r = scipy.integrate.ode(f_ode)
|
||||||
|
r.set_f_params(np.array([self.u1 * omega_max, self.u2 * omega_max]))
|
||||||
|
|
||||||
# solve mpc open loop problem
|
#print(self.x0)
|
||||||
res = self.ols.solve(x_pred, self.target, self.obstacles)
|
if self.x0 is None:
|
||||||
|
if self.xm_0 is not None:
|
||||||
|
self.x0 = self.xm_0
|
||||||
|
self.xs = self.x0
|
||||||
|
else:
|
||||||
|
print("error: no measurement available to initialize simulation")
|
||||||
|
x = self.x0
|
||||||
|
r.set_initial_value(x, self.t)
|
||||||
|
xnew = r.integrate(r.t + dt)
|
||||||
|
|
||||||
us1 = res[0]
|
self.t = tnew
|
||||||
us2 = res[1]
|
self.x0 = xnew
|
||||||
|
|
||||||
# save open loop trajectories for plotting
|
|
||||||
self.mutex.acquire()
|
self.mutex.acquire()
|
||||||
try:
|
try:
|
||||||
self.ol_x = res[2]
|
self.ts = np.append(self.ts, tnew)
|
||||||
self.ol_y = res[3]
|
self.xs = np.vstack((self.xs, xnew))
|
||||||
finally:
|
finally:
|
||||||
self.mutex.release()
|
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)
|
|
||||||
|
|
||||||
# send controls to the robot
|
elif keyboard_control_speed_test:
|
||||||
for i in range(0, 1): # option to use multistep mpc if len(range) > 1
|
events = pygame.event.get()
|
||||||
u1 = us1[i]
|
for event in events:
|
||||||
u2 = us2[i]
|
if event.type == pygame.KEYDOWN:
|
||||||
if self.rc_socket:
|
if event.key == pygame.K_1:
|
||||||
self.rc_socket.send('({},{})\n'.format(u1, u2))
|
self.controlling = True
|
||||||
self.t = time.time() # save time the most recent control was applied
|
forward = True
|
||||||
|
print("starting test")
|
||||||
|
self.mutex.acquire()
|
||||||
|
try:
|
||||||
|
init_pos = copy.deepcopy(self.xms[-1])
|
||||||
|
init_time = copy.deepcopy(self.tms[-1])
|
||||||
|
finally:
|
||||||
|
self.mutex.release()
|
||||||
|
if event.key == pygame.K_2:
|
||||||
|
self.controlling = True
|
||||||
|
forward = False
|
||||||
|
print("starting test")
|
||||||
|
self.mutex.acquire()
|
||||||
|
try:
|
||||||
|
init_pos = copy.deepcopy(self.xms[-1])
|
||||||
|
init_time = copy.deepcopy(self.tms[-1])
|
||||||
|
finally:
|
||||||
|
self.mutex.release()
|
||||||
|
elif event.key == pygame.K_3:
|
||||||
|
self.controlling = False
|
||||||
|
print("stopping test")
|
||||||
|
self.rc_socket.send('(0.1, 0.0,0.0)\n')
|
||||||
|
|
||||||
def get_measurement_prediction(self):
|
self.mutex.acquire()
|
||||||
# get measurement
|
try:
|
||||||
self.mutex.acquire()
|
final_pos = copy.deepcopy(self.xms[-1])
|
||||||
try:
|
final_time = copy.deepcopy(self.tms[-1])
|
||||||
last_measurement = copy.deepcopy(self.xms[-1])
|
finally:
|
||||||
last_time = copy.deepcopy(self.tms[-1])
|
self.mutex.release()
|
||||||
finally:
|
|
||||||
self.mutex.release()
|
|
||||||
|
|
||||||
# prediction of state at time the mpc will terminate
|
print("init_pos = {}".format(init_pos))
|
||||||
self.r.set_f_params(np.array([self.u1 * self.omega_max, self.u2 * self.omega_max]))
|
print("final_pos = {}".format(final_pos))
|
||||||
|
print("distance = {}".format(np.linalg.norm(init_pos[0:2]-final_pos[0:2])))
|
||||||
|
print("dt = {}".format(final_time - init_time))
|
||||||
|
|
||||||
self.r.set_initial_value(last_measurement, last_time)
|
d = np.linalg.norm(init_pos[0:2]-final_pos[0:2])
|
||||||
|
t = final_time - init_time
|
||||||
|
r = 0.03
|
||||||
|
|
||||||
x_pred = self.r.integrate(self.r.t + self.dt)
|
angular_velocity = d/r/t
|
||||||
|
print("average angular velocity = {}".format(angular_velocity))
|
||||||
|
|
||||||
return x_pred
|
|
||||||
|
|
||||||
|
if self.controlling:
|
||||||
|
if forward:
|
||||||
|
self.rc_socket.send('(0.1, 1.0,1.0)\n')
|
||||||
|
else:
|
||||||
|
self.rc_socket.send('(0.1, -1.0,-1.0)\n')
|
||||||
|
time.sleep(0.1)
|
||||||
|
#print("speed = {}".format(self.speed))
|
||||||
|
|
||||||
|
|
||||||
|
elif pid:
|
||||||
|
# pid controller
|
||||||
|
|
||||||
|
events = pygame.event.get()
|
||||||
|
for event in events:
|
||||||
|
if event.type == pygame.KEYDOWN:
|
||||||
|
if event.key == pygame.K_LEFT:
|
||||||
|
self.ii = self.ii / np.sqrt(np.sqrt(np.sqrt(10.0)))
|
||||||
|
print("ii = {}".format(self.pp))
|
||||||
|
elif event.key == pygame.K_RIGHT:
|
||||||
|
self.ii = self.ii * np.sqrt(np.sqrt(np.sqrt(10.0)))
|
||||||
|
print("ii = {}".format(self.pp))
|
||||||
|
elif event.key == pygame.K_UP:
|
||||||
|
self.controlling = True
|
||||||
|
elif event.key == pygame.K_DOWN:
|
||||||
|
self.controlling = False
|
||||||
|
self.rc_socket.send('({},{})\n'.format(0, 0))
|
||||||
|
|
||||||
|
dt = 0.05
|
||||||
|
|
||||||
|
if self.controlling:
|
||||||
|
# test: turn robot such that angle is zero
|
||||||
|
for r in self.robots:
|
||||||
|
if r.euler is not None:
|
||||||
|
self.k = self.k + 1
|
||||||
|
|
||||||
|
alpha = r.euler
|
||||||
|
self.alphas.append(alpha)
|
||||||
|
|
||||||
|
# compute error
|
||||||
|
e = alpha - 0
|
||||||
|
|
||||||
|
# compute integral of error (approximately)
|
||||||
|
self.inc += e * dt
|
||||||
|
|
||||||
|
# PID
|
||||||
|
p = self.pp * e
|
||||||
|
i = self.ii * self.inc
|
||||||
|
d = 0.0
|
||||||
|
|
||||||
|
# compute controls for robot from PID
|
||||||
|
u1 = p + i + d
|
||||||
|
u2 = - p - i - d
|
||||||
|
print("alpha = {}, u = ({}, {})".format(alpha, u1, u2))
|
||||||
|
self.rc_socket.send('({},{})\n'.format(u1, u2))
|
||||||
|
|
||||||
|
time.sleep(dt)
|
||||||
|
|
||||||
|
elif open_loop_solve:
|
||||||
|
# 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
|
||||||
|
self.rc_socket.send('(0.1, 0.0,0.0)\n')
|
||||||
|
elif event.key == pygame.K_0:
|
||||||
|
self.target = (0.0, 0.0, 0.0)
|
||||||
|
elif event.key == pygame.K_1:
|
||||||
|
self.target = (0.5,0.5, -np.pi/2.0)
|
||||||
|
elif event.key == pygame.K_2:
|
||||||
|
self.target = (0.5, -0.5, 0.0)
|
||||||
|
elif event.key == pygame.K_3:
|
||||||
|
self.target = (-0.5,-0.5, np.pi/2.0)
|
||||||
|
elif event.key == pygame.K_4:
|
||||||
|
self.target = (-0.5,0.5, 0.0)
|
||||||
|
if self.controlling:
|
||||||
|
tmpc_start = time.time()
|
||||||
|
# get measurement
|
||||||
|
self.mutex.acquire()
|
||||||
|
try:
|
||||||
|
last_measurement = copy.deepcopy(self.xms[-1])
|
||||||
|
last_time = copy.deepcopy(self.tms[-1])
|
||||||
|
finally:
|
||||||
|
self.mutex.release()
|
||||||
|
|
||||||
|
print("current measurement (t, x) = ({}, {})".format(last_time, last_measurement))
|
||||||
|
print("current control (u1, u2) = ({}, {})".format(u1, u2))
|
||||||
|
|
||||||
|
# prediction of state at time the mpc will terminate
|
||||||
|
r.set_f_params(np.array([u1 * omega_max, u2 * omega_max]))
|
||||||
|
|
||||||
|
r.set_initial_value(last_measurement, last_time)
|
||||||
|
dt = self.ols.T/self.ols.N
|
||||||
|
print("integrating for {} seconds".format((dt)))
|
||||||
|
x_pred = r.integrate(r.t + (dt))
|
||||||
|
|
||||||
|
print("predicted initial state x_pred = ({})".format(x_pred))
|
||||||
|
|
||||||
|
res = self.ols.solve(x_pred, self.target)
|
||||||
|
#tgrid = res[0]
|
||||||
|
us1 = res[0]
|
||||||
|
us2 = res[1]
|
||||||
|
|
||||||
|
# tt = 0
|
||||||
|
# x = last_measurement
|
||||||
|
# t_ol = np.array([tt])
|
||||||
|
# x_ol = np.array([x])
|
||||||
|
# # compute open loop prediction
|
||||||
|
# for i in range(len(us1)):
|
||||||
|
# r = scipy.integrate.ode(f_ode)
|
||||||
|
# r.set_f_params(np.array([us1[i] * 13.32, us2[i] * 13.32]))
|
||||||
|
# r.set_initial_value(x, tt)
|
||||||
|
#
|
||||||
|
# tt = tt + 0.1
|
||||||
|
# x = r.integrate(tt)
|
||||||
|
#
|
||||||
|
# t_ol = np.vstack((t_ol, tt))
|
||||||
|
# x_ol = np.vstack((x_ol, x))
|
||||||
|
|
||||||
|
#plt.figure(4)
|
||||||
|
#plt.plot(x_ol[:,0], x_ol[:,1])
|
||||||
|
|
||||||
|
|
||||||
|
#if event.key == pygame.K_DOWN:
|
||||||
|
# if tgrid is not None:
|
||||||
|
tmpc_end = time.time()
|
||||||
|
print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
||||||
|
dt_mpc = time.time() - self.t
|
||||||
|
if dt_mpc < dt:
|
||||||
|
print("sleeping for {} seconds...".format(dt - dt_mpc))
|
||||||
|
time.sleep(dt - dt_mpc)
|
||||||
|
|
||||||
|
self.mutex.acquire()
|
||||||
|
try:
|
||||||
|
second_measurement = copy.deepcopy(self.xms[-1])
|
||||||
|
second_time = copy.deepcopy(self.tms[-1])
|
||||||
|
finally:
|
||||||
|
self.mutex.release()
|
||||||
|
|
||||||
|
print("(last_time, second_time, dt) = ({}, {}, {})".format(last_time, second_time, second_time - last_time))
|
||||||
|
print("mismatch between predicted state and measured state: {}\n\n".format(second_measurement - last_measurement))
|
||||||
|
|
||||||
|
for i in range(0, 1):
|
||||||
|
u1 = us1[i]
|
||||||
|
u2 = us2[i]
|
||||||
|
self.rc_socket.send('({},{},{})\n'.format(dt,u1, u2))
|
||||||
|
self.t = time.time()
|
||||||
|
#time.sleep(0.2)
|
||||||
|
#
|
||||||
|
|
||||||
|
|
||||||
|
pass
|
||||||
|
|
||||||
def main(args):
|
def main(args):
|
||||||
rospy.init_node('controller_node', anonymous=True)
|
rospy.init_node('controller_node', anonymous=True)
|
||||||
|
@ -347,9 +505,8 @@ def main(args):
|
||||||
|
|
||||||
screenheight = 480
|
screenheight = 480
|
||||||
screenwidth = 640
|
screenwidth = 640
|
||||||
pygame.display.set_mode([screenwidth, screenheight])
|
screen = pygame.display.set_mode([screenwidth, screenheight])
|
||||||
|
|
||||||
#threading.Thread(target=rc.input_handling).start()
|
|
||||||
threading.Thread(target=rc.controller).start()
|
threading.Thread(target=rc.controller).start()
|
||||||
|
|
||||||
rc.ani()
|
rc.ani()
|
||||||
|
|
Loading…
Reference in New Issue
Block a user