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
