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 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(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 FixedLyapunovMaximizeLevelSet(self, prog, x, V, Vdot, multiplier_degree=None): ''' Assumes V>0. Vdot >= 0 => V >= rho (or x=0) via maximize rho subject to (V-rho)*x'*x - Lambda*Vdot is SOS, Lambda is SOS. ''' if multiplier_degree is None: # There are no guarantees... this is a reasonable guess: multiplier_degree = Polynomial(Vdot).TotalDegree() print "Using a degree " + str(multiplier_degree) + " multiplier for the S-procedure" # TODO(russt): implement numerical "balancing" from matlab version. Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() rho = prog.NewContinuousVariables(1, "rho")[0] prog.AddSosConstraint((V-rho)*(x.dot(x)) - Lambda*Vdot) prog.AddCost(-rho) # mosek = MosekSolver() # mosek.set_stream_logging(True, 'mosek.out') # result = mosek.Solve(prog, None, None) result = Solve(prog) print "Using " + result.get_solver_id().name() assert result.is_success() assert result.GetSolution(rho) > 0, "Optimization failed (rho <= 0)." return V/result.GetSolution(rho)
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 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)
# 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)
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
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)
# 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
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)
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