#[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