1# 2# This file is part of CasADi. 3# 4# CasADi -- A symbolic framework for dynamic optimization. 5# Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, 6# K.U. Leuven. All rights reserved. 7# Copyright (C) 2011-2014 Greg Horn 8# 9# CasADi is free software; you can redistribute it and/or 10# modify it under the terms of the GNU Lesser General Public 11# License as published by the Free Software Foundation; either 12# version 3 of the License, or (at your option) any later version. 13# 14# CasADi is distributed in the hope that it will be useful, 15# but WITHOUT ANY WARRANTY; without even the implied warranty of 16# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17# Lesser General Public License for more details. 18# 19# You should have received a copy of the GNU Lesser General Public 20# License along with CasADi; if not, write to the Free Software 21# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 22# 23# 24from pylab import * 25from numpy import * 26from casadi import * 27from casadi.tools import * 28import scipy.linalg 29import numpy as np 30 31""" 32This example how an Linear Quadratic Regulator (LQR) can be designed 33and simulated for a linear-time-invariant (LTI) system 34""" 35 36# Problem statement 37# ----------------------------------- 38 39# System dimensions 40ns = 3 # number of states 41nu = 2 # number of controls 42ny = 2 # number of outputs 43N = 21 # number of control intervals 44te = 10 # end time [s] 45 46# The system equations: x' = A.x + B.u 47A = DM([[0.4,0.1,-2],[0,-0.3,4],[1,0,0]]) 48B = DM([[1,1],[0,1],[1,0]]) 49 50# Inspect the open-loop system 51[D,V] = linalg.eig(A) 52print('Open-loop eigenvalues: ', D) 53 54tn = linspace(0,te,N) 55 56# Check if the system is controllable 57# ----------------------------------- 58R = [] 59 60p = B 61for i in range(ns): 62 R.append(p) 63 p = mtimes([A,p]) 64 65R = horzcat(*R) 66 67# R must be of full rank 68_, s, _ = linalg.svd(R) 69eps = 1e-9 70rank = len([x for x in s if abs(x) > eps]) 71assert(rank==ns) 72 73# Simulation of the open-loop system 74# ----------------------------------- 75 76y = SX.sym('y',ns) 77u = SX.sym('u',nu) 78 79x0 = DM([1,0,0]) 80# no control 81u_ = repmat(DM([[ -1, 1 ],[1,-1]]),1,int((N-1)/2)) 82 83print(u_) 84 85p = SX.sym('p') 86 87# tn = np.linspace(0,te,N) 88# cdae = SX.fun('cdae', controldaeIn(x=y,u=u),[mtimes(A,y)+mtimes(B,u)]) 89 90# opts = {} 91# opts['integrator'] = 'cvodes' 92# opts['integrator_options'] = {'fsens_err_con': True,'reltol':1e-12} 93# opts['nf'] = 20 94 95# sim = ControlSimulator('sim', cdae, tn, opts) 96# sim.setInput(x0,'x0') 97# sim.setInput(u_,'u') 98# sim.evaluate() 99 100# tf = sim.minorT() 101 102# figure(1) 103# plot(tf,sim.getOutput().T) 104# legend(('s1', 's2','s3')) 105# title('reference simulation, open-loop, zero controls') 106# out = sim.getOutput() 107 108# # Simulation of the open-loop system 109# # sensitivity for initial conditions 110# # ----------------------------------- 111 112# x0_pert = DM([0,0,1])*1e-4 113# sim.setInput(x0+x0_pert,'x0') 114# sim.setInput(u_,'u') 115# sim.evaluate() 116 117# tf = list(sim.minorT()) 118 119# figure(2) 120# title('Deviation from reference simulation, with perturbed initial condition') 121# plot(tf,sim.getOutput().T-out.T,linewidth=3) 122 123# # Not supported in current revision, cf. #929 124# # jacsim = sim.jacobian_old(CONTROLSIMULATOR_X0,0) 125# # jacsim.setInput(x0,'x0') 126# # jacsim.setInput(u_,'u') 127 128# # jacsim.evaluate() 129 130# # dev_est = [] 131# # for i in range(len(tf)): 132# # dev_est.append(mtimes(jacsim.getOutput()[i*ns:(i+1)*ns,:],x0_pert)) 133 134# # dev_est = horzcat(dev_est).T 135# # plot(tf,dev_est,'+k') 136# # legend(('s1 dev', 's2 dev','s3 dev','s1 dev (est.)', 's2 dev (est.)','s3 dev (est.)'),loc='upper left') 137 138 139# # M = jacsim.getOutput()[-ns:,:] 140# # # In the case of zero input, we could also use the matrix exponential to obtain sensitivity 141# # Mref = scipy.linalg.expm(A*te) 142 143# # e = fabs(M - Mref) 144# # e = max(e)/max(fabs(M)) 145# # print e 146# # assert(e<1e-6) 147 148 149# # Simulation of the open-loop system 150# # sensitivity for controls 151# # ----------------------------------- 152# # What if we perturb the input? 153 154# u_perturb = DM(u_) 155# u_perturb[0,N/5] = 1e-4 156# sim.setInput(x0,'x0') 157# sim.setInput(u_+u_perturb,'u') 158# sim.evaluate() 159 160# figure(3) 161# title('Deviation from reference simulation, with perturbed controls') 162# plot(tf,sim.getOutput().T-out.T,linewidth=3) 163 164 165# # Not supported in current revision, cf. #929 166# # jacsim = sim.jacobian_old(CONTROLSIMULATOR_U,0) 167# # jacsim.setInput(x0,'x0') 168# # jacsim.setInput(u_,'u') 169 170# # jacsim.evaluate() 171 172# # dev_est = [] 173# # for i in range(len(tf)): 174# # dev_est.append(mtimes(jacsim.getOutput()[i*ns:(i+1)*ns,:],vec(u_perturb))) 175 176# # dev_est = horzcat(dev_est).T 177# # plot(tf,dev_est,'+k') 178# # legend(('s1 dev', 's2 dev','s3 dev','s1 dev (est.)', 's2 dev (est.)','s3 dev (est.)'),loc='upper left') 179 180 181# # Find feedforward controls 182# # ----------------------------------- 183# # 184# # Our goal is to reach a particular end-state xref_e 185# # We can find the necessary controls explicitly in continuous time 186# # with the controllability Gramian 187# # 188# # http://www-control.eng.cam.ac.uk/jmm/3f2/handout4.pdf 189# # http://www.ece.rutgers.edu/~gajic/psfiles/chap5.pdf 190 191# x0 = vertcat([1,0,0]) 192# xref_e = vertcat([1,0,0]) 193 194# states = struct_symSX([ 195# entry('eAt',shape=(ns,ns)), 196# entry('Wt',shape=(ns,ns)) 197# ]) 198 199# eAt = states['eAt'] 200# Wt = states['Wt'] 201 202# # We will find the control that allow to reach xref_e at t1 203# t1 = te 204 205# # Initial conditions 206# e = densify(DM.eye(ns)) 207# states_ = states(0) 208# states_['eAt'] = e 209# states_['Wt'] = 0 210 211# rhs = struct_SX(states) 212# rhs['eAt'] = mtimes(A,eAt) 213# rhs['Wt'] = mtimes([eAt,B,B.T,eAt.T]) 214 215# dae = SX.fun('dae', daeIn(x=states),daeOut(ode=rhs)) 216 217# integrator = Integrator('integrator', 'cvodes', dae, {'tf':t1, 'reltol':1e-12}) 218# integrator.setInput(states_,'x0') 219# integrator.evaluate() 220 221# out = states(integrator.getOutput()) 222 223# Wt_ = out['Wt'] 224# eAt_ = out['eAt'] 225 226# # Check that eAt is indeed the matrix exponential 227# e = max(fabs(eAt_-scipy.linalg.expm(numpy.array(A*te))))/max(fabs(eAt_)) 228# assert(e<1e-7) 229 230# # Simulate with feedforward controls 231# # ----------------------------------- 232 233# states = struct_symSX([ 234# entry('y',shape=ns), # The regular states of the LTI system 235# entry('eAt',shape=(ns,ns)) # The matrix exponential exp(A*(t1-t)) 236# ]) 237 238# eAt = states['eAt'] 239# y = states['y'] 240 241# # Initial conditions 242# states_ = states(0) 243# states_['y'] = x0 244# states_['eAt'] = eAt_ 245 246# u = mtimes([B.T,eAt.T,inv(Wt_),xref_e-mtimes(eAt_,x0)]) 247 248# rhs = struct_SX(states) 249# rhs['y'] = mtimes(A,y)+mtimes(B,u) 250# rhs['eAt'] = -mtimes(A,eAt) 251 252# cdae = SX.fun('cdae', controldaeIn(x=states),[rhs]) 253 254# # Output function 255# out = SX.fun('out', controldaeIn(x=states),[states,u]) 256 257# opts = {} 258# opts['integrator'] = 'cvodes' 259# opts['integrator_options'] = {'fsens_err_con': True,'reltol':1e-12} 260# opts['nf'] = 20 261 262# sim = ControlSimulator('sim', cdae, out, tn, opts) 263# sim.setInput(states_,'x0') 264# sim.evaluate() 265 266# e = sim.getOutput()[states.i['y'],-1] - xref_e 267# assert(max(fabs(e))/max(fabs(xref_e))<1e-6) 268 269# tf = sim.minorT() 270# # This will be our reference trajectory for tracking 271 272# figure(4) 273# subplot(211) 274# title('Feedforward control, states') 275# plot(tf,sim.getOutput(0)[list(states.i['y']),:].T) 276# for i,c in enumerate(['b','g','r']): 277# plot(t1,xref_e[i],c+'o') 278# subplot(212) 279# title('Control action') 280# plot(tf,sim.getOutput(1).T) 281 282# # Design an infinite horizon LQR 283# # ----------------------------------- 284 285# # Weights for the infinite horizon LQR control 286# Q = DM.eye(ns) 287# R = DM.eye(nu) 288 289# # Continuous Riccati equation 290# P = SX.sym('P',ns,ns) 291 292# ric = (Q + mtimes(A.T,P) + mtimes(P,A) - mtimes([P,B,inv(R),B.T,P])) 293 294# dae = SX.fun('dae', daeIn(x=vec(P)),daeOut(ode=vec(ric))) 295 296# # We solve the ricatti equation by simulating backwards in time until steady state is reached. 297# opts = {'reltol':1e-16, 'stop_at_end':False, 'tf':1} 298# integrator = Integrator('integrator', 'cvodes', dae, opts) 299 300# # Start from P = identity matrix 301# u = densify(DM.eye(ns)) 302# xe = vec(u) 303 304# # Keep integrating until steady state is reached 305# for i in range(1,40): 306# x0 = xe 307# xe = integrator({'x0':x0})['xf'] 308# e = max(fabs(xe-x0)) 309# print 'it. %02d - deviation from steady state: %.2e' % (i, e) 310# if e < 1e-11: 311# break 312 313# # Obtain the solution of the ricatti equation 314# P_ = xe.reshape((ns,ns)) 315# print 'P=', P_ 316 317# [D,V] = linalg.eig(P_) 318# assert (min(real(D))>0) 319# print '(positive definite)' 320 321 322# # Check that it does indeed satisfy the ricatti equation 323# dae.setInput(integrator.getOutput(),'x') 324# dae.evaluate() 325# print max(fabs(dae.getOutput())) 326# assert(max(fabs(dae.getOutput()))<1e-8) 327 328# # From P, obtain a feedback matrix K 329# K = mtimes([inv(R),B.T,P_]) 330 331# print 'feedback matrix= ', K 332 333# # Inspect closed-loop eigenvalues 334# [D,V] = linalg.eig(A-mtimes(B,K)) 335# print 'Open-loop eigenvalues: ', D 336 337# # Check what happens if we integrate the Riccati equation forward in time 338# dae = SX.fun('dae', daeIn(x = vec(P)),daeOut(ode=vec(-ric))) 339 340# integrator = Integrator('integrator', 'cvodes', dae, {'reltol':1e-16, 'stop_at_end':False, 'tf':0.4}) 341# x0_pert = vec(P_) 342# x0_pert[0] += 1e-9 # Put a tiny perturbation 343# xe = x0_pert 344 345# for i in range(1,10): 346# x0 = xe 347# xe = integrator({'x0':x0})['xf'] 348# e = max(fabs(xe-x0)) 349# print 'Forward riccati simulation %d; error: %.2e' % (i, e) 350 351# # We notice divergence. Why? 352# stabric = SX.fun('stabric', [P],[jacobian(-ric,P)]) 353# stabric.setInput(P_) 354# stabric.evaluate() 355 356# S = stabric.getOutput() 357 358# [D,V] = linalg.eig(S) 359 360# print 'Forward riccati eigenvalues = ', D 361 362 363# # Simulation of the closed-loop system: 364# # continuous control action, various continuous references 365# # --------------------------------------------------------- 366# # 367 368# x0 = DM([1,0,0]) 369 370# y = SX.sym('y',ns) 371 372# C = DM([[1,0,0],[0,1,0]]) 373# D = DM([[0,0],[0,0]]) 374 375# temp = inv(blockcat([[A,B],[C,D]])) 376 377# F = temp[:ns,-ny:] 378# Nm = temp[ns:,-ny:] 379 380# t = SX.sym('t') 381 382# figure(6) 383 384# for k,yref in enumerate([ vertcat([-1,sqrt(t)]) , vertcat([-1,-0.5]), vertcat([-1,sin(t)])]): 385# u = -mtimes(K,y) + mtimes(mtimes(K,F)+Nm,yref) 386# rhs = mtimes(A,y)+mtimes(B,u) 387# cdae = SX.fun('cdae', controldaeIn(t=t, x=y),[rhs]) 388 389# # Output function 390# out = SX.fun('out', controldaeIn(t=t, x=y),[y,mtimes(C,y),u,yref]) 391 392# opts = {} 393# opts['integrator'] = 'cvodes' 394# opts['integrator_options'] = {'fsens_err_con': True,'reltol':1e-12} 395# opts['nf'] = 200 396# sim = ControlSimulator('sim', cdae, out, tn, opts) 397# sim.setInput(x0,'x0') 398# #sim.setInput(yref_,'u') 399# sim.evaluate() 400 401# tf = sim.minorT() 402 403# subplot(3,3,1+k*3) 404# plot(tf,sim.getOutput(0).T) 405# subplot(3,3,2+k*3) 406# title('ref ' + str(yref)) 407# for i,c in enumerate(['b','g']): 408# plot(tf,sim.getOutput(1)[i,:].T,c,linewidth=2) 409# plot(tf,sim.getOutput(3)[i,:].T,c+'-') 410# subplot(3,3,3+k*3) 411# plot(tf,sim.getOutput(2).T) 412 413 414# # Simulation of the closed-loop system: 415# # continuous control action, continuous feedforward reference 416# # ----------------------------------------------------------- 417# # To obtain a continous tracking reference, 418# # we augment statespace to construct it on the fly 419 420# x0 = vertcat([1,0,0]) 421 422# # Now simulate with open-loop controls 423# states = struct_symSX([ 424# entry('y',shape=ns), # The regular states of the LTI system 425# entry('yref',shape=ns), # States that constitute a tracking reference for the LTI system 426# entry('eAt',shape=(ns,ns)) # The matrix exponential exp(A*(t1-t)) 427# ]) 428 429# y = states['y'] 430# eAt = states['eAt'] 431 432# # Initial conditions 433# states_ = states(0) 434# states_['y'] = 2*x0 435# states_['yref'] = x0 436# states_['eAt'] = eAt_ 437 438 439# param = struct_symSX([entry('K',shape=(nu,ns))]) 440 441# param_ = param(0) 442 443# uref = mtimes([B.T,eAt.T,inv(Wt_),xref_e-mtimes(eAt_,x0)]) 444# u = uref - mtimes(param['K'],y-states['yref']) 445 446# rhs = struct_SX(states) 447# rhs['y'] = mtimes(A,y)+mtimes(B,u) 448# rhs['yref'] = mtimes(A,states['yref'])+mtimes(B,uref) 449# rhs['eAt'] = -mtimes(A,eAt) 450 451# cdae = SX.fun('cdae', controldaeIn(x=states, p=param),[rhs]) 452 453# # Output function 454# out = SX.fun('out', controldaeIn(x=states, p=param),[states,u,uref,states['yref']]) 455 456# opts = {} 457# opts['integrator'] = 'cvodes' 458# opts['integrator_options'] = {'fsens_err_con': True,'reltol':1e-8} 459# opts['nf'] = 20 460# sim = ControlSimulator('sim', cdae, out, tn, opts) 461 462# # Not supported in current revision, cf. #929 463# # jacsim = sim.jacobian_old(CONTROLSIMULATOR_X0,0) 464 465# figure(7) 466 467# for k,(caption,K_) in enumerate([('K: zero',DM.zeros((nu,ns))),('K: LQR',K)]): 468# param_['K'] = K_ 469 470# sim.setInput(states_,'x0') 471# sim.setInput(param_,'p') 472# sim.evaluate() 473# sim.getOutput() 474 475# tf = sim.minorT() 476 477# subplot(2,2,2*k+1) 478# title('states (%s)' % caption) 479# for i,c in enumerate(['b','g','r']): 480# plot(tf,sim.getOutput()[states.i['yref',i],:].T,c+'--') 481# plot(tf,sim.getOutput()[states.i['y',i],:].T,c,linewidth=2) 482# subplot(2,2,2*k+2) 483# for i,c in enumerate(['b','g']): 484# plot(tf,sim.getOutput(1)[i,:].T,c,linewidth=2) 485# plot(tf,sim.getOutput(2)[i,:].T,c+'--') 486# title('controls (%s)' % caption) 487 488# # Not supported in current revision, cf. #929 489# # # Calculate monodromy matrix 490# # jacsim.setInput(states_,'x0') 491# # jacsim.setInput(param_,'p') 492# # jacsim.evaluate() 493# # M = jacsim.getOutput()[-states.size:,:][list(states.i['y']),list(states.i['y'])] 494 495# # # Inspect the eigenvalues of M 496# # [D,V] = linalg.eig(M) 497# # print 'Spectral radius of monodromy (%s): ' % caption 498 499# # print max(abs(D)) 500 501# print 'Spectral radius of exp((A-BK)*te): ' 502 503# [D,V] = linalg.eig(scipy.linalg.expm(numpy.array((A-mtimes(B,K))*te))) 504# print max(abs(D)) 505 506# # Simulation of the controller: 507# # discrete reference, continuous control action 508# # ----------------------------------------------------------- 509 510# # Get discrete reference from previous simulation 511# mi = sim.getMajorIndex() 512# controls_ = sim.getOutput(2)[:,mi[:-1]] 513# yref_ = sim.getOutput(3)[:,mi[:-1]] 514 515# u_ = vertcat([controls_,yref_]) 516 517# x0 = DM([1,0,0]) 518 519# controls = struct_symSX([ 520# entry('uref',shape=nu), 521# entry('yref',shape=ns) 522# ]) 523 524# yref = SX.sym('yref',ns) 525# y = SX.sym('y',ns) 526# dy = SX.sym('dy',ns) 527# u = controls['uref']-mtimes(param['K'],y-controls['yref']) 528# rhs = mtimes(A,y)+mtimes(B,u) 529 530# cdae = SX.fun('cdae', controldaeIn(x=y, u=controls, p=param),[rhs]) 531 532# # Output function 533# out = SX.fun('out', controldaeIn(x=y, u=controls, p=param),[y,u,controls['uref'],controls['yref']]) 534 535# opts = {} 536# opts['integrator'] = 'cvodes' 537# opts['integrator_options'] = {'fsens_err_con': True,'reltol':1e-8} 538# opts['nf'] = 20 539# sim = ControlSimulator('sim', cdae, out, tn, opts) 540# sim.setInput(2*x0,'x0') 541# sim.setInput(param_,'p') 542# sim.setInput(u_,'u') 543# sim.evaluate() 544 545# tf = sim.minorT() 546 547# figure(8) 548# subplot(2,1,1) 549# title('states (%s)' % caption) 550# for i,c in enumerate(['b','g','r']): 551# plot(tf,sim.getOutput(3)[i,:].T,c+'--') 552# plot(tf,sim.getOutput()[i,:].T,c,linewidth=2) 553# subplot(2,1,2) 554# for i,c in enumerate(['b','g']): 555# plot(tf,sim.getOutput(1)[i,:].T,c,linewidth=2) 556# plot(tf,sim.getOutput(2)[i,:].T,c+'--') 557# title('controls (%s)' % caption) 558 559# # Not supported in current revision, cf. #929 560# # jacsim = sim.jacobian_old(CONTROLSIMULATOR_X0,0) 561 562# # # Calculate monodromy matrix 563# # jacsim.setInput(x0,'x0') 564# # jacsim.setInput(param_,'p') 565# # jacsim.setInput(u_,'u') 566# # jacsim.evaluate() 567# # M = jacsim.getOutput()[-ns:,:] 568 569# # # Inspect the eigenvalues of M 570# # [D,V] = linalg.eig(M) 571# # print 'Spectral radius of monodromy, discrete reference, continous control' 572 573# # print max(abs(D)) 574 575# # Simulation of the controller: 576# # discrete reference, discrete control action 577# # ----------------------------------------------------------- 578 579# y0 = SX.sym('y0',ns) 580 581# u = controls['uref']-mtimes(param['K'],y0-controls['yref']) 582# rhs = mtimes(A,y)+mtimes(B,u) 583 584# cdae = SX.fun('cdae', controldaeIn(x=y, x_major=y0, u=controls, p=param),[rhs]) 585 586# # Output function 587# out = SX.fun('out', controldaeIn(x=y, x_major=y0, u=controls, p=param),[y,u,controls['uref'],controls['yref']]) 588 589# opts = {} 590# opts['integrator'] = 'cvodes' 591# opts['integrator_options'] = {'fsens_err_con': True,'reltol':1e-8} 592# opts['nf'] = 20 593 594# sim = ControlSimulator('sim', cdae, out, tn, opts) 595# sim.setInput(2*x0,'x0') 596# sim.setInput(param_,'p') 597# sim.setInput(u_,'u') 598# sim.evaluate() 599 600# tf = sim.minorT() 601 602# figure(9) 603# subplot(2,1,1) 604# title('states (%s)' % caption) 605# for i,c in enumerate(['b','g','r']): 606# plot(tf,sim.getOutput(3)[i,:].T,c+'--') 607# plot(tf,sim.getOutput()[i,:].T,c,linewidth=2) 608# subplot(2,1,2) 609# for i,c in enumerate(['b','g']): 610# plot(tf,sim.getOutput(1)[i,:].T,c,linewidth=2) 611# plot(tf,sim.getOutput(2)[i,:].T,c+'--') 612# title('controls (%s)' % caption) 613 614# # Not supported in current revision, cf. #929 615# # jacsim = sim.jacobian_old(CONTROLSIMULATOR_X0,0) 616 617# # # Calculate monodromy matrix 618# # jacsim.setInput(x0,'x0') 619# # jacsim.setInput(param_,'p') 620# # jacsim.setInput(u_,'u') 621# # jacsim.evaluate() 622# # M = jacsim.getOutput()[-ns:,:] 623 624# # # Inspect the eigenvalues of M 625# # [D,V] = linalg.eig(M) 626# # print 'Spectral radius of monodromy, discrete reference, discrete control' 627 628# # print max(abs(D)) 629 630# show() 631