def plot2dCstr(aObj: Union[rel.constraint, rel.polynomial, rel.convexProg],
ax: pt.matplotlib.axes,
x0: np.ndarray = None,
opts={},
fig=None):
opts_ = {
'binaryPlot': True,
'filled': False,
'nGrid': 200,
'cbar': True,
'ctrOpts': {}
}
#opts_.update(opts)
recursiveExclusiveUpdate(opts_, opts)
if isinstance(aObj, rel.polynomial):
bRel = rel.lasserreRelax(aObj.repr)
aObj = rel.lasserreConstraint(bRel, aObj)
try:
opts_['nGrid'][0]
except TypeError:
opts_['nGrid'] = (opts_['nGrid'], opts_['nGrid'])
#Get points
xg, yg = pt.ax2Grid(ax, opts_['nGrid'])
XX = np.vstack([xg.flatten(), yg.flatten()])
if x0 is not None:
DXX = XX - x0
else:
DXX = XX
ZZ = aObj.poly.repr.evalAllMonoms(DXX)
if isinstance(aObj, rel.convexProg):
YY = sum([
aCstr.poly.eval2(ZZ) for aCstr in aObj.constraints.l +
aObj.constraints.q + aObj.constraints.s
])
else:
YY = aObj.poly.eval2(ZZ)
if opts_['binaryPlot']:
YY = (YY >= 0.).astype(nfloat)
opts_.update({'levels': [0., 1.]})
if opts_['filled']:
cc = ax.contourf(xg, yg, YY.reshape(xg.shape), **opts_['ctrOpts'])
else:
cc = ax.contour(xg, yg, YY.reshape(xg.shape), **opts_['ctrOpts'])
if opts_['cbar']:
if fig is None:
print("Cannot print colorbar without figure")
else:
fig.colorbar(cc)
return cc
def test2(funnel: distributedFunnel):
import plotting as plot
import relaxations as relax
Ngrid = 100
repr_ = funnel.dynSys.repr
relLasserre = relax.lasserreRelax(repr_)
thisPoly = polynomial(repr_)
center = 2. * (np.random.rand(2, 1) - .5)
P = 1.5 * (np.random.rand(2, 2) - .5)
P = ndot(P.T, P) + .5 * nidentity(2)
thisPoly.setEllipsoidalConstraint(center, 1., P)
lass_cstr = relax.lasserreConstraint(relLasserre, thisPoly)
ff, aa = plot.plt.subplots(1, 1)
plot.plotEllipse(aa, center, P, 1., faceAlpha=0.)
aa.autoscale()
aa.axis('equal')
xx, yy, XX = plot.ax2Grid(aa, Ngrid, True)
z = lass_cstr.poly.eval2(XX).reshape((Ngrid, Ngrid))
aa.contour(xx, yy, z, [-0.1, 0., 0.01])
is_valid = lass_cstr.isValid(XX, simpleEval=False)
is_n_valid = np.logical_not(is_valid)
aa.plot(XX[0, is_valid], XX[1, is_valid], '.g')
aa.plot(XX[0, is_n_valid], XX[1, is_n_valid], '.r')
aa.plot(center[0, :], center[1, :], 'sk')
return None
aa[0].autoscale()
xx, yy = plot.ax2Grid(aa[0], Ngrid)
XX = np.vstack((xx.flatten(), yy.flatten()))
valCstr = cstrPolySep.eval2(XX - refTraj.getX(0.))
aa[0].contour(xx, yy, valCstr.reshape((Ngrid, Ngrid)), [0.])
# Evaluate the norm of the control input
uNorm = ndot(
uCtrlLin, np.vstack((np.zeros(
(1, XX.shape[1])), XX - refTraj.getX(0.)))).flatten()
plot.plotEllipse(aa[1], refTraj.getX(0.), lyapF.P, 1., faceAlpha=0.)
aa[1].contourf(xx, yy, uNorm.reshape((Ngrid, Ngrid)))
aa[1].contour(xx, yy, uNorm.reshape((Ngrid, Ngrid)), [-10, -5, 0, 5, 10])
# Build up the optimization problem
baseRelax = relax.lasserreRelax(thisRepr)
cstrRelaxSep = relax.lasserreConstraint(
baseRelax,
cstrPolySep) # Here the 'plus' space is singled out -> negative input
cstrPolyEllip = poly.polynomial(thisRepr)
cstrPolyEllip.setQuadraticForm(
-lyapF.P, thisRepr.varNumsPerDeg[1], narray([lyapF.alpha],
dtype=nfloat),
thisRepr.varNumsPerDeg[0]) # x'.P.x<=alpha --> x'.(-P).x + alpha >= 0
cstrRelaxEllip = relax.lasserreConstraint(baseRelax, cstrPolyEllip)
# Exclude inner as zero always yields zero
excludeInner = 0.1
cstrPolyEllipInner = poly.polynomial(thisRepr)
cstrPolyEllipInner.setQuadraticForm(
lyapF.P, thisRepr.varNumsPerDeg[1],
narray([-excludeInner * lyapF.alpha],
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')
def plot2dProof(funnel: fn.distributedFunnel, t=0.0, opts={}):
from plotting.plottingCstr import plot2dCstr
opts_ = {
'zoneOpts': {
'pltStyle': 'proj',
'linewidth': 1.,
'color': [0.0, 0.0, 1.0, 1.0],
'faceAlpha': 0.0,
'linestyle': '-',
'plotAx': np.array([0, 1])
},
'streamOpts': {
'cmap': 'viridis',
'colorStreams': 'ang',
'nGrid': 200,
'cbar': True,
'plotValidOnly': True,
'validEps': -.1
},
'modeDyn': [3, 3],
'cstrOpts': {
'binaryPlot': True,
'filled': False,
'nGrid': 200,
'cbar': False,
'ctrOpts': {}
}
}
recursiveExclusiveUpdate(opts_, opts)
optsStream_ = opts_['streamOpts']
# Helper stuff
thisPoly = poly.polynomial(funnel.repr)
thisRelax = relax.lasserreRelax(funnel.repr)
#Get the subproof
try:
subProofList = funnel.proof_[t]
except KeyError:
keysT = narray(list(funnel.proof_.keys()), dtype=nfloat)
tprime = keysT[np.argmin(np.abs(keysT - t))]
print(f"Using time point {tprime} instead of {t}")
t = tprime
subProofList = funnel.proof_[t]
x0, dx0, uRef = funnel.dynSys.ctrlInput.refTraj.getX(
t), funnel.dynSys.ctrlInput.refTraj.getDX(
t), funnel.dynSys.ctrlInput.refTraj.getU(t)
allDict = {}
for nSub, (subProof, _) in enumerate(subProofList):
allDict[nSub] = {}
thisDict = allDict[nSub]
nProbs = len(subProof['origProb'])
nax = [1, 1]
while True:
if nax[0] * nax[1] >= nProbs:
break
nax[1] += 1
if nax[0] * nax[1] >= nProbs:
break
nax[0] += 1
ff, aa = plt.subplots(*nax, sharex=True, sharey=True)
aa = narray(aa, ndmin=2)
thisDict['fig'] = ff
thisDict['ax'] = aa
ff.suptitle(f"Proof {nSub} at {t:.4e}")
#Loop over the significant problems and their solutions
for k, (aVal, aProb) in enumerate(subProof['sigProbAndVals']):
thisDict[k] = {}
idx, idy = divmod(k, nax[1])
aa[idx, idy].set_title(
f"{aProb['sol']['primal objective']:.2e} : {list(aProb['origProb']['probDict']['u'].reshape((-1,)))}"
)
# Plot the zone
zonePlot = funnel.lyapFunc.plot(ax=aa[idx, idy],
t=t,
x0=x0,
opts=opts_['zoneOpts'])
# Get the grid
aa[idx, idy].autoscale()
aa[idx, idy].axis('equal')
xx, yy = ax2Grid(aa[idx, idy], opts_['streamOpts']['nGrid'])
XX = np.vstack([xx.flatten(), yy.flatten()])
DXX = XX - x0
# Plot the additional contraints and check which points satisfy them
idxValid = nones((DXX.shape[1], )).astype(np.bool_)
for aCstrCoeff in aProb['origProb']['cstr'][1:]:
thisPoly.coeffs = aCstrCoeff
thisCstr = relax.lasserreConstraint(thisRelax, thisPoly)
# Plot
plot2dCstr(thisCstr,
aa[idx, idy],
x0=x0,
opts=opts_['cstrOpts'])
# Check feasibility of points
idxValid = np.logical_and(
idxValid,
thisCstr.isValid(XX, atol=opts_['streamOpts']['validEps']))
# Get the velocities
UU = funnel.dynSys.ctrlInput.getUVal(
DXX,
aProb['origProb']['probDict']['u'],
t,
zone=funnel.lyapFunc.getZone(t),
gTaylor=funnel.dynSys.getTaylorApprox(x0)[1],
uRef=uRef)
VV = funnel.dynSys(XX, UU, mode=opts_['modeDyn'], x0=x0, dx0=dx0)
streamColor = getStreamColor(funnel.lyapFunc, XX, VV, t,
opts_['streamOpts'])
thisStream = aa[idx, idy].streamplot(
xx,
yy,
VV[0, :].reshape((optsStream_['nGrid'], optsStream_['nGrid'])),
VV[1, :].reshape((optsStream_['nGrid'], optsStream_['nGrid'])),
color=streamColor,
cmap=optsStream_['cmap'])
if optsStream_['cbar']:
thisCBar = ff.colorbar(thisStream.lines)
else:
thisCBar = None
thisDict[k]['zonePlot'] = zonePlot
thisDict[k]['stream'] = thisStream
thisDict[k]['cbar'] = thisCBar
return allDict
def doTesting(funnel: distributedFunnel):
import plotting as plot
import relaxations as relax
lyapFunc_ = funnel.lyapFunc
dynSys_ = funnel.dynSys
repr_ = funnel.dynSys.repr
relLasserre = relax.lasserreRelax(repr_)
thisPoly0 = polynomial(repr_)
thisPoly1 = polynomial(repr_)
thisPolyCstr = polynomial(repr_)
# Fill up the Lyapunov function
P0 = np.array([[2.1, 0.4], [0.4, 0.95]], dtype=nfloat)
P1 = np.array([[1.5, -0.4], [-0.4, 1.25]], dtype=nfloat)
lyapFunc_.reset()
lyapFunc_.register(0., (P0, 1.05))
lyapFunc_.register(0.5, (P1, 0.95))
t = 0.133
# Get the zone
zone = lyapFunc_.getZone(0.133)
x0, dx0, u0 = funnel.dynSys.ctrlInput.refTraj(t)
# Taylor
fTaylor, gTaylor = dynSys_.getTaylorApprox(x0)
# Get the control dict
ctrlDict, zoneP = lyapFunc_.getCtrlDict(t, fTaylor, gTaylor, True)
# Project to unit sphere
ctrlDictProj = dp(ctrlDict)
funnel.doProjection(zone, [], ctrlDictProj)
# Get the linear control law
K, Kmonoms = dynSys_.ctrlInput.getU2(2 * nones((dynSys_.nu, ), dtype=nint),
t, zone, gTaylor, x0, True, u0)
# Get a grid to evaluate on
ff, aa = plot.plt.subplots(1, 2)
lyapFunc_.plot(aa[0], t)
aa[0].autoscale()
aa[0].axis('equal')
aa[1].autoscale()
aa[1].axis('equal')
xx, yy = plot.ax2Grid(aa[0], 100)
xx *= 1.25
yy *= 1.25
XX = np.vstack((xx.flatten(), yy.flatten()))
ZZ = repr_.evalAllMonoms(XX)
dXX = XX - x0
dZX = repr_.evalAllMonoms(dXX)
# Project onto sphere
dYY = lyapFunc_.ellip2Sphere(t, dXX)
dZY = repr_.evalAllMonoms(dYY)
# Check if the taylor approx is the same as the original for polynomials
idx2 = np.random.choice(np.arange(xx.shape[1]), 1)
idx3 = np.random.choice(np.arange(xx.shape[1]), 1)
fTaylor2, gTaylor2 = dynSys_.getTaylorApprox(XX[:, idx2])
dXX2 = XX - XX[:, idx2]
dZX2 = repr_.evalAllMonoms(dXX2)
fXX = np.transpose(dynSys_.fEval(XX), (2, 1, 0))[0, :, :]
# Test other
fXXCall = dynSys_(XX - XX[:, idx3],
u=nzeros((dynSys_.nu, XX.shape[1])),
restrictInput=False,
mode=[3, 3],
x0=XX[:, idx3])
fXXtaylor1 = ndot(fTaylor, dZX[:fTaylor.shape[1], :])
fXXtaylor2 = ndot(fTaylor2, dZX2[:fTaylor2.shape[1], :])
GXX = dynSys_.gEval(XX)
GXXtaylor1 = neinsum("zij,zn->nij", gTaylor, dZX[:gTaylor.shape[0], :])
GXXtaylor2 = neinsum("zij,zn->nij", gTaylor2, dZX2[:gTaylor.shape[0], :])
if not np.allclose(fXX, fXXCall):
print(f"Prob in call ot dynSys")
if not np.allclose(fXX, fXXtaylor1):
print(f"f failed for taylor1 or dynSys")
if not np.allclose(fXX, fXXtaylor2):
print(f"f failed for taylor2 or dynSys")
if not np.allclose(GXX, GXXtaylor1):
print(f"G failed for taylor1 or dynSys")
if not np.allclose(GXX, GXXtaylor2):
print(f"G failed for taylor2 or dynSys")
fff, aaa = plot.plt.subplots(1, 1)
lyapFunc_.plot(aaa, t, opts={'faceAlpha': 0.})
aaa.axis('equal')
aaa.autoscale()
aaa.streamplot(xx, yy, fXXCall[0, :].reshape((100, 100)),
fXXCall[1, :].reshape((100, 100)))
# Check if
# Control input does not exceed limits
Vxx = lyapFunc_.evalV(dXX, t, kd=False)
idxInside = (Vxx <= zone[1]).squeeze()
aa[0].plot(XX[0, idxInside], XX[1, idxInside], '.')
# Compare with projection
Vyy = norm(dYY, axis=0, keepdims=False)
idxInsideY = Vyy <= 1.
if not nall(idxInside == idxInsideY):
idxFalse = idxInside != idxInsideY
print(
f"Failed on \n {dXX[:, idxFalse]} \n with \n {Vxx[:, idxFalse]} and \n {dYY[:, idxFalse]} \n with \n {Vyy[:, idxFalse]}"
)
aa[1].plot(dYY[0, idxInside], dYY[1, idxInside], '.')
# Compute control
# First row of K is constant term
Ulinx = ndot(K, dZX[:3, :])
# Check if limits are nowhere exceeded
uMin, uMax = np.tile(dynSys_.ctrlInput.getMinU(t),
(1, XX.shape[1])), np.tile(
dynSys_.ctrlInput.getMaxU(t), (1, XX.shape[1]))
if not nall(
np.logical_and(uMin[:, idxInside] <= Ulinx[:, idxInside],
Ulinx[:, idxInside] <= uMax[:, idxInside])):
idx = np.logical_and(
idxInside,
np.any(np.logical_or(uMin <= Ulinx, Ulinx <= uMax), axis=0))
print(f"Failed input check on :\n")
for ii, aidx in enumerate(idx):
if not aidx:
continue
print(
f"X: {list(dXX[:, ii])}; V: {float(Vxx[ii])}; U: {list(Ulinx[:,ii])}"
)
# Check consistency of convergence polynomials
# Step compare projected and original polynomials
for i in range(-1, dynSys_.nu):
for type in range(3):
try:
thisPoly0.coeffs = ctrlDict[i][type]
thisPoly1.coeffs = ctrlDictProj[i][type]
if not np.allclose(thisPoly0.eval2(dZX), thisPoly1.eval2(dZY)):
print(
f"Failed on {i}-{type}: {np.where(np.isclose(thisPoly0.eval2(dZX), thisPoly1.eval2(dZY)))}"
)
except KeyError:
print(f"Ignore keys {i}-{type}")
# Now compare the control dict convergence with the actual convergence
# No input
dxUzero = dynSys_(XX,
u=np.zeros_like(uMin),
restrictInput=False,
mode=[3, 3],
x0=x0,
dx0=dx0)
# Minimal input
dxUmin = dynSys_(XX,
u=uMin,
restrictInput=False,
mode=[3, 3],
x0=x0,
dx0=dx0)
# Maximal input
dxUmax = dynSys_(XX,
u=uMax,
restrictInput=False,
mode=[3, 3],
x0=x0,
dx0=dx0)
# Linear [2,2]
dxUlin = dynSys_(XX,
u=Ulinx,
restrictInput=False,
mode=[3, 3],
x0=x0,
dx0=dx0)
dxUlinlim = dynSys_(XX,
u=Ulinx,
restrictInput=True,
mode=[3, 3],
x0=x0,
dx0=dx0)
# Mixed [1,2]
dxUmix = dynSys_(XX,
u=np.vstack([uMin[0, :], Ulinx[1, :]]),
restrictInput=False,
mode=[3, 3],
x0=x0,
dx0=dx0)
# Using dynSys
convDynSysZeroU = lyapFunc_.evalVd(dXX, dxUzero, t, False)
convDynSysMinU = lyapFunc_.evalVd(dXX, dxUmin, t, False)
convDynSysMaxU = lyapFunc_.evalVd(dXX, dxUmax, t, False)
convDynSysLinU = lyapFunc_.evalVd(dXX, dxUlin, t, False)
convDynSysLinLimU = lyapFunc_.evalVd(dXX, dxUlinlim, t, False)
convDynSysMixU = lyapFunc_.evalVd(dXX, dxUmix, t, False)
# Using ctrlDict
convCtrlDictZeroU = ctrlDict[-1][0].copy()
thisPoly0.coeffs = convCtrlDictZeroU
convCtrlDictZeroU = thisPoly0.eval2(dZX).reshape((-1, ))
convCtrlDictMinU = ctrlDict[-1][0].copy()
convCtrlDictMinU += ctrlDict[0][-1]
convCtrlDictMinU += ctrlDict[1][-1]
thisPoly0.coeffs = convCtrlDictMinU
convCtrlDictMinU = thisPoly0.eval2(dZX).reshape((-1, ))
convCtrlDictMaxU = ctrlDict[-1][0].copy()
convCtrlDictMaxU += ctrlDict[0][1]
convCtrlDictMaxU += ctrlDict[1][1]
thisPoly0.coeffs = convCtrlDictMaxU
convCtrlDictMaxU = thisPoly0.eval2(dZX).reshape((-1, ))
convCtrlDictLinU = ctrlDict[-1][0].copy()
convCtrlDictLinU += ctrlDict[0][2]
convCtrlDictLinU += ctrlDict[1][2]
thisPoly0.coeffs = convCtrlDictLinU
convCtrlDictLinU = thisPoly0.eval2(dZX).reshape((-1, ))
convCtrlDictMixU = ctrlDict[-1][0].copy()
convCtrlDictMixU += ctrlDict[0][-1]
convCtrlDictMixU += ctrlDict[1][2]
thisPoly0.coeffs = convCtrlDictMixU
convCtrlDictMixU = thisPoly0.eval2(dZX).reshape((-1, ))
ff, aa = plot.plt.subplots(1, 1)
aa.plot(convDynSysMinU, 'r')
aa.plot(convDynSysMaxU, 'b')
aa.plot(convDynSysMixU, 'g')
aa.plot(convCtrlDictMinU, '.-r')
aa.plot(convCtrlDictMaxU, '--b')
aa.plot(convCtrlDictMixU, '*-g')
ff, aa = plot.plt.subplots(5, 1)
aa[0].plot(convDynSysZeroU, 'r')
aa[0].plot(convCtrlDictZeroU, '--b')
aa[1].plot(convDynSysMinU, 'r')
aa[1].plot(convCtrlDictMinU, '--b')
aa[2].plot(convDynSysMaxU, 'r')
aa[2].plot(convCtrlDictMaxU, '--b')
aa[3].plot(convDynSysLinU, 'r')
aa[3].plot(convCtrlDictLinU, '--b')
aa[3].plot(convDynSysLinLimU, '-.g')
aa[4].plot(convDynSysMixU, 'r')
aa[4].plot(convCtrlDictMixU, '--b')
if not np.allclose(convDynSysZeroU, convCtrlDictZeroU):
print("Failed on zero U")
if not np.allclose(convDynSysMinU, convCtrlDictMinU):
print("Failed on min U")
if not np.allclose(convDynSysMaxU, convCtrlDictMaxU):
print("Failed on max U")
if not np.allclose(convDynSysLinU, convCtrlDictLinU):
print("Failed on lin U") #todo
if not np.allclose(convDynSysMixU, convCtrlDictMixU):
print("Failed on mix U") #todo
return None