def LQR(self, ztraj, utraj, Q, R, Qf):
tspan = utraj.get_segment_times()
dztrajdt = ztraj.derivative(1)
context = self.CreateDefaultContext()
nZ = context.num_continuous_states()
nU = self.GetInputPort('u').size()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
z = prog.NewIndeterminates(nZ,'z')
ucon = prog.NewIndeterminates(nU,'u')
#nY = self.GetOutputPort('z').size()
#N = np.zeros([nX, nU])
times = ztraj.get_segment_times()
K = []
S = []
for t in times:
# option 1
z0 = ztraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
sym_context.SetContinuousState(z0+z)
sym_context.FixInputPort(0, u0+ucon )
# zdot=f(z,u)==>zhdot=f(zh+z0,uh+u0)-z0dot
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose()
mapping = dict(zip(z, z0))
mapping.update(dict(zip(ucon, u0)))
A = Evaluate(Jacobian(f, z), mapping)
B = Evaluate(Jacobian(f, ucon), mapping)
k, s = LinearQuadraticRegulator(A, B, Q, R)
import pdb; pdb.set_trace()
if(len(K) == 0):
K = np.ravel(k).reshape(nU*nZ,1)
S = np.ravel(s).reshape(nZ*nZ,1)
else:
K = np.hstack( (K, np.ravel(k).reshape(nU*nZ,1)) )
S = np.hstack( (S, np.ravel(s).reshape(nZ*nZ,1)) )
#
# option 2
#context.SetContinuousState(xtraj.value(t) )
#context.FixInputPort(0, utraj.value(t) )
#linearized_plant = Linearize(self, context)
#K.append(LinearQuadraticRegulator(linearized_plant.A(),
# linearized_plant.B(),
# Q, R)) #self, context, Q, R)
Kpp = PiecewisePolynomial.FirstOrderHold(times, K)
return Kpp
def RegionOfAttraction(self, context, zdotraj, V=None):
z0 = context.get_continuous_state_vector().CopyToVector()
if(zdotraj is None):
zdotraj = 0.0*z0
# Check that x0 is a "fixed point" (on the trajectory).
zdot0 = self.EvalTimeDerivatives(context).CopyToVector()
assert np.allclose(zdot0, zdotraj), "context does not describe a fixed point." #0*xdot0),
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
z = prog.NewIndeterminates(sym_context.num_continuous_states(),'z')
# Evaluate the dynamics (in relative coordinates)
sym_context.SetContinuousState(z0+z)
#import pdb; pdb.set_trace()
uinput = self.GetInputPort('u')
u0 = uinput.EvalBasicVector(context).CopyToVector()
nU = uinput.size()
ucon = prog.NewIndeterminates(nU,'u')
sym_context.FixInputPort(0, u0+ucon )
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
mapping = dict(zip(z, z0))
mapping.update(dict(zip(ucon, u0)))
if V is None:
# Solve a Lyapunov equation to find the Lyapunov candidate.
A = Evaluate(Jacobian(f, z), mapping)
Q = np.eye(sym_context.num_continuous_states())
import pdb; pdb.set_trace()
P = RealContinuousLyapunovEquation(A, Q)
V = x.dot(P.dot(z))
Vdot = V.Jacobian(z).dot(f)
# Check Hessian of Vdot at origin
H = Evaluate(0.5*Jacobian(Vdot.Jacobian(z),z), mapping)
assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, z, V, Vdot)
V = self.FixedLyapunovSearchRho(prog, z, V, Vdot)
# Put V back into global coordinates
mapping = dict(zip(z,z-z0))
mapping.update(dict(zip(ucon, u0)))
V = V.Substitute(mapping)
return V
def SOS_compute_3(self, K, l_coeff, rho_max=10.):
prog = MathematicalProgram()
# fix u and lbda, search for V and rho
x = prog.NewIndeterminates(2, "x")
# get lbda from before
l = np.array([x[1], x[0], 1])
lbda = l.dot(np.dot(l_coeff, l))
# rho is decision variable now
rho = prog.NewContinuousVariables(1, "rho")[0]
# create lyap V
s = prog.NewContinuousVariables(4, "s")
S = np.array([[s[0], s[1]], [s[2], s[3]]])
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
prog.AddSosConstraint(V)
prog.AddSosConstraint(-Vdot - lbda * (rho - V))
prog.AddLinearCost(-rho)
prog.AddLinearConstraint(rho <= rho_max)
prog.Solve()
rho = prog.GetSolution(rho)
s = prog.GetSolution(s)
return s, rho
def SOS_compute_2(self, l_coeff, S, rho_max=10.):
prog = MathematicalProgram()
# fix V and lbda, searcu for u and rho
x = prog.NewIndeterminates(2, "x")
# get lbda from before
l = np.array([x[1], x[0], 1])
lbda = l.dot(np.dot(l_coeff, l))
# Define u
K = prog.NewContinuousVariables(2, "K")
# Fixed Lyapunov
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
# rho is decision variable now
rho = prog.NewContinuousVariables(1, "rho")[0]
prog.AddSosConstraint(-Vdot - lbda * (rho - V))
prog.AddLinearConstraint(rho <= rho_max)
prog.AddLinearCost(-rho)
prog.Solve()
rho = prog.GetSolution(rho)
K = prog.GetSolution(K)
return rho, K
def SOS_compute_1(self, S, rho_prev):
# fix V and rho, search for L and u
prog = MathematicalProgram()
x = prog.NewIndeterminates(2, "x")
# Define u
K = prog.NewContinuousVariables(2, "K")
# Fixed Lyapunov
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
# Define the Lagrange multipliers.
(lambda_, constraint) = prog.NewSosPolynomial(Variables(x), 2)
prog.AddLinearConstraint(K[0] * x[0] <= 2.5)
prog.AddSosConstraint(-Vdot - lambda_.ToExpression() * (rho_prev - V))
result = prog.Solve()
# print(lambda_.ToExpression())
# print(lambda_.decision_variables())
lc = [prog.GetSolution(var) for var in lambda_.decision_variables()]
lbda_coeff = np.ones([3, 3])
lbda_coeff[0, 0] = lc[0]
lbda_coeff[0, 1] = lbda_coeff[1, 0] = lc[1]
lbda_coeff[2, 0] = lbda_coeff[0, 2] = lc[2]
lbda_coeff[1, 1] = lc[3]
lbda_coeff[2, 1] = lbda_coeff[1, 2] = lc[4]
lbda_coeff[2, 2] = lc[5]
return lbda_coeff
def my_plot_sublevelset_expression(self, ax, e, x0, vertices=51): #, **kwargs):
p = Polynomial(e)
assert p.TotalDegree() == 2
x = list(e.GetVariables())
env = {a: 0 for a in x}
c = e.Evaluate(env)
e1 = e.Jacobian(x)
b = Evaluate(e1, env)
e2 = Jacobian(e1, x)
A = 0.5*Evaluate(e2, env)
# simple projection to 2D assuming we want to plot on (x1,x2)
A = A[0:2,0:2]
b = b[0:2]*0.0
c = 0.0
#Plots the 2D ellipse representing x'Ax + b'x + c <= 1, e.g.
#the one sub-level set of a quadratic form.
H = .5*(A+A.T)
xmin = np.linalg.solve(-2*H, np.reshape(b, (2, 1)))
fmin = -xmin.T.dot(H).dot(xmin) + c # since b = -2*H*xmin
assert fmin <= 1, "The minimum value is > 1; there is no sub-level set " \
"to plot"
# To plot the contour at f = (x-xmin)'H(x-xmin) + fmin = 1,
# we make a circle of values y, such that: y'y = 1-fmin,
th = np.linspace(0, 2*np.pi, vertices)
Y = np.sqrt(1-fmin)*np.vstack([np.sin(th), np.cos(th)])
# then choose L'*(x - xmin) = y, where H = LL'.
L = np.linalg.cholesky(H)
X = np.tile(xmin, vertices) + np.linalg.inv(np.transpose(L)).dot(Y)
return ax.fill(X[0, :]+x0[0], X[1, :]+x0[1], "b")
def LQR(self, xtraj, utraj, Q, R, Qf):
tspan = utraj.get_segment_times()
context = self.CreateDefaultContext()
nX = context.num_continuous_states()
nU = self.GetInputPort('u').size()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(nX, 'x')
ucon = prog.NewIndeterminates(nU, 'u')
#nY = self.GetOutputPort('x').size()
#N = np.zeros([nX, nU])
times = xtraj.get_segment_times()
K = []
import pdb
pdb.set_trace()
for t in times:
# option 1
x0 = xtraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
sym_context.SetContinuousState(x0 + x)
sym_context.FixInputPort(0, u0 + ucon)
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
B = Evaluate(Jacobian(f, ucon), dict(zip(ucon, u0)))
K.append(LinearQuadraticRegulator(A, B, Q, R))
# option 2
#context.SetContinuousState(xtraj.value(t) )
#context.FixInputPort(0, utraj.value(t) )
#linearized_plant = Linearize(self, context)
#K.append(LinearQuadraticRegulator(linearized_plant.A(),
# linearized_plant.B(),
# Q, R)) #self, context, Q, R)
Kpp = PiecewisePolynomial.FirstOrderHold(times, K)
return Kpp
def RegionOfAttraction(self, context, V=None):
x0 = context.get_continuous_state_vector().CopyToVector()
# Check that x0 is a fixed point.
xdot0 = self.EvalTimeDerivatives(context).CopyToVector()
assert np.allclose(
xdot0, 0 * xdot0), "context does not describe a fixed point."
import pdb
pdb.set_trace()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(sym_context.num_continuous_states(),
'x')
# Evaluate the dynamics (in relative coordinates)
sym_context.SetContinuousState(x0 + x)
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
if V is None:
# Solve a Lyapunov equation to find the Lyapunov candidate.
A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
Q = np.eye(sym_context.num_continuous_states())
import pdb
pdb.set_trace()
P = RealContinuousLyapunovEquation(A, Q)
V = x.dot(P.dot(x))
Vdot = V.Jacobian(x).dot(f)
# Check Hessian of Vdot at origin
H = Evaluate(0.5 * Jacobian(Vdot.Jacobian(x), x), dict(zip(x, x0)))
assert isPositiveDefinite(
-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, x, V, Vdot)
V = self.FixedLyapunovSearchRho(prog, x, V, Vdot)
# Put V back into global coordinates
V = V.Substitute(dict(zip(x, x - x0)))
return V
def RegionOfAttraction(system, context, V=None):
# TODO(russt): Add python binding for has_only_continuous_state
# assert(context.has_only_continuous_state())
# TODO(russt): Handle more general cases.
x0 = context.get_continuous_state_vector().CopyToVector()
# Check that x0 is a fixed point.
xdot0 = system.EvalTimeDerivatives(context).get_vector().CopyToVector()
#assert np.allclose(xdot0, 0*xdot0), "context does not describe a fixed point."
sym_system = system.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(sym_context.num_continuous_states(),'x')
# Evaluate the dynamics (in relative coordinates)
sym_context.SetContinuousState(x0+x)
f = sym_system.EvalTimeDerivatives(sym_context).get_vector().CopyToVector()
if V is None:
# Solve a Lyapunov equation to find the Lyapunov candidate.
#A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
#A = np.array([[0., 1.], [-10.*np.cos(x[0]), -0.1]])
#A = A.Evaluate(dict(zip(x, x0)))
A = np.array([[0., 1.], [-10.*np.cos(x0[0]), -0.1]])
Q = np.eye(sym_context.num_continuous_states())
P = RealContinuousLyapunovEquation(A, Q)
V = x.dot(P.dot(x))
#for i in range(len(f)):
# f[i] = f[i].Substitute(dict(zip(x,x-x0)))
Vdot = V.Jacobian(x).dot(f)
# Check Hessian of Vdot at origin
import pdb; pdb.set_trace()
H = Evaluate(0.5*Jacobian(Vdot.Jacobian(x),x), dict(zip(x, 0*x0)))
print('H=')
print(H)
print('P(Lyapunov)=')
print(P)
assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, x, V, Vdot)
V = FixedLyapunovSearchRho(prog, x, V, Vdot)
# Put V back into global coordinates
V = V.Substitute(dict(zip(x,x-x0)))
return V
def Cdre_CL(self, t, S, Q, Rinv, xtraj, utraj, x, ucon, K): x0 = xtraj.value(t).transpose()[0] u0 = utraj.value(t).transpose()[0] x0d = xtraj.derivative(1).value(t).transpose()[0] Kf = K.value(t).reshape(self.nU, self.nX) zero_map = dict(zip(x,np.zeros(self.nX))) f_Lcl = self.EvalClosedLoopDynamics(x, ucon, x0, u0, x0d, Kf, order=1) # linearization CL Af = Evaluate(Jacobian(f_Lcl, x), zero_map) # because this is rel. coord. s1 = S.reshape(self.nX, self.nX) # comes in as vector, switch to matrix S1dot = -(Af.transpose().dot(s1) + s1.dot(Af) + Q) return S1dot.ravel() # return as vector
def SOS_traj_optim(S, rho_guess):
# S provides the initial V guess
# STEP 1: search for L and u with fixed V and p
mp1 = MathematicalProgram()
x = mp1.NewIndeterminates(3, "x")
V = x.dot(np.dot(S, x))
print(S)
# Define the Lagrange multipliers.
(lambda_, constraint) = mp1.NewSosPolynomial(Variables(x), 4)
xd = mp1.NewFreePolynomial(Variables(x), 2)
yd = mp1.NewFreePolynomial(Variables(x), 2)
thetd = mp1.NewFreePolynomial(Variables(x), 2)
u = np.vstack((xd, yd))
u = np.vstack((u, thetd))
Vdot = Jacobian([V], x).dot(plant(x, u))[0]
mp1.AddSosConstraint(-Vdot + lambda_.ToExpression() * (V - rho_guess))
result = mp1.Solve()
# print(type(lambda_).__dict__.keys())
print(type(lambda_.decision_variables()).__dict__.keys())
L = [mp1.GetSolution(var) for var in lambda_.decision_variables()]
# print(lambda_.monomial_to_coefficient_map())
return L, u
def RegionOfAttraction(self, xtraj, utraj, V=None):
# Construct a polynomial V that contains all monomials with s,c,thetadot up to degree 2.
#deg_V = 2
do_normalization = False
do_balancing = False #True
do_use_Slti = True
# Construct a polynomial L representing the "Lagrange multiplier".
deg_L = 4
taylor_order = 3
Q = 10.0*np.eye(self.nX)
R = 1.0*np.eye(self.nU)
Qf = 1.0*np.eye(self.nX)
rho_f = 1.0
xdotraj = xtraj.derivative(1)
# set some constraints on inputs
#context.SetContinuousState(xtraj.value(xtraj.end_time()))
#context.FixInputPort(0, utraj.value(utraj.end_time()))
#x0 = context.get_continuous_state_vector().CopyToVector()
#if(xdotraj is None):
# xdotraj = xtraj.Derivative(1)
# Check that x0 is a "fixed point" (on the trajectory).
#xdot0 = self.EvalTimeDerivatives(context).CopyToVector()
#assert np.allclose(xdot0, xdotraj), "context does not describe valid path." #0*xdot0),
prog = MathematicalProgram()
x = prog.NewIndeterminates(self.nX, 'x')
ucon = prog.NewIndeterminates(self.nU, 'u')
#sym_system = self.ToSymbolic()
#sym_context = sym_system.CreateDefaultContext()
#sym_context.SetContinuousState(x)
#sym_context.FixInputPort(0, ucon )
#f = sym_system.EvalTimeDerivativesTaylor(sym_context).CopyToVector()
times = xtraj.get_segment_times()
all_V = [] #might be transformed
all_Vd = [] #might be transformed
all_Vd2 = []
all_fcl = [] #might be transformed
all_T = [] #might be transformed
all_x0 = [] #might be transformed
sumV = 0.0
zero_map = dict(zip(x,np.zeros(self.nX)))
if(do_use_Slti):
# get the final ROA to be the initial condition (of t=end) of the S from Ricatti)
for tf in [times[-1]]:
#tf = times[-1]
xf = xtraj.value(tf).transpose()[0]
xdf = xdotraj.value(tf).transpose()[0]
uf = utraj.value(tf).transpose()[0]
Af, Bf = self.PlantDerivatives(xf, uf) #0.0*uf)
Kf, __ = LinearQuadraticRegulator(Af, Bf, Q, R)
# get a polynomial representation of f_closedloop, xdot = f_cl(x)
# where x is actually in rel. coord.
f_Lcl = self.EvalClosedLoopDynamics(x, ucon, xf, uf, xdf, Kf, order=1) # linearization CL
Af = Evaluate(Jacobian(f_Lcl, x), zero_map) # because this is rel. coord.
Qf = RealContinuousLyapunovEquation(Af, Q)
f_cl = self.EvalClosedLoopDynamics(x, ucon, xf, uf, xdf, Kf, order=taylor_order) # for dynamics CL
# make sure Lyapunov candidate is pd
if (self.isPositiveDefinite(Qf)==False):
assert False, '******\nQf is not PD for t=%f\n******' %(tf)
Vf = (x-xf).transpose().dot(Qf.dot((x-xf)))
#import pdb; pdb.set_trace()
if(do_normalization):
#coeffs = Polynomial(Vf).monomial_to_coefficient_map().values()
#sumV = 0.
#for coeff in coeffs:
# sumV = sumV + np.abs(coeff.Evaluate())
sumV = Vf.Evaluate(dict(zip(x, xf+np.ones(self.nX)))) #do: V(1,1,...)=1
Vf = Vf / sumV #normalize coefficient sum to one
Qf = Qf / sumV
Vfdot = Vf.Jacobian(x).dot(f_cl) # we're just doing the static final point to get Rho_f
#H = Evaluate(0.5*Jacobian(Vfdot.Jacobian(x),x), zero_map)
#if (self.isPositiveDefinite(-H)==False):
# assert False, '******\nVdot is not ND at the end point, for t=%f\n******' %(tf)
if(do_balancing):
#import pdb; pdb.set_trace()
S1 = Evaluate(0.5*Jacobian(Vf.Jacobian(x),x), zero_map)
S2 = Evaluate(0.5*Jacobian(Vfdot.Jacobian(x),x), zero_map)
T = self.balanceQuadraticForm(S1, S2)
balanced_x = T.dot(x)
balance_map = dict(zip(x, balanced_x))
Vf = Vf.Substitute(balance_map)
Vfdot = Vfdot.Substitute(balance_map)
for i in range(len(f_cl)):
f_cl[i] = f_cl[i].Substitute(balance_map)
xf = np.linalg.inv(T).dot(xf) #the new coordinates of the equilibrium point
rhomin = 0.0
rhomax = 1.0
#import pdb; pdb.set_trace()
#deg_L = Polynomial(Vfdot).TotalDegree()
# First bracket the solution
while self.CheckLevelSet(x, xf, Vf, Vfdot, rhomax, multiplier_degree=deg_L) > 0:
rhomin = rhomax
rhomax = 1.2*rhomax
#print('Rho_max = %f' %(rhomax))
tolerance = 1e-4
slack = -1.0
while rhomax - rhomin > tolerance:
rho_f = (rhomin + rhomax)/2
slack = self.CheckLevelSet(x, xf, Vf, Vfdot, rho_f, multiplier_degree=deg_L)
if slack >= 0:
rhomin = rho_f
else:
rhomax = rho_f
rho_f = (rhomin + rhomax)/2
rho_f = rho_f*0.8 # just to be on the safe-side. REMOVE WHEN WORKING
print('Rho_final(t=%f) = %f; slack=%f' %(tf, rho_f, slack))
#import pdb; pdb.set_trace()
#import pdb; pdb.set_trace()
# end of getting initial conditions
if V is None:
# Do optimization to find the Lyapunov candidate.
#print('******\nRunning SOS to find Lyapunov function ...')
#V, Vdot = self.findLyapunovFunctionSOS(xtraj, utraj, deg_V, deg_L)
# or Do tvlqr to get S
print('******\nRunning TVLQR ...')
K, S = self.TVLQR(xtraj, utraj, Q, R, Qf)
#S0 = S.value(times[-1]).reshape(self.nX, self.nX)
#print(Polynomial(x.dot(S0.dot(x))).RemoveTermsWithSmallCoefficients(1e-6))
print('Done\n******')
for t in times:
x0 = xtraj.value(t).transpose()[0]
xd0 = xdotraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
K0 = K.value(t).reshape(self.nU, self.nX)
S0 = S.value(t).reshape(self.nX, self.nX)
S0d = S.derivative(1).value(t).reshape(self.nX, self.nX) # Sdot
# make sure Lyapunov candidate is pd
if (self.isPositiveDefinite(S0)==False):
assert False, '******\nS is not PD for t=%f\n******' %(t)
# for debugging
#S0 = np.eye(self.nX)
#V = x.dot(S0.dot(x))
V = (x-x0).transpose().dot(S0.dot((x-x0)))
# normalization of the lyapunov function
if(do_normalization):
#coeffs = Polynomial(V).monomial_to_coefficient_map().values()
#sumV = 0.
#for coeff in coeffs:
# sumV = sumV + np.abs(coeff.Evaluate())
#sumV = V.Evaluate(dict(zip(x, x0+np.ones(self.nX)))) #do: V(1,1,...)=1
V = V / sumV #normalize coefficient sum to one
# get a polynomial representation of f_closedloop, xdot = f_cl(x)
f_cl = self.EvalClosedLoopDynamics(x, ucon, x0, u0, xd0, K0, order=taylor_order)
#import pdb; pdb.set_trace()
# vdot = x'*Sdot*x + dV/dx*fcl_poly
#Vdot = (x.transpose().dot(S0d)).dot(x) + V.Jacobian(x).dot(f_cl(xbar))
Vdot = ((x-x0).transpose().dot(S0d)).dot((x-x0)) + V.Jacobian(x).dot(f_cl)
#deg_L = np.max([Polynomial(Vdot).TotalDegree(), deg_L])
if(do_balancing):
#import pdb; pdb.set_trace()
S1 = Evaluate(0.5*Jacobian(V.Jacobian(x),x), zero_map)
S2 = Evaluate(0.5*Jacobian(Vdot.Jacobian(x),x), zero_map)
T = self.balanceQuadraticForm(S1, S2)
balanced_x = T.dot(x)
balance_map = dict(zip(x, balanced_x))
V = V.Substitute(balance_map)
Vdot = Vdot.Substitute(balance_map)
for i in range(len(f_cl)):
f_cl[i] = f_cl[i].Substitute(balance_map)
x0 = np.linalg.inv(T).dot(x0) #the new coordinates of the equilibrium point
else:
T = np.eye(self.nX)
# store it for later use
all_V.append(V)
all_fcl.append(f_cl)
all_Vd.append(Vdot)
all_T.append(T)
all_x0.append(x0)
for i in range(len(times)-1):
xd0 = xdotraj.value(times[i]).transpose()[0]
Vdot = (all_V[i+1]-all_V[i])/(times[i+1]-times[i]) + all_V[i].Jacobian(x).dot(all_fcl[i])
all_Vd2.append(Vdot)
#import pdb; pdb.set_trace()
#rho_f = 1.0
# time-varying PolynomialLyapunovFunction who's one-level set defines the verified invariant region
V = self.TimeVaryingLyapunovSearchRho(x, all_V, all_Vd, all_T, times, xtraj, utraj, rho_f, \
multiplier_degree=deg_L)
# Check Hessian of Vdot at origin
#H = Evaluate(0.5*Jacobian(Vdot.Jacobian(x),x), dict(zip(x, x0)))
#assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
return V
import math
from pydrake.all import Jacobian, MathematicalProgram, Solve
def dynamics(x):
return -x + x**3
prog = MathematicalProgram()
x = prog.NewIndeterminates(1, "x")
rho = prog.NewContinuousVariables(1, "rho")[0]
# Define the Lyapunov function.
V = x.dot(x)
Vdot = Jacobian([V], x).dot(dynamics(x))[0]
# Define the Lagrange multiplier.
lambda_ = prog.NewContinuousVariables(1, "lambda")[0]
prog.AddConstraint(lambda_ >= 0)
prog.AddSosConstraint((V - rho) * x.dot(x) - lambda_ * Vdot)
prog.AddLinearCost(-rho)
result = Solve(prog)
assert result.is_success()
print("Verified that " + str(V) + " < " + str(result.GetSolution(rho)) +
" is in the region of attraction.")
# To plot the contour at f = (x-xmin)'H(x-xmin) + fmin = 1, # we make a circle of values y, such that: y'y = 1-fmin, th = np.linspace(0, 2*np.pi, vertices) Y = np.sqrt(1-fmin)*np.vstack([np.sin(th), np.cos(th)]) # then choose L'*(x - xmin) = y, where H = LL'. L = np.linalg.cholesky(H) X = np.tile(xmin, vertices) + np.linalg.inv(np.transpose(L)).dot(Y) return ax.fill(X[0, :]+x0[0], X[1, :]+x0[1], color=color) prog = MathematicalProgram() x = prog.NewIndeterminates(2, 'x') V1 = prog.NewSosPolynomial(Variables(x), 2)[0].ToExpression() V2 = prog.NewSosPolynomial(Variables(x), 2)[0].ToExpression() a = 0.5*Jacobian(V1.Jacobian(x),x) b = 0.5*Jacobian(V2.Jacobian(x),x) pxi = np.array([[-0.5,-0.5], [-0.5, 0.5], [0.5,0.5], [0.5,-0.5]]) for pt in pxi: prog.AddConstraint( pt.T.dot(a).dot(pt) <= 1.0) prog.AddConstraint( pt.T.dot(b).dot(pt) <= 1.0) pxi = np.array([[0.0, 1.0] ]) prog.AddConstraint( pxi[-1].T.dot(b).dot(pxi[-1]) <= 1.0) prog.AddMaximizeLogDeterminantSymmetricMatrixCost(a) prog.AddMaximizeLogDeterminantSymmetricMatrixCost(b) result = Solve(prog) assert(result.is_success()) V1 = result.GetSolution(V1)