From f3de1b173a5f69f37f49f837e5702c271afccc42 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Fri, 24 May 2019 09:20:49 -0500 Subject: [PATCH] example for optimal control of robot --- remote_control/casadi_opt.py | 224 +++++++++++++++++++++++++++++++++++ 1 file changed, 224 insertions(+) create mode 100644 remote_control/casadi_opt.py diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py new file mode 100644 index 0000000..2cddef9 --- /dev/null +++ b/remote_control/casadi_opt.py @@ -0,0 +1,224 @@ +from casadi import * + +# look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py + +T = 3.0 +N = 30 + +x = SX.sym('x') +y = SX.sym('y') +theta = SX.sym('theta') +state = vertcat(x, y, theta) +r = 0.03 +R = 0.05 +d = 0.02 +#r = SX.sym('r') +#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) + +# Fixed step Runge-Kutta 4 integrator +M = 4 # RK4 steps per interval +DT = T/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 = 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']) + +Fk = F(x0=[0.2,0.3, 0.0],p=[-1.1, 1.1]) +print(Fk['xf']) +print(Fk['qf']) + +# Start with an empty NLP +w=[] +w0 = [] +lbw = [] +ubw = [] +J = 0 +g=[] +lbg = [] +ubg = [] + +# Formulate the NLP +Xk = MX([1.1, 1.1, 0.0]) +for k in range(N): + # 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 + Fk = F(x0=Xk, p=U1k) + Xk = Fk['xf'] + J=J+Fk['qf'] + + # Add inequality constraint + #g += [Xk[1]] + #lbg += [-.0] + #ubg += [inf] + +# Create an NLP solver +prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} +solver = nlpsol('solver', 'ipopt', prob); + +# Solve the NLP +sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) +w_opt = sol['x'] + +# Plot the solution +u_opt = w_opt +x_opt = [[1.1, 1.1, -0.0]] +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)] +import matplotlib.pyplot as plt +plt.figure(1) +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!) +opti = Opti() # Optimization problem + +# ---- decision variables --------- +X = opti.variable(3,N+1) # state trajectory +Q = opti.variable(1,N+1) # state trajectory +posx = X[0,:] +posy = X[1,:] +angle = X[2,:] +U = opti.variable(2,N) # control trajectory (throttle) +#T = opti.variable() # final time + +# ---- objective --------- +#opti.minimize(T) # race in minimal time + +# ---- dynamic constraints -------- +#f = lambda x,u: vertcat(f1, f2, f3) # dx/dt = f(x,u) + +dt = T/N # length of a control interval +for k in range(N): # loop over control intervals + # 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 ----------- +#limit = lambda pos: 1-sin(2*pi*pos)/2 +#opti.subject_to(speed<=limit(pos)) # track speed limit +opti.subject_to(opti.bounded(-10,U,10)) # control is limited + +# ---- boundary conditions -------- +opti.subject_to(posx[0]==1.10) # start at position 0 ... +opti.subject_to(posy[0]==1.10) # ... from stand-still +opti.subject_to(angle[0]==0.0) # finish line at position 1 +#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 + +r = 0.25 +p = (0.5, 0.5) +for k in range(N): + opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2) + pass + + +# ---- initial values for solver --- +#opti.set_initial(speed, 1) +#opti.set_initial(T, 1) + +# ---- solve NLP ------ +opti.solver("ipopt") # set numerical backend +sol = opti.solve() # actual solve + +#x0 = sol.value(opti.x) +#lam_g0 = sol.value(opti.lam_g) +#opti.set_initial(opti.lam_g, lam_g0) +#opti.set_initial(opti.x, x0) +#opti.solve() + +from pylab import plot, step, figure, legend, show, spy + +plot(sol.value(posx),label="posx") +plot(sol.value(posy),label="posy") +plot(sol.value(angle),label="angle") + +plt.figure() +plot(sol.value(posx), sol.value(posy)) +ax = plt.gca() +circle = plt.Circle(p, r) +ax.add_artist(circle) +#plot(limit(sol.value(pos)),'r--',label="speed limit") +#step(range(N),sol.value(U),'k',label="throttle") +legend(loc="upper left") +plt.show() +pass +# linearization +# A = zeros(states.shape[0], states.shape[0]) +# for i in range(f.shape[0]): +# for j in range(states.shape[0]): +# A[i,j] = diff(f[i,0], states[j]) +# Alin = A.subs([(theta,0), (omegar,0), (omegal,0)]) +# print("A = {}".format(Alin)) +# B = zeros(f.shape[0], controls.shape[0]) +# for i in range(f.shape[0]): +# for j in range(controls.shape[0]): +# B[i,j] = diff(f[i,0], controls[j]) +# print("B = {}".format(B)) +# dfdtheta = diff(f, theta) +#print(dfdtheta.doit()) + +# takeaway: linearization is not helpful, because the linearized system is not stabilizable +# -> alternative: use nonlinear control method