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 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 = prog3.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 = prog3.NewFreePolynomial(Variables(x), l_deg).ToExpression() # level set as optimization variable rho = prog3.NewContinuousVariables(1, 'rho')[0] # write here the SOS condition described in the "Not quite there yet..." section above prog3.AddSosConstraint(x.dot(x)*(V-rho) - l*V_dot) # insert here the objective function (maximize rho) prog3.AddLinearCost(-rho) # solve program only if the lines above are filled if len(prog3.GetAllConstraints()) != 0: # solve SOS program result = Solve(prog3)
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 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
# 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 # to degree 2. deg_V = 2 V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression() # Add a constraint to enforce that V is strictly positive away from x0. # (Note that because our coordinate system is sine and cosine, V is also zero # at theta=2pi, etc). eps = 1e-4 constraint1 = prog.AddSosConstraint(V - eps*(x-x0).dot(x-x0)) # Construct the polynomial which is the time derivative of V. Vdot = V.Jacobian(x).dot(f) # Construct a polynomial L representing the "Lagrange multiplier". deg_L = 2 L = prog.NewFreePolynomial(Variables(x), deg_L).ToExpression() # Add a constraint that Vdot is strictly negative away from x0 (but make an
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