def prepOMPLTraj(dynSys, basicTraj, dT=0.01, toFile=None):
zoneFac = 1.
thisTSteps = basicTraj.t
thisLyap = lyap.lqrShapeGen(dynSys)
#Get a lqr controller
allK = []
allP = []
for aT in thisTSteps:
P, _ = thisLyap(basicTraj.xref(aT))
allK.append(np.copy(.0 * thisLyap.lastK))
allP.append(np.copy(P))
allT = np.arange(thisTSteps[0], thisTSteps[-1], dT)
allT = np.hstack((allT, thisTSteps[-1]))
#Get a smooth trajectory with corrected inputs
allX = np.zeros((allT.size, dynSys.nx))
allXd = np.zeros((allT.size, dynSys.nx))
allU = np.zeros((allT.size, dynSys.nu))
allX[0, :] = basicTraj.xref(0.0).squeeze()
allU[0, :] = basicTraj.uref(0.0).squeeze()
#fInt = lambda thisX, thisT: dynSys( thisX.reshape((dynSys.nx,1)), basicTraj.uref(thisT)).squeeze()
#intAllX = scipy.integrate.odeint(fInt, allX[0,:].squeeze(), allT)
dynSys.innerZone *= zoneFac
for k in range(1, allT.size):
##Get the integration function
#(self, x, u, prec = 64, doRestrain=True)
indK = np.maximum(0, np.searchsorted(thisTSteps, allT[k - 1]) - 1)
#fInt = lambda thisX, thisT: dynSys( thisX.reshape((dynSys.nx,1)), basicTraj.uref(thisT) - np.dot(allK[indK], replaceSo2(thisX.reshape((dynSys.nx,1))-basicTraj.xref(thisT), dynSys.so2Dims) )).squeeze()
fInt = lambda thisX, thisT: dynSys(
thisX.reshape((dynSys.nx, 1)),
basicTraj.uref(thisT) - np.dot(
allK[indK],
thisX.reshape(
(dynSys.nx, 1)) - basicTraj.xref(thisT))).squeeze()
#getBestF(self, Pt, x0, x, mode='OO', u0 = None, t=0.):
#fInt = lambda thisX, thisT: dynSys.getBestF(allP[indK], basicTraj.xref(thisT), thisX.reshape((dynSys.nx,1)), mode="OR", u0=basicTraj.uref(thisT), t=thisT).squeeze()
intX = (scipy.integrate.odeint(fInt, allX[k - 1, :].squeeze(),
allT[[k - 1, k]])[1, :]).reshape(
(dynSys.nx, 1))
intU = basicTraj.uref(allT[k]) - np.dot(
allK[indK],
replaceSo2(intX - basicTraj.xref(allT[k]), dynSys.so2Dims))
allX[k, :] = intX.squeeze()
allU[k, :] = dynSys.inputCstr(intU, t=allT[k]).squeeze()
allXd[k, :] = dynSys(intX, dynSys.inputCstr(intU, t=allT[k])).squeeze()
#Treat so2
if not (dynSys.so2Dims is None) and False:
allX[dynSys.so2Dims, :] = toSo2(allX[dynSys.so2Dims, :])
if not (toFile is None):
with open(toFile + "/X.txt", "w") as f:
np.savetxt(f, allX.T)
with open(toFile + "/Xd.txt", "w") as f:
np.savetxt(f, allXd.T)
with open(toFile + "/U.txt", "w") as f:
np.savetxt(f, allU.T)
with open(toFile + "/T.txt", "w") as f:
np.savetxt(f, allT.squeeze())
dynSys.innerZone *= (1. / zoneFac)
return allT.squeeze(), allX.T, allXd.T, allU.T
##Import prob
from invertedPendulum import *
t = sympy.symbols('t')
xrefs = sMa([np.pi, 0.0])
xdrefs = sMa([0.0, 0.0])
urefs = sMa([[0]])
#Trajectories
refTraj = traj.analyticTraj(nx, nu, xrefs, urefs, t, xdrefs)
#Get a LQR controller as shape generator
Q = np.diag([1, 1])
R = np.array([[5.]])
shapeGenStd = lyapS.lqrShapeGen(dynSys, reshape=False, Q=Q, R=R)
shapeGen = shapeGenStd
#How to calculate the conv
relaxationClass = lyapPar.NCrelax(nx, mode='PL', excludeInner=excludeInner)
#Some shortcuts
iMx, iMX, iMZt, iMA, iMX2z, numVars = relaxationClass.iMx, relaxationClass.iMX, relaxationClass.iMZt, relaxationClass.iMA, relaxationClass.iMX2z, relaxationClass.numVars
#Get the position to search a surrounding region of stabilizability
xFinal = refTraj(2.)[0]
#There are two modes:
shapeMode = 4
#1 Use the LQR shape
#2 Use the shape obtained beforehand using the drake toolbox
#>3 Predefined showcase
def prepOMPLTraj2(dynSys, basicTraj, **kwargs):
opts = {
'dT': 0.01,
'toFile': None,
'zoneFac': 1.,
'gain': 0.001,
'intType': 'LQR',
'gainVel': 0.0,
'velInd': np.arange(dynSys.nx // 2, dynSys.nx, 1, dtype=np.int_)
}
opts.update(kwargs)
dynSys.innerZone *= opts['zoneFac']
#Get a lqr controller
allK = []
allP = []
thisLyap = lyap.lqrShapeGen(dynSys, reshape=False)
for aT in basicTraj.t:
P, _ = thisLyap(basicTraj.xref(aT))
allK.append(np.copy(opts['gain'] * thisLyap.lastK))
allK[-1][:,
opts['velInd']] = opts['gainVel'] * allK[-1][:,
opts['velInd']]
allP.append(np.copy(P))
allT = np.array(basicTraj.t[0])
allX = np.zeros((dynSys.nx, 1))
allXd = np.zeros((dynSys.nx, 1))
allU = np.zeros((dynSys.nu, 1))
allX[:, 0] = basicTraj.xref(basicTraj.t[0]).squeeze()
allU[:, 0] = basicTraj.uref(basicTraj.t[0]).squeeze()
indCurr = 0
for k in range(basicTraj.t.size - 1):
thisT = np.linspace(
basicTraj.t[k],
basicTraj.t[k + 1],
int((basicTraj.t[k + 1] - basicTraj.t[k]) / opts['dT']) + 1,
endpoint=True)
if opts['intType'] == 'LQR':
fInt = lambda thisX, thisT: dynSys(
thisX.reshape((dynSys.nx, 1)),
basicTraj.uref(thisT) - np.dot(
allK[k],
replaceSo2(
thisX.reshape((dynSys.nx, 1)) - basicTraj.xref(thisT),
dynSys.so2Dims))).squeeze()
else:
fInt = lambda thisX, thisT: dynSys.getBestF(
allP[k],
basicTraj.xref(thisT),
thisX.reshape((dynSys.nx, 1)),
mode="OR",
u0=basicTraj.uref(thisT),
t=thisT).squeeze()
intX = scipy.integrate.odeint(fInt, allX[:, indCurr].squeeze(), thisT)
intX = intX.T
#Get the other values
refX = basicTraj.xref(thisT)
intU = dynSys.inputCstr(
basicTraj.uref(thisT) -
np.dot(allK[k], replaceSo2(intX - refX, dynSys.so2Dims)),
t=thisT)
intXd = dynSys(intX, intU)
allT = np.hstack((allT, thisT[1:]))
allX = np.hstack((allX, intX[:, 1:]))
allXd = np.hstack((allXd, intXd[:, 1:]))
allU = np.hstack((allU, intU[:, 1:]))
indCurr = allT.size - 1
#Treat so2
if not (dynSys.so2Dims is None) and False:
allX[dynSys.so2Dims, :] = toSo2(allX[dynSys.so2Dims, :])
if not (opts['toFile'] is None):
with open(opts['toFile'] + "/X.txt", "w") as f:
np.savetxt(f, allX.T)
with open(opts['toFile'] + "/Xd.txt", "w") as f:
np.savetxt(f, allXd.T)
with open(opts['toFile'] + "/U.txt", "w") as f:
np.savetxt(f, allU.T)
with open(opts['toFile'] + "/T.txt", "w") as f:
np.savetxt(f, allT.squeeze())
dynSys.innerZone *= (1. / opts['zoneFac'])
return allT.squeeze(), allX, allXd, allU
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
shapeGenStd = lyapS.lqrShapeGen(
dynSys, reshape=False, Q=np.diag([10, 10, 1, 1]),
R=np.array([[1.e-1]])) #For the final zone; Can also be imposed by hand
shapeGen = lyapS.timeVaryingLqrShapegen(dynSys,
Q=np.diag([1., 1., 1., 1.]),
R=np.array([[1.e-1]]),
reshape=False)
shapeGen.restart = True
shapeGen.refTraj = refTraj
shapeGen.ctrlSafeFac = 1.
shapeGen.retAll = True
shapeGen.interSteps = 10
#How to calculate the conv
relaxationClass = lyapPar.NCrelax(nx, mode='PL', excludeInner=excludeInner)
#Some shortcuts