def findLyapunovFunctionSOS(self, x0, deg_V, deg_L): prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the polynomials z = prog.NewIndeterminates(nZ,'z') x = prog.NewIndeterminates(1, 'x')[0] y = prog.NewIndeterminates(1, 'y')[0] thetadot = prog.NewIndeterminates(1, 'thetadot')[0] X = np.array([s, c, thetadot]) sym_system = self.ToSymbolic() sym_context = sym_system.CreateDefaultContext() sym_context.SetContinuousState(z0+z) sym_context.FixInputPort(0, u0+ucon ) f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose() # Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n. V = prog.NewFreePolynomial(Variables(X), deg_V).ToExpression() eps = 1e-4 constraint1 = prog.AddSosConstraint(V - eps*(X-x0).dot(X-x0)) # constraint to enforce that V is strictly positive away from x0. Vdot = V.Jacobian(X).dot(f) # Construct the polynomial which is the time derivative of V L = prog.NewFreePolynomial(Variables(X), deg_L).ToExpression() # Construct a polynomial L representing the "Lagrange multiplier". constraint2 = prog.AddSosConstraint(-Vdot - L*(x**2+y**2-1) - eps*(X-x0).dot(X-x0)*y**2) # Add a constraint that Vdot is strictly negative away from x0 # Add V(0) = 0 constraint constraint3 = prog.AddLinearConstraint(V.Substitute({y: 0, x: 1, thetadot: 0}) == 0) # Add V(theta=xxx) = 1, just to set the scale. constraint4 = prog.AddLinearConstraint(V.Substitute({y: 1, x: 0, thetadot: 0}) == 1) # Call the solver. result = Solve(prog) Vsol = Polynomial(result.GetSolution(V)) return Vsol
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_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 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_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 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 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(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 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 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 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 TimeVaryingLyapunovSearchRho(self, prev_x, Vs, Vdots, Ts, times, xtraj, utraj, \ rho_f, multiplier_degree=None): C = 1.0 #8.0 #rho_f = 3.0 tries = 40 prev_rhointegral = 0. N = len(times)-1 #Vmin = self.minimumV(prev_x, Vs) #0.05*np.ones((1, len(times))) # need to do something here instead of this!! dt = np.diff(times) #rho = np.flipud(rho_f*np.exp(-C*(np.array(times)-times[0])/(times[-1]-times[0])))# + np.max(Vmin) #rho = np.linspace(0.1, rho_f, N+1) #rho = np.linspace(rho_f/2.0, rho_f, N+1) rho = np.linspace(rho_f*3.0, rho_f, N+1) fig, ax = plt.subplots() fig.suptitle('Rho progression') ax.set(xlabel='index', ylabel='rho') ax.grid(True) plt.show(block = False) need_to_break = False for idx in range(tries): print('starting iteration #%d with rho=' %(idx)) print(rho) ax.plot(rho) plt.pause(0.05) # start of number of iterations if we want rhodot = np.diff(rho)/dt # sampleCheck() Lambda_vec = [] x_vec = [] #import pdb; pdb.set_trace() #fix rho, optimize Lagrange multipliers for i in range(N): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs[i].Substitute(dict(zip(prev_x, x))) Vdot = Vdots[i].Substitute(dict(zip(prev_x, x))) x0 = xtraj.value(times[i]).transpose()[0] Ttrans = np.linalg.inv(Ts[i]) x0 = Ttrans.dot(x0) #xmin, vmin, vdmin = self.SampleCheck(x, V, Vdot) #if(vdmin > rhodot[i]): # print('Vdot is greater than rhodot!') #Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = prog.NewFreePolynomial(Variables(x), multiplier_degree).ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) gamma = prog.NewContinuousVariables(1,'g')[0] # Jdot-rhodot+Lambda*(rho-J) < -gamma prog.AddSosConstraint( -gamma*V - (Vdot-rhodot[i] + Lambda*(rho[i]-V)) ) prog.AddCost(-gamma) #maximize gamma result = Solve(prog) if result.is_success() == False: need_to_break = True print('Solver could not solve anymore') import pdb; pdb.set_trace() break else: Lambda_vec.append(result.GetSolution(Lambda)) slack = result.GetSolution(gamma) print('Slack #%d = %f' %(idx, slack)) x_vec.append(x) if(slack < 0.0): print('In iter#%d, found negative slack so going to end prematurely... :(' %(idx)) need_to_break = True if(need_to_break == True): break; # fix Lagrange multipliers, maximize rho rhointegral = 0.0 prog = MathematicalProgram() xx = prog.NewIndeterminates(len(x),'x') t = prog.NewContinuousVariables(N,'t') #import pdb; pdb.set_trace() #rho = np.concatenate((t,[rho_f])) + Vmin rho_x = np.concatenate((t,[rho[-1]])) #+ rho #import pdb; pdb.set_trace() for i in range(N): #prog.AddConstraint(t[i]>=0.0) # does this mean [prog,rho] = new(prog,N,'pos'); in matlab?? rhod_x = (rho_x[i+1]-rho_x[i])/dt[i] #prog.AddConstraint(rhod_x<=0.0) rhointegral = rhointegral+rho_x[i]*dt[i] + 0.5*rhod_x*(dt[i]**2) V = Vs[i].Substitute(dict(zip(prev_x, xx))) Vdot = Vdots[i].Substitute(dict(zip(prev_x, xx))) #x0 = xtraj.value(times[i]).transpose()[0] L1 = Lambda_vec[i].Substitute(dict(zip(x_vec[i], xx))) #Vdot = Vdot*rho_x[i] - V*rhod_x prog.AddSosConstraint( -(Vdot - rhod_x + L1 * ( rho_x[i]-V ) ) ) prog.AddCost(-rhointegral) result = Solve(prog) assert result.is_success() rhos = result.GetSolution(rho_x) rho = [] for r in rhos: rho.append(r[0].Evaluate()) rhointegral = result.GetSolution(rhointegral).Evaluate() if( (rhointegral-prev_rhointegral)/rhointegral < 1E-5): # 0.1% print('Rho integral converged') need_to_break = True break; else: prev_rhointegral = rhointegral print('End of iteration #%d: rhointegral=%f' %(idx, rhointegral)) if(need_to_break == True): print('In iter#%d, found negative slack so ending prematurely... :(' %(idx)) break; # end of iterations if we want ax.plot(rho) plt.pause(0.05) print('done computing funnel.\nFinal rho= ') print(rho) #import pdb; pdb.set_trace() for i in range(len(rho)): Vs[i] = Vs[i]/rho[i] return Vs
- A line where you add the SOS constraint described in the "Not quite there yet..." subsection above. To do this, use the method `AddSosConstraint` of `MathematicalProgram` (same method we've used in the previous case). - A line where you set the objective function of the SOS program. Remember that we'd like to maximize `rho`. To this end, use the method `AddLinearCost` of `MathematicalProgram`, but notice that writing `prog.AddLinearCost(rho)` the variable `rho` will be *minimized*. Any idea for a quick workaround? Hint: it shouldn't take more than one character! """ # initialize optimization problem prog2 = MathematicalProgram() # 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
def findLyapunovFunctionSOS(self, xtraj, utraj, deg_V, deg_L): times = xtraj.get_segment_times() prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the polynomials x = prog.NewIndeterminates(3, 'x') ucon = prog.NewIndeterminates(2, '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() # - dztrajdt.value(t).transpose() for t in times: x0 = xtraj.value(t).transpose()[0] u0 = utraj.value(t).transpose()[0] f_fb = self.EvalClosedLoopDynamics(x, ucon, f, x0, u0) # Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n. V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression() eps = 1e-4 constraint1 = prog.AddSosConstraint( V - eps * (x - x0).dot(x - x0) ) # constraint to enforce that V is strictly positive away from x0. Vdot = V.Jacobian(x).dot( f_fb ) # Construct the polynomial which is the time derivative of V #L = prog.NewFreePolynomial(Variables(x), deg_L).ToExpression() # Construct a polynomial L representing the # "Lagrange multiplier". # Add a constraint that Vdot is strictly negative away from x0 constraint2 = prog.AddSosConstraint(-Vdot[0] - eps * (x - x0).dot(x - x0)) #import pdb; pdb.set_trace() # Add V(0) = 0 constraint constraint3 = prog.AddLinearConstraint( V.Substitute({ x[0]: 0.0, x[1]: 1.0, x[2]: 0.0 }) == 1.0) # Add V(theta=xxx) = 1, just to set the scale. constraint4 = prog.AddLinearConstraint( V.Substitute({ x[0]: 1.0, x[1]: 0.0, x[2]: 0.0 }) == 1.0) # Call the solver (default). #result = Solve(prog) # Call the solver (specific). #solver = CsdpSolver() #solver = ScsSolver() #solver = IpoptSolver() #result = solver.Solve(prog, None, None) result = Solve(prog) # print out the result. print("Success? ", result.is_success()) print(result.get_solver_id().name()) Vsol = Polynomial(result.GetSolution(V)) print('Time t=%f:\nV=' % (t)) print(Vsol.RemoveTermsWithSmallCoefficients(1e-6)) return Vsol, Vsol.Jacobian(x).dot(f_fb)
def __init__(self, start, goal, degree, n_segments, duration, regions, H): #sample_times, num_vars, continuity_degree): R = len(regions) N = n_segments M = 100 prog = MathematicalProgram() # Set H if H is None: H = prog.NewBinaryVariables(R, N) for n_idx in range(N): prog.AddLinearConstraint(np.sum(H[:, n_idx]) == 1) poly_xs, poly_ys, poly_zs = [], [], [] vars_ = np.zeros((N, )).astype(object) for n_idx in range(N): # for each segment t = prog.NewIndeterminates(1, "t" + str(n_idx)) vars_[n_idx] = t[0] poly_x = prog.NewFreePolynomial(Variables(t), degree, "c_x_" + str(n_idx)) poly_y = prog.NewFreePolynomial(Variables(t), degree, "c_y_" + str(n_idx)) poly_z = prog.NewFreePolynomial(Variables(t), degree, "c_z_" + str(n_idx)) for var_ in poly_x.decision_variables(): prog.AddLinearConstraint(var_ <= M) prog.AddLinearConstraint(var_ >= -M) for var_ in poly_y.decision_variables(): prog.AddLinearConstraint(var_ <= M) prog.AddLinearConstraint(var_ >= -M) for var_ in poly_z.decision_variables(): prog.AddLinearConstraint(var_ <= M) prog.AddLinearConstraint(var_ >= -M) poly_xs.append(poly_x) poly_ys.append(poly_y) poly_zs.append(poly_z) phi = np.array([poly_x, poly_y, poly_z]) for r_idx, region in enumerate(regions): # if r_idx == 0: # break A = region[0] b = region[1] b = b + (1 - H[r_idx, n_idx]) * M b = [Polynomial(this_b) for this_b in b] q = b - A.dot(phi) sigma = [] for q_idx in range(len(q)): sigma_1 = prog.NewFreePolynomial(Variables(t), degree - 1) prog.AddSosConstraint(sigma_1) sigma_2 = prog.NewFreePolynomial(Variables(t), degree - 1) prog.AddSosConstraint(sigma_2) sigma.append( Polynomial(t[0]) * sigma_1 + (1 - Polynomial(t[0])) * sigma_2) # for var_ in sigma[q_idx].decision_variables(): # prog.AddLinearConstraint(var_<=M) # prog.AddLinearConstraint(var_>=-M) q_coeffs = q[q_idx].monomial_to_coefficient_map() sigma_coeffs = sigma[q_idx].monomial_to_coefficient_map() both_coeffs = set(q_coeffs.keys()) & set( sigma_coeffs.keys()) for coeff in both_coeffs: # import pdb; pdb.set_trace() prog.AddConstraint( q_coeffs[coeff] == sigma_coeffs[coeff]) # res = Solve(prog) # print("x: " + str(res.GetSolution(poly_xs[0].ToExpression()))) # print("y: " + str(res.GetSolution(poly_ys[0].ToExpression()))) # print("z: " + str(res.GetSolution(poly_zs[0].ToExpression()))) # import pdb; pdb.set_trace() # for this_q in q: # prog.AddSosConstraint(this_q) # import pdb; pdb.set_trace() # cost = 0 print("Constraint: x0(0)=x0") prog.AddConstraint( poly_xs[0].ToExpression().Substitute(vars_[0], 0.0) == start[0]) prog.AddConstraint( poly_ys[0].ToExpression().Substitute(vars_[0], 0.0) == start[1]) prog.AddConstraint( poly_zs[0].ToExpression().Substitute(vars_[0], 0.0) == start[2]) for idx, poly_x, poly_y, poly_z in zip(range(N), poly_xs, poly_ys, poly_zs): if idx < N - 1: print("Constraint: x" + str(idx) + "(1)=x" + str(idx + 1) + "(0)") next_poly_x, next_poly_y, next_poly_z = poly_xs[ idx + 1], poly_ys[idx + 1], poly_zs[idx + 1] prog.AddConstraint( poly_x.ToExpression().Substitute(vars_[idx], 1.0) == next_poly_x.ToExpression().Substitute(vars_[idx + 1], 0.0)) prog.AddConstraint( poly_y.ToExpression().Substitute(vars_[idx], 1.0) == next_poly_y.ToExpression().Substitute(vars_[idx + 1], 0.0)) prog.AddConstraint( poly_z.ToExpression().Substitute(vars_[idx], 1.0) == next_poly_z.ToExpression().Substitute(vars_[idx + 1], 0.0)) else: print("Constraint: x" + str(idx) + "(1)=xf") prog.AddConstraint(poly_x.ToExpression().Substitute( vars_[idx], 1.0) == goal[0]) prog.AddConstraint(poly_y.ToExpression().Substitute( vars_[idx], 1.0) == goal[1]) prog.AddConstraint(poly_z.ToExpression().Substitute( vars_[idx], 1.0) == goal[2]) # for cost = Expression() for var_, polys in zip(vars_, [poly_xs, poly_ys, poly_zs]): for poly in polys: for _ in range(2): #range(2): poly = poly.Differentiate(var_) poly = poly.ToExpression().Substitute({var_: 1.0}) cost += poly**2 # for dv in poly.decision_variables(): # cost += (Expression(dv))**2 prog.AddCost(cost) res = Solve(prog) print("x: " + str(res.GetSolution(poly_xs[0].ToExpression()))) print("y: " + str(res.GetSolution(poly_ys[0].ToExpression()))) print("z: " + str(res.GetSolution(poly_zs[0].ToExpression()))) self.poly_xs = [ res.GetSolution(poly_x.ToExpression()) for poly_x in poly_xs ] self.poly_ys = [ res.GetSolution(poly_y.ToExpression()) for poly_y in poly_ys ] self.poly_zs = [ res.GetSolution(poly_z.ToExpression()) for poly_z in poly_zs ] self.vars_ = vars_ self.degree = degree
import numpy as np import matplotlib.pyplot as plt from pydrake.all import MathematicalProgram, Solve, Variables from pydrake.symbolic import Polynomial from pydrake.examples.pendulum import PendulumParams prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the # polynomials, but are NOT decision variables in the optimization. We will # add constraints below that must hold FOR ALL x. s = prog.NewIndeterminates(1, 's')[0] c = prog.NewIndeterminates(1, 'c')[0] v = prog.NewIndeterminates(1, 'v')[0] theta = prog.NewIndeterminates(1, 'theta')[0] ua = prog.NewIndeterminates(1, 'ua')[0] uw = prog.NewIndeterminates(1, 'uw')[0] x = np.array([s, c, v, theta]) # Write out the dynamics in terms of sin(theta), cos(theta), and thetadot f = [-s * uw, c * uw, ua, uw] # The fixed-point in this coordinate (because cos(0)=1). x0 = np.array([0, 1, 0]) # Construct a polynomial V that contains all monomials with s,c,thetadot up # to degree 2. deg_V = 2 V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression()
import numpy as np import matplotlib.pyplot as plt from pydrake.all import MathematicalProgram, Solve, Variables from pydrake.symbolic import Polynomial from pydrake.examples.pendulum import PendulumParams prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the # polynomials, but are NOT decision variables in the optimization. We will # add constraints below that must hold FOR ALL x. s = prog.NewIndeterminates(1, "s")[0] c = prog.NewIndeterminates(1, "c")[0] thetadot = prog.NewIndeterminates(1, "thetadot")[0] # TODO(russt): bind the sugar methods so I can write # x = prog.NewIndeterminates(["s", "c", "thetadot"]) x = np.array([s, c, thetadot]) # Write out the dynamics in terms of sin(theta), cos(theta), and thetadot p = PendulumParams() f = [ c * thetadot, -s * thetadot, (-p.damping() * thetadot - p.mass() * p.gravity() * p.length() * s) / (p.mass() * p.length() * p.length()) ] # The fixed-point in this coordinate (because cos(0)=1). x0 = np.array([0, 1, 0]) # Construct a polynomial V that contains all monomials with s,c,thetadot up
from pydrake.all import MathematicalProgram, Solve prog = MathematicalProgram() x = prog.NewIndeterminates(2, "x") f = [-x[0] - 2 * x[1]**2, -x[1] - x[0] * x[1] - 2 * x[1]**3] V = x[0]**2 + 2 * x[1]**2 Vdot = V.Jacobian(x).dot(f) prog.AddSosConstraint(-Vdot) result = Solve(prog) assert result.is_success() print("Successfully verified Lyapunov candidate")
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
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], 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)
#!/usr/bin/env python import numpy as np import matplotlib.pyplot as plt from pydrake.all import MathematicalProgram, Solve, Variables from pydrake.symbolic import Polynomial from pydrake.examples.pendulum import PendulumParams prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the # polynomials, but are NOT decision variables in the optimization. We will # add constraints below that must hold FOR ALL x. s = prog.NewIndeterminates(1, 's')[0] c = prog.NewIndeterminates(1, 'c')[0] thetadot = prog.NewIndeterminates(1, 'thetadot')[0] # TODO(russt): bind the sugar methods so I can write # x = prog.NewIndeterminates(['s','c','thetadot']) x = np.array([s, c, thetadot]) # Write out the dynamics in terms of sin(theta), cos(theta), and thetadot p = PendulumParams() f = [c*thetadot, -s*thetadot, (-p.damping()*thetadot - p.mass()*p.gravity()*p.length()*s) / (p.mass()*p.length()*p.length())] # The fixed-point in this coordinate (because cos(0)=1). x0 = np.array([0, 1, 0]) # Construct a polynomial V that contains all monomials with s,c,thetadot up