def getSysStablePos(nDims: int,
deg: int,
P=None,
G=None,
f0=None,
randomize=None):
import polynomial as poly
import trajectories as traj
repr = poly.polynomialRepr(nDims, deg)
# Get the dynamical system
pSys = getSys(repr, P, G, f0=f0, randomize=randomize)
# Get the trajectory
xTraj = lambda t: nzeros((nDims, 1), dtype=nfloat)
dxTraj = xTraj
# Compute necessary input (here 0.)
uRefTmp = pSys.getUopt(xTraj(0.), dxTraj(0.), respectCstr=False)
uTraj = lambda t: uRefTmp.copy()
# def __int__(self, fX: Callable, fU: Callable, nx: int, nu: int, fXd: Callable = None, tMin: float = 0., tMax: float = 1.):
refTraj = traj.analyticTrajectory(xTraj, uTraj, nDims, 1, dxTraj)
print(refTraj(0.))
# Get the input constraints along the refTraj
pSys.ctrlInput = boxInputCstrLFBG(repr, refTraj, pSys.nu, *getUlims())
return pSys
def getSysUnstablePos(deg: int):
from coreUtils import np
import polynomial as poly
import trajectories as traj
repr = poly.polynomialRepr(2, deg)
# Get the dynamical system
pendSys = getSys(repr)
# Get the trajectory
xTraj = lambda t: narray([[np.pi], [0.]], dtype=nfloat)
dxTraj = lambda t: narray([[0.], [0.]], dtype=nfloat)
# Compute necessary input (here 0.)
uRefTmp = pendSys.getUopt(xTraj(0, ),
dxTraj(0.),
respectCstr=False,
fullDeriv=True)
uTraj = lambda t: uRefTmp.copy()
# def __int__(self, fX: Callable, fU: Callable, nx: int, nu: int, fXd: Callable = None, tMin: float = 0., tMax: float = 1.):
refTraj = traj.analyticTrajectory(xTraj, uTraj, 2, 1, dxTraj)
print(refTraj(0.))
# Get the input constraints along the refTraj
pendSys.ctrlInput = boxInputCstrLFBG(repr, refTraj, 1, *getUlims())
return pendSys
import Lyapunov as lyap
import relaxations as relax
import plotting as plot
from plotting import plt
from scipy.integrate import solve_ivp
if __name__ == "__main__":
import plotting as plot
from plotting import plt
# Get the polynomial representation which also decides on the maximal relaxation
# Let use full here
thisRepr = poly.polynomialRepr(
2, 6, digits=1) #Todo debug digits. there is an error somewhere
#Get the dynamical system
pendSys = getSys(thisRepr, fileName=None) #"~/tmp/pendulumDict.pickle")
#Get the trajectory
xTraj = lambda t: narray([[np.pi], [0.]], dtype=nfloat)
dxTraj = lambda t: narray([[0.], [0.]], dtype=nfloat)
#Compute necessary input (here 0.)
uRefTmp = pendSys.getUopt(xTraj(0, ),
dxTraj(0.),
respectCstr=False,
fullDeriv=True)
uTraj = lambda t: uRefTmp.copy()
def testSol(sol, ctrlDict:dict):
nDim, maxDeg = sol['probDict']['dimsNDeg']
# Set up helpers
thisRepr = poly.polynomialRepr(nDim, maxDeg)
thisPoly = poly.polynomial(thisRepr)
# Test if solution(s) is(are) valid
ySol = sol['ySol']
print(f"Minimizers are:\n{ySol}")
zySol = thisRepr.evalAllMonoms(ySol)
try:
PG0n = ctrlDict['PG0']/(norm(ctrlDict['PG0'], axis=0, keepdims=True)+floatEps)
except KeyError:
pass
# Loop over constraint polynomials
for k, acstr in enumerate(sol['origProb']['cstr']):
thisPoly.coeffs = acstr
for i in range(zySol.shape[1]):
if not thisPoly.eval2(zySol[:,[i]])>=0.:
thisRelax = relax.lasserreRelax(thisRepr)
print(f"Failed on cstr {k} with {thisPoly.eval2(thisPoly.eval2(zySol[:,[i]]))} for point {i}")
thisCstr = relax.lasserreConstraint(thisRelax, thisPoly)
print(f"Original sol with {sol['sol']['x_np']} is valid : {thisCstr.isValid(sol['sol']['x_np'].T, simpleEval=False)}")
print(f"Reconstructed sol with {ySol} is valid : {thisCstr.isValid(ySol, simpleEval=False)}")
if nany(thisCstr.isValid(sol['sol']['x_np'].T, simpleEval=False) != thisCstr.isValid(ySol, simpleEval=False)):
raise RuntimeError
# Check if minimizer is compatible with control law definition
if 'PG0' in ctrlDict.keys():
dist2Planes = ndot(PG0n.T, ySol)
signPlanes = np.sign(dist2Planes).astype(nint)
signPlanes[signPlanes==0] = 1
for i,type in enumerate(sol['probDict']['u'].squeeze()):
if (nany(signPlanes[0,:] != signPlanes[0,0])) and not type==2:
print(f"Attention: minimizers are on different signs of the plane. Check wich separation is used")
if not type in list(-signPlanes[i,:])+[2]:
print(f"Failed on input {i} with dist {dist2Planes[i]} and ctrl {type}")
raise RuntimeError
# Check if minimal value corresponds to ctrlDict value
thisPoly.coeffs = -ctrlDict[-1][0]
optsVals = thisPoly.eval2(zySol).reshape((-1,))
for i,type in enumerate(sol['probDict']['u'].squeeze()):
thisPoly.coeffs = -ctrlDict[i][type]
for k in range(zySol.shape[1]):
thisVal = thisPoly.eval2(zySol[:,[k]])
optsVals[k] += thisVal
if thisVal <= 0.:
print(f"The control type {type} for input {i} failed on minimizer {k}")
print(f"global minimum was \n{sol['sol']['primal objective']}\n, computed values from ctrlDict are \n{optsVals}")
print('a')
from polynomial import polynomialRepr
from systems import acrobot
import sys
sys.path.append("/home/elfuius/ownCloud/thesis/RoAAcc3/RoA/src")
import acrobot as acroOld
polyRepr = polynomialRepr(4, 4)
acroNew = acrobot.getSys(polyRepr)
from dynamicalSystems import *
from polynomial import polynomialRepr
from systems.acrobot import getSys
if __name__ == '__main__':
thisRepr = polynomialRepr(2, 4)
thisAcro = getSys(thisRepr)
x = np.frombuffer(b'p\x0823\xd4=\xc6\xbf$\xaen\x1c\xc2\xf3\xd4\xbf@\x8fn\xe6\xa4\xa2\xb4\xbfP\x9f\x13\x92)\xec\xd8\xbf\xd4\x7fu\x96\xa4\xe3\xd3\xbfHv\x1c\x13\x16:\xca\xbf8\xe6\xef\xb0\xf5\xc4\xee\xbfT\x01;M\t\r\xd4\xbf\x08A\xc7\xa6\xdc2\xee?.\x0e\xec\x90(\x15\xe3\xbf\x00\xad\x0b\xd0\x96\xa8\x95?\xf02Mz\xfc+\xd0\xbf\xe4\x94\xe4D\x9f\x11\xe6?\xdeHR\xbb\x85B\xe7?\xc0p1\x0e\xbc\x95\xcb\xbf\xdc\x98\xb0{\xf8\x7f\xea\xbf\xe4b"1\xf2\t\xe2\xbf\x00\x87\x1c\x89\xf8\x97\xcd\xbfD\xe2S\x8c\x06\xe8\xda?\x02.>T\x9b^\xe3\xbf')
x = x.reshape((4,-1))
for k in range(x.shape[1]):
taylorAll = thisAcro.getTaylorApprox(x[:,k])
print("string fTaylor")
print(taylorAll[0].tostring())
print("string GTaylor")
print(taylorAll[1].tostring())