def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs.Substitute(dict(zip(prev_x, x))) Vdot = Vsdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] #mapping = dict(zip(x, np.ones(len(x)))) #V_norm = 0.0*V #for i in range(len(x)): # basis = np.ones(len(x)) # V_norm = V_norm + V.Substitute(dict(zip(x, basis))) #V = V/V_norm #Vdot = Vdot/V_norm #prog.AddConstraint(V_norm == 0) # in relative state (lambda(xbar) Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) #import pdb; pdb.set_trace() result = Solve(prog) if(not result.is_success()): print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) ) print('slack = %f' %(result.GetSolution(slack)) ) print('Rho = %f' %(rho)) #assert result.is_success() return -1.0 return result.GetSolution(slack)
def is_verified(rho): # initialize optimization problem # (with Drake there is no need to specify that # this is going to be a SOS program!) prog = MathematicalProgram() # SOS indeterminates x = prog.NewIndeterminates(2, 'x') # Lyapunov function V = x.dot(P).dot(x) V_dot = 2*x.dot(P).dot(f(x)) # degree of the polynomial lambda(x) # no need to change it, but if you really want to, # keep l_deg even (why?) and do not set l_deg greater than 10 # (otherwise optimizations will take forever) l_deg = 4 assert l_deg % 2 == 0 # SOS Lagrange multipliers l = prog.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression() # main condition above eps = 1e-3 # do not change prog.AddSosConstraint(- V_dot - l * (rho - V) - eps*x.dot(x)) # solve SOS program # no objective function in this formulation result = Solve(prog) # return True if feasible, False if infeasible return result.is_success()
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 CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1)[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) assert result.is_success() return result.GetSolution(slack)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): #import pdb; pdb.set_trace() prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() #print('V degree: %d' %(Polynomial(V).TotalDegree())) #print('Vdot degree: %d' %(Polynomial(Vdot).TotalDegree())) #print('SOS degree: %d' %(Polynomial(-Vdot + Lambda*(V - rho) - slack*V).TotalDegree())) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) #assert result.is_success() return result.GetSolution(slack)
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
# SOS indeterminates x = prog2.NewIndeterminates(2, 'x') # Lyapunov function V = x.dot(P).dot(x) V_dot = 2*x.dot(P).dot(f(x)) # degree of the polynomial lambda(x) # no need to change it, but if you really want to, # keep l_deg even and do not set l_deg greater than 10 l_deg = 4 assert l_deg % 2 == 0 # SOS Lagrange multipliers l = prog2.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression() # level set as optimization variable rho = prog2.NewContinuousVariables(1, 'rho')[0] # write here the SOS condition described in the "Not quite there yet..." section above prog2.AddSosConstraint(x.dot(x)*(V-rho) - l*V_dot) # insert here the objective function (maximize rho) prog2.AddLinearCost(-rho) # solve program only if the lines above are filled if len(prog2.GetAllConstraints()) != 0: # solve SOS program result = Solve(prog2)
from pydrake.all import (Jacobian, MathematicalProgram, SolutionResult, Variables) 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 multipliers. (lambda_, constraint) = prog.NewSosPolynomial(Variables(x), 4) prog.AddSosConstraint((V-rho) * x.dot(x) - lambda_.ToExpression() * Vdot) prog.AddLinearCost(-rho) result = prog.Solve() assert(result == SolutionResult.kSolutionFound) print("Verified that " + str(V) + " < " + str(prog.GetSolution(rho)) + " is in the region of attraction.") assert(math.fabs(prog.GetSolution(rho) - 1) < 1e-5)
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], 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)