#[x1,x2,x3,x4] = [q1, q2, qd1, qd2] dynF = sMz(4,1) dynF[0:2,0]=x[2:,0] dynF[2:,0]=xdd #Control mapping -> Non-linear here #M.inv()*B*u dynG = sMz(4,1) dynG[2:,0] = Mi*sMa([0,1]) #Numbers nx = 4 nu = 1 #Create the dict/matrices needed for numeric vars nX = (nx*(nx+1))//2 #uLim = [min(1.25*np.min(allU),-2.), max(1.25*np.max(allU),2)] inputCstr = ds.boxCstr(nu, uLim[0],uLim[1]) #dynSys = ds.dynamicalSys(dynF, dynG, varNames=x, inputCstr=inputCstr, so2DimsIn=[0,1]) dynSys = ds.dynamicalSys(dynF, dynG, varNames=x, inputCstr=inputCstr, sysName='acrobot', so2DimsIn=None)#The first two elements of x are indeed so2 states. However this currently provide no advantage and complicates the calculation of x.T.P.x and the plotting dynSys.innerZone = 10e-2; #Define a function for plotting def getDrawPos(X): pos = np.zeros((2,3)) pos[0,1] = l1*np.sin(X[0]) pos[1,1] = -l1*np.cos(X[0]) pos[0,2] = pos[0,1] + l2*np.sin(X[0]+X[1]) pos[1,2] = pos[1,1] - l2*np.cos(X[0]+X[1]) return pos
ellipInterpMode = 6 doPlot = 1 #Whether plotting is to be done doCalc = 1 #Whether the calculation should be performed #Trajectories #Load interpolation Traj try: #Try to load a saved interpolation basicTraj = traj.OMPLtrajectory(4, 1, "../trajectories/acroSwingUp10.txt") refTraj = traj.OMPLtrajectory2(dynSys, "../trajectories/acroSwingUp10/") except: #Do a finer interpolation #and save the result basicTraj = traj.OMPLtrajectory(4, 1, "../trajectories/acroSwingUp10.txt") dynSys.inputCstr = ds.boxCstr(nu, -10.1, 10.1) newT, newX, newXd, newU = traj.prepOMPLTraj2(dynSys, basicTraj, dT=0.001, toFile=None) #Get a new trajectory based on the pretreated values refTraj = traj.OMPLtrajectory2(dynSys, newX, newU, newT) refTraj.saveToPath("../trajectories/acroSwingUp10/") inputCstr = ds.boxCstr(nu, -20., 20.) #Set the actuator limits dynSys.inputCstr = inputCstr #Get the shape generator #In this case a modified version of finite horizon lqr controller #that is modified such that input constraints are partially accounted for
from coreUtils import * from plotUtils import * import trajectories as traj import dynamicSystems as ds import lyapunovShapes as lyapS import lyapunov_Parallel as lyapPar import time solvers.options['show_progress'] = False #Get the dynamical system definitions from acrobot import * #Define the input bounds inputCstr = ds.boxCstr(nu, -20, 20) dynSys.inputCstr = inputCstr #Options excludeInner = None ellipInterpMode = 2 mtlbPath = './matlab/Acrobot/' #Get a symbolic input that corresponds to the upright position #The algorithm always except a "trajectory" so this is a workaround Tlim = [0.0, 0.75] Nsteps = 2 tSteps = np.linspace(Tlim[0], Tlim[1], Nsteps) t = sympy.symbols('t') urefSym = sMa([0.])
import lyapunovShapes as lyapShape import lyapunov_Parallel as lyapPar import dynamicSystems as ds from copy import deepcopy as dp import matplotlib.animation as animation import matplotlib as mpl mpl.verbose.set_level('helpful') plt.rc('text', usetex=True) plt.rc('font', family='serif') #Get the system from acrobot import * inputCstr = ds.boxCstr(nu, -20., 20.) dynSys.inputCstr = inputCstr dynSys.innerZone = 1.e-2 #Smoothening zone #Get the trajectory try: basicTraj = traj.OMPLtrajectory( 4, 1, "/home/elfuius/ownCloud/thesis/RoA/ompl/ompl/build/acroSwingUp8.txt") refTraj = traj.OMPLtrajectory2( dynSys, "/home/elfuius/ownCloud/thesis/RoA/ompl/ompl/build/acroSwingUp8/") except: basicTraj = traj.OMPLtrajectory( 4, 1, "/home/elfuius/ownCloud/thesis/RoA/ompl/ompl/build/acroSwingUp8.txt")
import time from invertedPendulum import * #Get the dynamic systems solvers.options['show_progress'] = False excludeInner = None #The inner ellipsoid in which convergence is not verified; Convergence is proved for \all x: excludeInner <= x.T.P.x <= 1. ellipInterpMode = 6 #Linear interpolation of cholesky factorizations doCalc = 1 #Whether plotting is to be done doPlot = 1 #Whether the calculation should be performed #Get the trajectories basicTraj = traj.OMPLtrajectory(2, 1, "../trajectories/invPendSwingUp.txt") dynSys.inputCstr = ds.boxCstr(nu, -2.6, 2.6) newT, newX, newXd, newU = traj.prepOMPLTraj2(dynSys, basicTraj, dT=0.01, toFile=None) #Get a new trajectory based on the pretreated values refTraj = traj.OMPLtrajectory2(dynSys, newX, newU, newT) #Reset the actuator limits to have a margin inputCstr = ds.boxCstr(nu, -3, 3) dynSys.inputCstr = inputCstr #Use the standard finite horizon lqr controller Q = np.diag([1., 1.4]) R = np.array([[0.005]])
dynF = [ 'x1', '(-{0:.8f}*{1:.8f}*{2:.8f}*sin(x0) - {3:.8f}*x1)/{4:.8f}'.format( m, g, lc, b, I) ] #Input dynamics (linear !) dynG = [['0.'], ['1./{0:.8f}'.format(I)]] #Numbers nx = 2 nX = (nx * (nx + 1)) // 2 nu = 1 #dynSys = ds.dynamicalSys(dynF, dynG, so2DimsIn=[0]) dynSys = ds.dynamicalSys( dynF, dynG, so2DimsIn=None, sysName="invPend" ) #The first element of x is indeed so2 states. However this currently provide no advantage and complicates the calculation of x.T.P.x and the plotting dynSys.innerZone = 10e-2 xSym, ySym, uSym = dynSys.x, dynSys.xTrans, dynSys.u #Constraints inputCstr = ds.boxCstr(nu, -3, 3) dynSys.inputCstr = inputCstr #Define a function for plotting def getDrawPos(X): pos = np.zeros((2, 2)) pos[0, 1] = l * np.sin(X[0]) pos[1, 1] = -l * np.cos(X[0]) return pos