started working on optimization based controller
This commit is contained in:
parent
0d14738671
commit
f100f21162
|
|
@ -2,223 +2,231 @@ from casadi import *
|
||||||
|
|
||||||
# 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
|
||||||
|
|
||||||
T = 3.0
|
class OpenLoopSolver:
|
||||||
N = 30
|
def __init__(self, N=60, T=6.0):
|
||||||
|
self.T = T
|
||||||
|
self.N = N
|
||||||
|
|
||||||
x = SX.sym('x')
|
def solve(self, x0):
|
||||||
y = SX.sym('y')
|
x = SX.sym('x')
|
||||||
theta = SX.sym('theta')
|
y = SX.sym('y')
|
||||||
state = vertcat(x, y, theta)
|
theta = SX.sym('theta')
|
||||||
r = 0.03
|
state = vertcat(x, y, theta)
|
||||||
R = 0.05
|
r = 0.03
|
||||||
d = 0.02
|
R = 0.05
|
||||||
#r = SX.sym('r')
|
d = 0.02
|
||||||
#R = SX.sym('R')
|
|
||||||
#d = SX.sym('d')
|
|
||||||
omegar = SX.sym('omegar')
|
|
||||||
omegal = SX.sym('omegal')
|
|
||||||
control = vertcat(omegar, omegal)
|
|
||||||
f1 = (r/2 * cos(theta) - r*d/(2*R) * sin(theta)) * omegar + (r/2 * cos(theta) + r*d/(2*R) * sin(theta)) * omegal
|
|
||||||
f2 = (r/2 * sin(theta) + r*d/(2*R) * cos(theta)) * omegar + (r/2 * sin(theta) - r*d/(2*R) * cos(theta)) * omegal
|
|
||||||
f3 = r/(2*R) * omegar - r/(2*R) * omegal
|
|
||||||
xdot = vertcat(f1, f2, f3)
|
|
||||||
f = Function('f', [x,y,theta, omegar, omegal], [f1, f2, f3])
|
|
||||||
print("f = {}".format(f))
|
|
||||||
|
|
||||||
L = x**2 + y**2 + 1e-2 * theta**2 + 1e-4 * (omegar**2 + omegal**2)
|
omegar = SX.sym('omegar')
|
||||||
|
omegal = SX.sym('omegal')
|
||||||
|
control = vertcat(omegar, omegal)
|
||||||
|
|
||||||
# Fixed step Runge-Kutta 4 integrator
|
# model equation
|
||||||
M = 4 # RK4 steps per interval
|
f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar + (r / 2 * cos(theta) + r * d / (2 * R) * sin(
|
||||||
DT = T/N/M
|
theta)) * omegal
|
||||||
print("DT = {}".format(DT))
|
f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar + (r / 2 * sin(theta) - r * d / (2 * R) * cos(
|
||||||
f = Function('f', [state, control], [xdot, L])
|
theta)) * omegal
|
||||||
X0 = MX.sym('X0', 3)
|
f3 = r / (2 * R) * omegar - r / (2 * R) * omegal
|
||||||
U = MX.sym('U', 2)
|
xdot = vertcat(f1, f2, f3)
|
||||||
X = X0
|
f = Function('f', [x, y, theta, omegar, omegal], [f1, f2, f3])
|
||||||
Q = 0
|
print("f = {}".format(f))
|
||||||
runge_kutta = True
|
|
||||||
if runge_kutta:
|
|
||||||
for j in range(M):
|
|
||||||
k1, k1_q = f(X, U)
|
|
||||||
k2, k2_q = f(X + DT/2 * k1, U)
|
|
||||||
k3, k3_q = f(X + DT/2 * k2, U)
|
|
||||||
k4, k4_q = f(X + DT * k3, U)
|
|
||||||
X=X+DT/6*(k1 +2*k2 +2*k3 +k4)
|
|
||||||
Q = Q + DT/6*(k1_q + 2*k2_q + 2*k3_q + k4_q)
|
|
||||||
else:
|
|
||||||
DT = T/N
|
|
||||||
k1, k1_q = f(X, U)
|
|
||||||
X = X + DT * k1
|
|
||||||
Q = Q + DT * k1_q
|
|
||||||
F = Function('F', [X0, U], [X, Q],['x0','p'],['xf','qf'])
|
|
||||||
|
|
||||||
#F_euler = Function('F_euler', [X0, U], [Xeuler, Qeuler], ['x0', 'p'], ['xf', 'qf'])
|
# cost functional
|
||||||
|
L = x ** 2 + y ** 2 + 1e-2 * theta ** 2 + 1e-4 * (omegar ** 2 + omegal ** 2)
|
||||||
|
|
||||||
Fk = F(x0=[0.2,0.3, 0.0],p=[-1.1, 1.1])
|
# Fixed step Runge-Kutta 4 integrator
|
||||||
print(Fk['xf'])
|
M = 4 # RK4 steps per interval
|
||||||
print(Fk['qf'])
|
DT = self.T / self.N / M
|
||||||
|
print("DT = {}".format(DT))
|
||||||
|
f = Function('f', [state, control], [xdot, L])
|
||||||
|
X0 = MX.sym('X0', 3)
|
||||||
|
U = MX.sym('U', 2)
|
||||||
|
X = X0
|
||||||
|
Q = 0
|
||||||
|
runge_kutta = True
|
||||||
|
if runge_kutta:
|
||||||
|
for j in range(M):
|
||||||
|
k1, k1_q = f(X, U)
|
||||||
|
k2, k2_q = f(X + DT / 2 * k1, U)
|
||||||
|
k3, k3_q = f(X + DT / 2 * k2, U)
|
||||||
|
k4, k4_q = f(X + DT * k3, U)
|
||||||
|
X = X + DT / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
|
||||||
|
Q = Q + DT / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
|
||||||
|
else:
|
||||||
|
DT = self.T / self.N
|
||||||
|
k1, k1_q = f(X, U)
|
||||||
|
X = X + DT * k1
|
||||||
|
Q = Q + DT * k1_q
|
||||||
|
F = Function('F', [X0, U], [X, Q], ['x0', 'p'], ['xf', 'qf'])
|
||||||
|
|
||||||
# Start with an empty NLP
|
# F_euler = Function('F_euler', [X0, U], [Xeuler, Qeuler], ['x0', 'p'], ['xf', 'qf'])
|
||||||
w=[]
|
|
||||||
w0 = []
|
|
||||||
lbw = []
|
|
||||||
ubw = []
|
|
||||||
J = 0
|
|
||||||
g=[]
|
|
||||||
lbg = []
|
|
||||||
ubg = []
|
|
||||||
|
|
||||||
# Formulate the NLP
|
Fk = F(x0=[0.2, 0.3, 0.0], p=[-1.1, 1.1])
|
||||||
Xk = MX([1.1, 1.1, 0.0])
|
print(Fk['xf'])
|
||||||
for k in range(N):
|
print(Fk['qf'])
|
||||||
# New NLP variable for the control
|
|
||||||
U1k = MX.sym('U1_' + str(k), 2)
|
|
||||||
#U2k = MX.sym('U2_' + str(k))
|
|
||||||
w += [U1k]
|
|
||||||
lbw += [-10, -10]
|
|
||||||
ubw += [10, 10]
|
|
||||||
w0 += [0, 0]
|
|
||||||
|
|
||||||
# Integrate till the end of the interval
|
# Start with an empty NLP
|
||||||
Fk = F(x0=Xk, p=U1k)
|
w = []
|
||||||
Xk = Fk['xf']
|
w0 = []
|
||||||
J=J+Fk['qf']
|
lbw = []
|
||||||
|
ubw = []
|
||||||
|
J = 0
|
||||||
|
g = []
|
||||||
|
lbg = []
|
||||||
|
ubg = []
|
||||||
|
|
||||||
# Add inequality constraint
|
# Formulate the NLP
|
||||||
#g += [Xk[1]]
|
Xk = MX(x0)
|
||||||
#lbg += [-.0]
|
for k in range(self.N):
|
||||||
#ubg += [inf]
|
# New NLP variable for the control
|
||||||
|
U1k = MX.sym('U1_' + str(k), 2)
|
||||||
|
# U2k = MX.sym('U2_' + str(k))
|
||||||
|
w += [U1k]
|
||||||
|
lbw += [-10, -10]
|
||||||
|
ubw += [10, 10]
|
||||||
|
w0 += [0, 0]
|
||||||
|
|
||||||
# Create an NLP solver
|
# Integrate till the end of the interval
|
||||||
prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
|
Fk = F(x0=Xk, p=U1k)
|
||||||
solver = nlpsol('solver', 'ipopt', prob);
|
Xk = Fk['xf']
|
||||||
|
J = J + Fk['qf']
|
||||||
|
|
||||||
# Solve the NLP
|
# Add inequality constraint
|
||||||
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
|
# g += [Xk[1]]
|
||||||
w_opt = sol['x']
|
# lbg += [-.0]
|
||||||
|
# ubg += [inf]
|
||||||
|
|
||||||
# Plot the solution
|
# Create an NLP solver
|
||||||
u_opt = w_opt
|
prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
|
||||||
x_opt = [[1.1, 1.1, -0.0]]
|
self.solver = nlpsol('solver', 'ipopt', prob)
|
||||||
for k in range(N):
|
|
||||||
Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2])
|
|
||||||
x_opt += [Fk['xf'].full()]
|
|
||||||
x1_opt = [r[0] for r in x_opt]
|
|
||||||
x2_opt = [r[1] for r in x_opt]
|
|
||||||
x3_opt = [r[2] for r in x_opt]
|
|
||||||
|
|
||||||
tgrid = [T/N*k for k in range(N+1)]
|
# Solve the NLP
|
||||||
import matplotlib.pyplot as plt
|
sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
|
||||||
plt.figure(1)
|
w_opt = sol['x']
|
||||||
plt.clf()
|
|
||||||
plt.plot(tgrid, x1_opt, '--')
|
|
||||||
plt.plot(tgrid, x2_opt, '-')
|
|
||||||
plt.plot(tgrid, x3_opt, '*')
|
|
||||||
#plt.step(tgrid, vertcat(DM.nan(1), u_opt), '-.')
|
|
||||||
plt.xlabel('t')
|
|
||||||
plt.legend(['x1','x2','x3','u'])
|
|
||||||
plt.grid()
|
|
||||||
#plt.show()
|
|
||||||
|
|
||||||
# alternative solution using multiple shooting (way faster!)
|
# Plot the solution
|
||||||
opti = Opti() # Optimization problem
|
u_opt = w_opt
|
||||||
|
x_opt = [x0]
|
||||||
|
for k in range(self.N):
|
||||||
|
Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2])
|
||||||
|
x_opt += [Fk['xf'].full()]
|
||||||
|
x1_opt = [r[0] for r in x_opt]
|
||||||
|
x2_opt = [r[1] for r in x_opt]
|
||||||
|
x3_opt = [r[2] for r in x_opt]
|
||||||
|
|
||||||
# ---- decision variables ---------
|
tgrid = [self.T/self.N*k for k in range(self.N+1)]
|
||||||
X = opti.variable(3,N+1) # state trajectory
|
import matplotlib.pyplot as plt
|
||||||
Q = opti.variable(1,N+1) # state trajectory
|
plt.figure(2)
|
||||||
posx = X[0,:]
|
plt.clf()
|
||||||
posy = X[1,:]
|
plt.plot(tgrid, x1_opt, '--')
|
||||||
angle = X[2,:]
|
plt.plot(tgrid, x2_opt, '-')
|
||||||
U = opti.variable(2,N) # control trajectory (throttle)
|
plt.plot(tgrid, x3_opt, '*')
|
||||||
#T = opti.variable() # final time
|
#plt.step(tgrid, vertcat(DM.nan(1), u_opt), '-.')
|
||||||
|
plt.xlabel('t')
|
||||||
|
plt.legend(['x1','x2','x3','u'])
|
||||||
|
plt.grid()
|
||||||
|
#plt.show()
|
||||||
|
#return
|
||||||
|
|
||||||
# ---- objective ---------
|
# alternative solution using multiple shooting (way faster!)
|
||||||
#opti.minimize(T) # race in minimal time
|
opti = Opti() # Optimization problem
|
||||||
|
|
||||||
# ---- dynamic constraints --------
|
# ---- decision variables ---------
|
||||||
#f = lambda x,u: vertcat(f1, f2, f3) # dx/dt = f(x,u)
|
X = opti.variable(3,self.N+1) # state trajectory
|
||||||
|
Q = opti.variable(1,self.N+1) # state trajectory
|
||||||
|
posx = X[0,:]
|
||||||
|
posy = X[1,:]
|
||||||
|
angle = X[2,:]
|
||||||
|
U = opti.variable(2,self.N) # control trajectory (throttle)
|
||||||
|
#T = opti.variable() # final time
|
||||||
|
|
||||||
dt = T/N # length of a control interval
|
# ---- objective ---------
|
||||||
for k in range(N): # loop over control intervals
|
#opti.minimize(T) # race in minimal time
|
||||||
# Runge-Kutta 4 integration
|
|
||||||
k1, k1_q = f(X[:,k], U[:,k])
|
|
||||||
k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k])
|
|
||||||
k3, k3_q = f(X[:,k]+dt/2*k2, U[:,k])
|
|
||||||
k4, k4_q = f(X[:,k]+dt*k3, U[:,k])
|
|
||||||
x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4)
|
|
||||||
q_next = Q[:,k] + dt/6*(k1_q + 2 * k2_q + 2 * k3_q + k4_q)
|
|
||||||
opti.subject_to(X[:,k+1]==x_next) # close the gaps
|
|
||||||
opti.subject_to(Q[:,k+1]==q_next) # close the gaps
|
|
||||||
opti.minimize(Q[:,N])
|
|
||||||
|
|
||||||
# ---- path constraints -----------
|
# ---- dynamic constraints --------
|
||||||
#limit = lambda pos: 1-sin(2*pi*pos)/2
|
#f = lambda x,u: vertcat(f1, f2, f3) # dx/dt = f(x,u)
|
||||||
#opti.subject_to(speed<=limit(pos)) # track speed limit
|
|
||||||
opti.subject_to(opti.bounded(-10,U,10)) # control is limited
|
|
||||||
|
|
||||||
# ---- boundary conditions --------
|
dt = self.T/self.N # length of a control interval
|
||||||
opti.subject_to(posx[0]==1.10) # start at position 0 ...
|
for k in range(self.N): # loop over control intervals
|
||||||
opti.subject_to(posy[0]==1.10) # ... from stand-still
|
# Runge-Kutta 4 integration
|
||||||
opti.subject_to(angle[0]==0.0) # finish line at position 1
|
k1, k1_q = f(X[:,k], U[:,k])
|
||||||
#opti.subject_to(speed[-1]==0) # .. with speed 0
|
k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k])
|
||||||
opti.subject_to(Q[:,0]==0.0)
|
k3, k3_q = f(X[:,k]+dt/2*k2, U[:,k])
|
||||||
|
k4, k4_q = f(X[:,k]+dt*k3, U[:,k])
|
||||||
|
x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4)
|
||||||
|
q_next = Q[:,k] + dt/6*(k1_q + 2 * k2_q + 2 * k3_q + k4_q)
|
||||||
|
opti.subject_to(X[:,k+1]==x_next) # close the gaps
|
||||||
|
opti.subject_to(Q[:,k+1]==q_next) # close the gaps
|
||||||
|
opti.minimize(Q[:,self.N])
|
||||||
|
|
||||||
# ---- misc. constraints ----------
|
# ---- path constraints -----------
|
||||||
#opti.subject_to(X[1,:]>=0) # Time must be positive
|
#limit = lambda pos: 1-sin(2*pi*pos)/2
|
||||||
#opti.subject_to(X[2,:]<=4) # Time must be positive
|
#opti.subject_to(speed<=limit(pos)) # track speed limit
|
||||||
#opti.subject_to(X[2,:]>=-2) # Time must be positive
|
opti.subject_to(opti.bounded(-10,U,10)) # control is limited
|
||||||
|
|
||||||
r = 0.25
|
# ---- boundary conditions --------
|
||||||
p = (0.5, 0.5)
|
opti.subject_to(posx[0]==x0[0]) # start at position 0 ...
|
||||||
for k in range(N):
|
opti.subject_to(posy[0]==x0[1]) # ... from stand-still
|
||||||
opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2)
|
opti.subject_to(angle[0]==x0[2]) # finish line at position 1
|
||||||
pass
|
#opti.subject_to(speed[-1]==0) # .. with speed 0
|
||||||
|
opti.subject_to(Q[:,0]==0.0)
|
||||||
|
|
||||||
|
# ---- misc. constraints ----------
|
||||||
|
#opti.subject_to(X[1,:]>=0) # Time must be positive
|
||||||
|
#opti.subject_to(X[2,:]<=4) # Time must be positive
|
||||||
|
#opti.subject_to(X[2,:]>=-2) # Time must be positive
|
||||||
|
|
||||||
|
# avoid obstacle
|
||||||
|
#r = 0.25
|
||||||
|
#p = (0.5, 0.5)
|
||||||
|
#for k in range(self.N):
|
||||||
|
# opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2)
|
||||||
|
# pass
|
||||||
|
|
||||||
|
|
||||||
# ---- initial values for solver ---
|
# ---- initial values for solver ---
|
||||||
#opti.set_initial(speed, 1)
|
#opti.set_initial(speed, 1)
|
||||||
#opti.set_initial(T, 1)
|
#opti.set_initial(T, 1)
|
||||||
|
|
||||||
# ---- solve NLP ------
|
# ---- solve NLP ------
|
||||||
opti.solver("ipopt") # set numerical backend
|
opti.solver("ipopt") # set numerical backend
|
||||||
sol = opti.solve() # actual solve
|
sol = opti.solve() # actual solve
|
||||||
|
|
||||||
#x0 = sol.value(opti.x)
|
#x0 = sol.value(opti.x)
|
||||||
#lam_g0 = sol.value(opti.lam_g)
|
#lam_g0 = sol.value(opti.lam_g)
|
||||||
#opti.set_initial(opti.lam_g, lam_g0)
|
#opti.set_initial(opti.lam_g, lam_g0)
|
||||||
#opti.set_initial(opti.x, x0)
|
#opti.set_initial(opti.x, x0)
|
||||||
#opti.solve()
|
#opti.solve()
|
||||||
|
|
||||||
from pylab import plot, step, figure, legend, show, spy
|
from pylab import plot, step, figure, legend, show, spy
|
||||||
|
|
||||||
plot(sol.value(posx),label="posx")
|
plot(sol.value(posx),label="posx")
|
||||||
plot(sol.value(posy),label="posy")
|
plot(sol.value(posy),label="posy")
|
||||||
plot(sol.value(angle),label="angle")
|
plot(sol.value(angle),label="angle")
|
||||||
|
|
||||||
plt.figure()
|
plt.figure(3)
|
||||||
plot(sol.value(posx), sol.value(posy))
|
plot(sol.value(posx), sol.value(posy))
|
||||||
ax = plt.gca()
|
ax = plt.gca()
|
||||||
circle = plt.Circle(p, r)
|
circle = plt.Circle(p, r)
|
||||||
ax.add_artist(circle)
|
ax.add_artist(circle)
|
||||||
#plot(limit(sol.value(pos)),'r--',label="speed limit")
|
#plot(limit(sol.value(pos)),'r--',label="speed limit")
|
||||||
#step(range(N),sol.value(U),'k',label="throttle")
|
#step(range(N),sol.value(U),'k',label="throttle")
|
||||||
legend(loc="upper left")
|
legend(loc="upper left")
|
||||||
plt.show()
|
plt.show()
|
||||||
pass
|
pass
|
||||||
# linearization
|
# linearization
|
||||||
# A = zeros(states.shape[0], states.shape[0])
|
# A = zeros(states.shape[0], states.shape[0])
|
||||||
# for i in range(f.shape[0]):
|
# for i in range(f.shape[0]):
|
||||||
# for j in range(states.shape[0]):
|
# for j in range(states.shape[0]):
|
||||||
# A[i,j] = diff(f[i,0], states[j])
|
# A[i,j] = diff(f[i,0], states[j])
|
||||||
# Alin = A.subs([(theta,0), (omegar,0), (omegal,0)])
|
# Alin = A.subs([(theta,0), (omegar,0), (omegal,0)])
|
||||||
# print("A = {}".format(Alin))
|
# print("A = {}".format(Alin))
|
||||||
# B = zeros(f.shape[0], controls.shape[0])
|
# B = zeros(f.shape[0], controls.shape[0])
|
||||||
# for i in range(f.shape[0]):
|
# for i in range(f.shape[0]):
|
||||||
# for j in range(controls.shape[0]):
|
# for j in range(controls.shape[0]):
|
||||||
# B[i,j] = diff(f[i,0], controls[j])
|
# B[i,j] = diff(f[i,0], controls[j])
|
||||||
# print("B = {}".format(B))
|
# print("B = {}".format(B))
|
||||||
# dfdtheta = diff(f, theta)
|
# dfdtheta = diff(f, theta)
|
||||||
#print(dfdtheta.doit())
|
#print(dfdtheta.doit())
|
||||||
|
|
||||||
# takeaway: linearization is not helpful, because the linearized system is not stabilizable
|
# takeaway: linearization is not helpful, because the linearized system is not stabilizable
|
||||||
# -> alternative: use nonlinear control method
|
# -> alternative: use nonlinear control method
|
||||||
|
|
|
||||||
|
|
@ -20,6 +20,8 @@ import matplotlib.animation as anim
|
||||||
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
|
from casadi_opt import OpenLoopSolver
|
||||||
|
|
||||||
from marker_pos_angle.msg import id_pos_angle
|
from marker_pos_angle.msg import id_pos_angle
|
||||||
|
|
||||||
class Robot:
|
class Robot:
|
||||||
|
|
@ -59,7 +61,7 @@ def f_ode(t, x, u):
|
||||||
class RemoteController:
|
class RemoteController:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
|
||||||
self.robots = [Robot(3)]
|
self.robots = [Robot(5)]
|
||||||
|
|
||||||
self.robot_ids = {}
|
self.robot_ids = {}
|
||||||
for r in self.robots:
|
for r in self.robots:
|
||||||
|
|
@ -69,7 +71,7 @@ class RemoteController:
|
||||||
self.rc_socket = socket.socket()
|
self.rc_socket = socket.socket()
|
||||||
try:
|
try:
|
||||||
pass
|
pass
|
||||||
self.rc_socket.connect(('192.168.1.101', 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")
|
||||||
|
|
||||||
|
|
@ -117,6 +119,8 @@ class RemoteController:
|
||||||
plt.xlabel('x-position')
|
plt.xlabel('x-position')
|
||||||
plt.ylabel('y-position')
|
plt.ylabel('y-position')
|
||||||
|
|
||||||
|
self.ols = OpenLoopSolver()
|
||||||
|
|
||||||
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()
|
||||||
|
|
@ -208,7 +212,8 @@ class RemoteController:
|
||||||
|
|
||||||
keyboard_control = False
|
keyboard_control = False
|
||||||
keyboard_control_speed_test = False
|
keyboard_control_speed_test = False
|
||||||
pid = True
|
pid = False
|
||||||
|
open_loop_solve = True
|
||||||
|
|
||||||
if keyboard_control: # keyboard controller
|
if keyboard_control: # keyboard controller
|
||||||
events = pygame.event.get()
|
events = pygame.event.get()
|
||||||
|
|
@ -330,6 +335,15 @@ class RemoteController:
|
||||||
|
|
||||||
time.sleep(dt)
|
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.ols.solve(self.xms[-1])
|
||||||
|
|
||||||
def main(args):
|
def main(args):
|
||||||
rospy.init_node('controller_node', anonymous=True)
|
rospy.init_node('controller_node', anonymous=True)
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user