## 2D Robot Arm Inverse Kinematics using Mixed Integer Programming in Cvxpy

Mixed Integer programming is crazy powerful. You can with ingenuity encode many problems into it. The following is a simplification of the ideas appearing in http://groups.csail.mit.edu/robotics-center/public_papers/Dai19.pdf . They do 3d robot arms, I do 2d. I also stick to completely linear approximations.

The surface of a circle is not a convex shape. If you include the interior of a circle it is. You can build a good approximation to the circle as polygons. A polygon is the union of it’s sides, each of which is a line segment. Line sgements are convex set. Unions of convex sets are encodable using mixed integer programming. What I do is sample N regular positions on the surface of a circle. These are the vertices of my polygon. Then I build boolean indicator variables for which segment we are on. Only one of them is be nonzero $\sum s_i == 1$. If we are on a segment, we are allowed to make positions $x$ that interpolate between the endpoints $x_i$ of that segment $x = \lambda_1 x_1 + \lambda_2 x_2$, where $\lambda_i >= 0$ and $\sum \lambda=1$. These $\lambda$ are only allowed to be nonzero if we are on the segment, so we suppress them with the indicator variables $\lambda_i <= s_i + s_{i+1}$. That’s the gist of it.

Given a point on the circle (basically sines and cosines of an angle) we can build a 2d rotation matrix $R$ from it. Then we can write down the equations connecting subsequent links on the arm. $p_{i+1}=p_{i} +Rl$. By using global rotations with respect to the world frame, these equations stay linear. That is a subtle point. $p$ and $R$ are variables, whereas $l$ is a constant describing the geometry of the robot arm. If we instead used rotation matrices connecting frame i to i+1 these R matrices would compound nonlinearly.

All in all, pretty cool!

import cvxpy as cvx
import numpy as np
import matplotlib.pyplot as plt

# builds a N sided polygon approximation of a circle for MIP. It is the union of the segments making up the polygon
# might also be useful to directly encode arcs. for joint constraint limits.
def circle(N):
x = cvx.Variable()
y = cvx.Variable()
l = cvx.Variable(N) #interpolation variables
segment = cvx.Variable(N,boolean=True) #segment indicator variables, relaxing the boolean constraint gives the convex hull of the polygon

angles = np.linspace(0, 2*np.pi, N, endpoint=False)
xs = np.cos(angles) #we're using a VRep
ys = np.sin(angles)

constraints = []
constraints += [x == l*xs, y == l*ys] # x and y are convex sum of the corner points
constraints += [cvx.sum(l) == 1, l <= 1, 0 <= l] #interpolations variables. Between 0 and 1 and sum up to 1
constraints += [cvx.sum(segment) == 1] # only one indicator variable can be nonzero

constraints += [l[N-1] <= segment[N-1] + segment] #special wrap around case
for i in range(N-1):
constraints += [l[i] <= segment[i] + segment[i+1]] # interpolation variables suppressed
return x, y, constraints

x, y, constraints = circle(8)
objective = cvx.Maximize(x-0.8*y)
prob = cvx.Problem(objective, constraints)
res = prob.solve(solver=cvx.GLPK_MI, verbose=True)

# build a 2d rotation matrix using circle
def R(N):
constraints = []
c, s, constraint = circle(N) # get cosines and sines from a circle
constraints += constraint

r = cvx.Variable((2,2)) # build rotation matrix
constraints += [r[0,0] == c, r[0,1] == s]
constraints += [r[1,0] == -s, r[1,1] == c]
return r, constraints
# np.array([[c , s],                [-s, c]])

#robot linkage of differing arm length
pivots = []
Rs = []
N = 8
constraints = []
origin = np.zeros(2)

p1 = origin
R1, c = R(8)
constraints += c

p2 = cvx.Variable(2)
constraints += [p2 == p1 + R1*np.array([l,0])] # R1 is global rotation with respect to world frame. This is important. It is what makes the encoding linear.

p1 = p2

pivots.append(p2)
Rs.append(R1)

end_position = np.array([-0.5, .7])
constraints += [p2 == end_position]

objective = cvx.Maximize(1)
prob = cvx.Problem(objective, constraints)
res = prob.solve(solver=cvx.GLPK_MI, verbose=True)
print(res)

#print(R(x,y))

print(p2.value)
print(list(map(lambda r: r.value, Rs)))
#print(y.value)

p1 = origin
for l, r in zip(link_lengths, Rs):
p2 = p1 + r.value@np.array([l,0])
plt.plot([p1,p2], [p1,p2], marker='o'),

p1 = p2
plt.show()

'''
print(xs)
print(ys)
print(angles)

print(l.value)
print(segment.value)
plt.plot(x.value, label='x')
plt.plot(v.value, label= 'v')
plt.plot(collision.value, label = 'collision bool')
plt.legend()
plt.xlabel('time')
plt.show()
'''

## The Beauty of the Cone: How Convex Cones Simplify Convex Programming

I watched the Stephen Boyd course to get me started in convex programming. At the beginning, he spends some time talking about convex sets rather than launching in convex optimization. I did not appreciate this sufficiently on the first pass. Convex sets are a very geometric topic and I think that for the most part, convex functions are best thought as a special case of them. The epigraph of a scalar valued convex function on $R^d$ , the filled in area above a graph, is a d+1 dimensional convex set. Convex constraints on the domain can be thought of as further cutting this shape. Finding the minimum of the shape can be thought of as a geometrical problem of finding the furthest point in the -y direction.

There is another mathematical topic that I did not appreciate for how powerful and clean it is. If you check out this textbook by Fenchel, he starts with the topic of convex cones rather than sets, I now realize for good reason.

I was sketching out a programmatic representation of convex sets and was annoyed at how ugly things were turning out. First off, infinity is a huge problem. Many convex problems have infinite answers.

The simplest problem is $\max_x c^T x$ with no constraints. The answer plunges off to infinity vaguely in the direction of $c$. The next simplest problem is $\max_x c^T x , a^T x \geq 0$. This either goes off to infinity away from the constraint plane, hits the constraint plane and goes off to infinity, or if c and a are parallel, it is an arbitrary location on the constraint plane.

In short, the very most simple convex problems have infinite answers. You actually need to have a fairly complex problem with many conditions before you can guarantee a finite answer. Once we have a bounded LP, or a positive definite quadratic problem do we start to guarantee boundedness.

In order to work with these problems, it is helpful (necessary?) to compactify your space. There are a couple options here. One is to arbitrarily make a box cutoff. If we limit ourselves to an arbitrary box of length 1e30, then every answer that came back as infinite before is now finite, albeit huge. This makes me queasy though. It is ad hoc, actually kind of annoying to program all the corner cases, and very likely to have numerical issues. Another possibility is to extend your space with rays. Rays are thought of as points at infinity. Now any optimization problem that has an infinite answer returns the ray in the direction the thing goes of to infinity at. It is also annoying to make every function work with either rays or points though.

Another slightly less bothersome aesthetic problem is that the natural representation of half spaces is a normal ray and offset $a^T x \geq b$ The principles of duality make one want to make this object as similar to our representation of points as possible, but it has 1-extra dimension and 1 arbitrary degree of freedom (scalar multiplying a and b by the same positive constant does not change the geometrical half space described). This is ugly.

In the field of projective geometry, there is a very beautiful thing that arises. In projective geometry, all scalar multiples of a ray are considered the same thing. This ray is considered a “point”. The reason this makes sense is that projective geometry is a model of perspective and cameras. Two points are completely equivalent from the perspective of a pinhole camera if they lie on the same ray connecting to the pinhole. This ray continues inside the camera and hits the photographic screen. Hence points on the 2D screen correspond to rays in 3D space. It makes elegant sense to consider the pinhole to be the origin or your space, and hence you come to the previous abstract definition. Points at infinity in 3D (like stars effectively) are not a problem since they lie on finitely describable rays. Points at infinite edge of the 2D screen are not really a problem either. Perfectly reasonable points in 3D can map to the infinite edge of the screen in principle. Someone standing perfectly to the side of the pinhole in 3d space has a ray that goes perfectly horizontally into the camera, and in a sense would only hit a hypothetical infinite screen at infinity.

A great many wonderful (and practical!) things fall out of the projective homogenous coordinates. They are ubiquitous in computer graphics, computer vision, and robotics. The mathematical language describing translations and rotations is unified. Both can be described using a single matrix. This is not the intention, but it is a pleasant surprise. Other geometrical questions become simple questions of linear or vector algebra. It is very cool.

Can we use this method for describing the space we want to find convex sets in? I think not. Unfortunately, the topology of projective space is goofy. At the very least in 2D projective space, which can be thought of as a sphere with opposite points identified, do not necessarily have an inside and outside (I’m questioning this idea now)? So convex sets and talking about maximal half planes and such seems questionable.

But I think we can fix it. Cones are good. In a slight twist on the projective geometry idea, what if you only non negative multiples of rays $\lambda \geq 0$ as the same “point”. You can take as a canonical plane $x_0 =1$ similar to the pinhole camera. This plane can be thought of as your more ordinary affine space. Now half spaces touching the origin (cones) correspond to affine half spaces. We have a reasonable way of describing points at infinity on this plane, which correspond to rays. Arbitrary convex sets on this plane correspond to cones of rays.

Cones in this context are sets closed under arbitrary non-negative sums of points within them. Hence a cone always includes the origin. Cones are basically convex sets of rays.

By adding in an arbtrary-ish degree of freedom to points, we bring points and half spaces much closer in alignment. Now evaluating whether a point in a half space looks like $a^T x \geq 0$ with no ugly extra b.

So in closing, as convex sets are kind of a cleaner version of convex functions, so are convex cones a cleaner version of convex sets. This is actually useful, since when you’re programming, you’ll have to deal with way less corner infinite cases. The theory is also more symmetrical and beautiful

## Cvxpy and NetworkX Flow Problems

Networkx outputs scipy sparse incidence matrices

https://networkx.github.io/documentation/networkx-1.9/reference/generated/networkx.linalg.graphmatrix.incidence_matrix.html#networkx.linalg.graphmatrix.incidence_matrix

https://docs.scipy.org/doc/scipy/reference/sparse.html

Networkx also has it’s own flow solvers, but cvxpy gives you some interesting flexibility, like turning the problem mixed integer, quadratic terms, and other goodies. Plus it is very easy to get going as you’ll see.

So here’s a basic example of putting these two together. Very straightforward and cool.


import networkx as nx
import cvxpy as cvx
import matplotlib.pyplot as plt
import numpy as np
from scipy.sparse import lil_matrix

#graph is an networkx graph from somewhere

#print(edgedict)
nEdges = len(graph.edges)
nNodes = len(graph.nodes)

posflow = cvx.Variable(nEdges)
negflow = cvx.Variable(nEdges)

# split flow into positive and negative parts so we can talk about absolute value.
# Perhaps I should let cvxpy do it for me
constraints = [ 0 <= posflow,  0 <= negflow ]

absflow = posflow + negflow
flow = posflow - negflow

L = nx.incidence_matrix(graph, oriented=True )

source = np.zeros(nNodes) #lil_matrix(n_nodes)
# just some random source placement.
source = 1
source = -1

# cvxpy needs sparse matrices wrapped.
Lcvx = cvx.Constant(L)
#sourcecvx = cvx.Constant(source)

# flow conservation
constraints.append(Lcvx*flow == source)

# can put other funky inequality constraints on things.

objective = cvx.Minimize(cvx.sum(absflow))

print("building problem")
prob = cvx.Problem(objective, constraints)
print("starting solve")
prob.solve(solver=cvx.OSQP, verbose = True) #or try cvx.CBC, cvx.CVXOPT, cvx.GLPK, others
np.set_printoptions(threshold=np.inf)
print(absflow.value)



Here was a cool visual from a multi commodity flow problem (nx.draw_networkx_edges)

Nice, huh.

## A Simple Interior Point Linear Programming Solver in Python

This solver is probably not useful for anything. For almost all purposes, let me point you to cvxpy.

If you want an open source solver CBC/CLP and GLPK and OSQP are good.

If you want proprietary, you can get a variable number constrained trial license to Gurobi for free.

Having said that, here we go.

The simplex method gets more press, and certainly has it’s advantages, but the interior point method makes much more sense to me. What follows is the basic implementation described in Stephen Boyd’s course and book http://web.stanford.edu/~boyd/cvxbook/

In the basic interior point method, you can achieve your inequality constraints $\phi(x) \ge 0$ by using a logarithmic potential to punish getting close to them $-\gamma \ln (\phi(x))$ where $\gamma$ is a parameter we’ll talk about in a bit.  From my perspective, the logarithm is a somewhat arbitrary choice. I believe some properties of the logarithmic potential is necessary for some convergence guarantees.

The basic unconstrained newton step takes a locally quadratic approximation to the function you’re trying to optimize and finds the minimum of that. This basically comes down to taking a step that is the inverse hessian applied to the gradient. $\min_{dx} f(x_0+dx) \approx f(x_0) + \nabla f(x_0)dx + \frac{1}{2} dx^T H dx$ $(H)_{ij} = \partial_{ij}f(x_0)$ $\nabla f(x_0) +H dx = 0 \rightarrow dx =- H^{-1}\nabla f$

We can maintain a linear constraint on the variable x during this newton step. Instead of setting the gradient to zero, we set it so that it is perpendicular to the constraint plane using the Lagrange multiplier procedure. $\nabla f(x_0) +H dx = -A^T \lambda \rightarrow Hdx + A^T \lambda = - \nabla f$ $A(x_0 + dx) = b$

This is a block linear system $\begin{bmatrix} H & A^T \\ A & 0 \\ \end{bmatrix} \begin{bmatrix} dx \\ \lambda \end{bmatrix} = \begin{bmatrix} -\nabla f \\ b - Ax_0 \end{bmatrix}$

Despite the logarithm potential, there is no guarantee that the newton step would not take us outside the allowed region. This is why we need a line search on top of the newton step. We scale the newton dx to $\alpha dx$. Because the function we’re optimizing is convex and the region we’re in is convex, there is some step length in that newton direction that will work. So if we keep decreasing the overall step size, we’ll eventually find something acceptable.

As part of the interior point method, once it has converged we decrease the parameter $\gamma$ applied to the logarithm potential. This will allow the inequality constraints to satisfied tighter and tighter with smaller gamma.

The standard form of an LP is $\min c^T x$ $A x = b$ $x \ge 0$

This doesn’t feel like the form you’d want. One way you can construct this is by adding slack variables and splitting regular variables into a positive and negative piece $x = x_+ - x_-$ $Ax \ge b \rightarrow Ax +s = b, s \ge 0$

The interior point formulation of this is $\min c^T x- \gamma \sum_i \ln(x_i)$ $Ax = b$

The Hessian and gradient are quite simple here $\nabla f = -\frac{\gamma}{x_i}$ $(H)_{ij} = \delta_{ij} \frac{\gamma}{x_i^2}$

The optimum conditions for this are $\nabla (c^T x - \gamma \ln(x))= c - \gamma \frac{1}{x} = 0$ $Ax=b$

Now in the above, I’m not sure I got all the signs right, but I did implement it in python. The result seems to be correct and does work. I haven’t tested extensively, YMMV. It’s a useful starting point.

#simple lp

import numpy as np
import scipy.sparse as sparse
import scipy.sparse.linalg as linalg
# min cx
# x >= 0
# Ax = b

def newtonDec(df, dx):
return np.dot(df,dx)

# assumes that x + alpha*dx can be made positive
def linesearch(x, dx):
alpha = 1.
while not np.all( x + alpha*dx > 0):
alpha *= 0.1
return alpha

# min cx

def solve_lp2(A, b, c, gamma, xstart=None):
#x = np.ones(A.shape)
#lam = np.zeros(b.shape)
xsize = A.shape
if xstart is not None:
x = xstart
else:
#xlam = np.ones(xsize + b.size)
x = np.ones(xsize) # xlam[:xsize]
#lam = xlam[xsize:]
while True :
print("Iterate")
H = sparse.bmat( [[ sparse.diags(gamma / x**2)   ,   A.T ],
[ A  ,                         0 ]]  )

dfdx = c - gamma / x #+  A.T@lam
dfdlam = A@x - b
df = np.concatenate((dfdx, dfdlam))#np.zeros(b.size))) # dfdlam))
#np.concatenate( , A@x - b)
dxlam = linalg.spsolve(H,df)
dx = - dxlam[:xsize]
lam = dxlam[xsize:]

alpha = linesearch(x,dx)
x += alpha * dx
#lam += dlam
if newtonDec(dfdx,dx) >= -1e-10:
print("stop")
break

return x, lam

def solve_lp(A,b,c, xstart=None):
gamma = 1.0
xsize = A.shape
x = np.ones(xsize)
for i in range(8):
x, lam = solve_lp2(A, b, c, gamma, xstart=x)
gamma *= 0.1
return x, lam

N = 12
A = np.ones(N).reshape(1,-1)
b = np.ones(1)*2
c = np.zeros(N)
c = -1

#print(solve_lp(A,b,c, 0.000001))
print(solve_lp(A,b,c))

def BB(A, b, c, best, xhint = None):
picked = np.zeros(xsize)
picked[pickvar] = 1
Anew = sparse.hstack((A, picked))
bnew = np.concatenate((b,choice))
x, lam =
np.dot(c,x)
if lp_solve(Anew, bnew, c) < best:
best, x = BB(Anew, bnew , c, best, xhint)
return best, x

'''
#min  cx + gamma * ln(x)
# s.t. Ax = b

# cx + gamma * ln(x) + lambda (Ax - b)

delx = c + gamma * 1/x + lambda A
dellam = Ax - b
# hess
dlx = A
dxl = A.T
dxx = - gamma (1/x**2)

H @ (x l) = (delx dell)
'''


Musings:

I wanted to build this because I’ve been getting really into mixed integer programming and have been wondering how much getting deep in the guts of the solver might help. Given my domain knowledge of the problems at hand, I have probably quite good heuristics. In addition, I’ve been curious about a paper that has pointed out an interesting relatively unexploited territory, combining machine learning with mixed integer programming https://arxiv.org/pdf/1811.06128

For these purposes, I want a really simple optimization solver.

But this is silly. I should use CLP or OSQP as a black box if I really want to worry about the mixed integer aspect.

MIOSQP is interesting.

It is interesting how the different domains of discrete optimization and search seem to have relatively similar sets of methods. Maybe I’m crazy. Maybe at the loose level I’m gonna talk almost anything is like almost anything else.

Clause learning and Cutting plane addition feel rather similar.

Relaxation to LP and unit propagation are somewhat similar. Or is unit propagation like elimination?

Mixed integer programs build their own heuristics.

Fourier Motzkin and resolution are similar methods. In Fourier motzkin, you eliminate variables in linear inequalities by using algebra to bring that variable by itself on one side of the inequality and then matching up all the <= to all the uses of >= of that variable. There are packages that compute these things. See CDD or Polyhedra.jl

Resolution takes boolean formula. You can eliminate a variable q from a CNF formula by taking all the negated instances $\not q$ and combining them with all positive instances.

## Q-Learning with Linear Programming (cvxpy, OpenAI Gym Pendulum)

http://web.mit.edu/~pucci/www/discountedLP.pdf

http://underactuated.mit.edu/underactuated.html?chapter=dp

There is a fun idea of using Linear Programming to do dynamic programming I originally saw in the underactuated robotics textbook.

In my experience reinforcement learning is finicky and depressing. It usually doesn’t work and is very hard to troubleshoot. Do you just need to run it for 10 minutes? 10 years? Is there a bug? God knows. I end up wriggling hyperparameters and praying a lot.

One part of this is the relative finickiness of neural network optimization compared to the technology of convex optimization. Convex optimization solvers are quite reliable and fast.

There is a way of phrasing Q learning as a linear programming problem

The linear programming approach relaxes the Bellman equations. $Q(s_t,a_t)=r_t + \gamma \max_a Q(s_{t+1},a)$

to $\forall a. Q(s_t,a_t) \ge r_t +\gamma Q(s_{t+1},a)$

We can approach this forall in a couple ways, one of which is just sampling actions somehow. To make the constraint tight in places you minimize a weighting of Q $\min \sum w_i * Q(s_i,a_i)$

If Q is written as a linear combination of basis functions $Q(s,a)=\sum \alpha_i f_i(s,a)$

The all of this put together is a linear program in the variables $\alpha_i$.

For ease, I used cvxpy. I don’t even store my state action pairs, which is quite lazy of me. Even here, compiling the linear program via cvxpy is kind of slow. This preprocessing step takes longer than the actual solve does. You could avoid cvxpy and directly interface a linear programming solver much faster, if that is your thing.

The whole process is still model free. I didn’t plug in pendulum dynamics anywhere. I run openAI gym and use the resulting state-action-state tuples to add inequalities to my cvxpy model. I weight where I want the inequalities to be tightest by using the actual states experienced.

Unfortunately, it still took a couple hours of hyper parameter tuning and fiddling to get the thing to work. So not a grand success on that point.

I made a lot of guesswork for what seemed reasonable

I parametrized the dependence of Q on a by a quadratic so that it is easy to maximize analytically. That is what the polyfit stuff is about. Maximum of $ax^2+bx+c$ is at $-b/2a$. I really should be checking the sign of the a coefficient. I am just assuming it is positive. Naughty boy.

m assuming that it

Chebyshev polynomials are probably good.

It seemed to help to use a slight perturbation of the actual action used on the right hand side of the Bellman inequality. My reasoning here is that the pendulum is actually a continuous system, so we should be using the differential Bellman equation really.

Should I allow for some kind of slack in the equations? Getting a bad reward or data point or one weird unusual state could ruin things for everyone. Inequalities are unforgiving.

Gamma seemed to matter a decent amount

The regularization of alpha seemed largely irrelevant.

Epsilon greediness seems to not matter much either.

Future ideas:

Might be good to replace the sampling of a with a Sum of Squares condition over the variable a.

Should I damp the update in some way? Add a cost the changing alpha from it’s previous value. A kind of damped update / using a prior.

import gym
import numpy as np
import cvxpy as cvx
from numpy.polynomial.chebyshev import chebval
env = gym.make('Pendulum-v0')
print(env.action_space)

print(env.observation_space)
print(env.observation_space.high)

print(env.observation_space.low) # [1,1,8]
print(env.action_space.high) # -2
print(env.action_space.low)

chebdeg = 4
alpha = cvx.Variable(3*chebdeg**3)
gamma = 1. - 1./100
def basis(s,a):
n = np.arange(4)
f1 = chebval(s/1.,np.eye(chebdeg)).reshape(-1,1,1,1)
f2 = chebval(s/1.,np.eye(chebdeg)).reshape(1,-1,1,1)
f3 = chebval(s/8.,np.eye(chebdeg)).reshape(1,1,-1,1)
f4 = ((a/2)**np.arange(3)).reshape(1,1,1,-1)
return f1*f2*f3*f4

def evalb(alpha, s,a):
f = basis(s,a)
return alpha*f.flatten()

def maxaction(alpha, obs):
f1 = np.sum(evalb(alpha.value, observation, 2))
f2 = np.sum(evalb(alpha.value, observation, 0))
f3 = np.sum(evalb(alpha.value, observation, -2))
coeff = np.polyfit([2,0,-2], [f1,f2,f3], deg=2)
action = -coeff/2/coeff
action = min(max(action,-2),2)
return np.array([action])

constraints = []
objective = 0

for x in range(4):

constraints = []
objective = 0
epsilon = 1.2/(x+1)
print("epsilon: ", epsilon)
for i_episode in range(50):
observation = env.reset()
reward = -100
for t in range(100):

prev_obs = observation
prev_reward = reward
if np.random.rand() < epsilon:
action = env.action_space.sample()
else:
action = maxaction(alpha, observation)
observation, reward, done, info = env.step(action)
constraints.append(evalb(alpha, prev_obs, action) >= prev_reward + gamma*evalb(alpha, observation, action + np.random.randn()*0.2))#maxaction(alpha, observation)))
objective += evalb(alpha, observation, env.action_space.sample())
if done:
print("Episode finished after {} timesteps".format(t+1))
break
loss = 0.1*cvx.sum(cvx.abs(alpha)) + objective
prob = cvx.Problem(cvx.Minimize(loss), constraints)
# The optimal objective value is returned by prob.solve().
print("solving problem")
result = prob.solve()
print(result)
# The optimal value for x is stored in x.value.
print(alpha.value)
#inspection loop
for i_episode in range(4):
observation = env.reset()
#env.env.state = np.random.randn()*sigma
for t in range(200):
env.render()
#print(observation)
prev_obs = observation
action = maxaction(alpha, observation)
#print(action)

observation, reward, done, info = env.step(action)
if done:
print("Episode finished after {} timesteps".format(t+1))
break


Edit:

A improved version. Fixed the bug in my maxaction function. I shouldn’t have been assuming that it was always concave down.

Also vectorized slightly. Fairly significantly improves the solve time. Not much time is spent in cvxpy, now the solve is dominated by about 3 legitimate seconds in OSQP.

You can flip stuff in and out of loops to try different versions. This method is off-policy, so I could keep data around forever. However, it mostly just slowed the solve time.

import gym
import numpy as np
import cvxpy as cvx
from numpy.polynomial.chebyshev import chebval
env = gym.make('Pendulum-v0')
print(env.action_space)

print(env.observation_space)
print(env.observation_space.high)

print(env.observation_space.low) # [1,1,8]
print(env.action_space.high) # -2
print(env.action_space.low)

chebdeg = 3
alpha = cvx.Variable(3*chebdeg**3)
alpha.value = np.zeros(3*chebdeg**3)
gamma = 1. - 1./100
def basis(s,a):
n = np.arange(4)
f1 = chebval(s/1.,np.eye(chebdeg)).reshape(-1,1,1,1)
f2 = chebval(s/1.,np.eye(chebdeg)).reshape(1,-1,1,1)
f3 = chebval(s/8.,np.eye(chebdeg)).reshape(1,1,-1,1)
f4 = ((a/2)**np.arange(3)).reshape(1,1,1,-1)
return f1*f2*f3*f4

def evalb(alpha, s,a):
f = basis(s,a)
return alpha*f.flatten()

def maxaction(alpha, obs):
f1 = np.sum(evalb(alpha.value, observation, 2))
f2 = np.sum(evalb(alpha.value, observation, 0))
f3 = np.sum(evalb(alpha.value, observation, -2))
coeff = np.polyfit([2,0,-2], [f1,f2,f3], deg=2)
if coeff < 0:
action = -coeff/2/coeff
action = min(max(action,-2),2)
elif f1 > f3:
action = 2
else:
action = -2
return np.array([action])

constraints = []
objective = 0

for x in range(4):
constraints = []
objective = 0

epsilon = 1.2/(x+1)
print("epsilon: ", epsilon)
for i_episode in range(50):
observation = env.reset()
observations = []
rewards = []
actions = []
reward = -100
for t in range(100):

prev_obs = observation
prev_reward = reward
if np.random.rand() < epsilon:
action = env.action_space.sample()
else:
action = maxaction(alpha, observation)
observation, reward, done, info = env.step(action)
observations.append(observation)
rewards.append(reward)
actions.append(action)

#constraints.append(evalb(alpha, prev_obs, action) >= prev_reward + gamma*evalb(alpha, observation, action + np.random.randn()*0.2))#maxaction(alpha, observation)))
#objective += evalb(alpha, observation, env.action_space.sample())
if done:
print("Episode finished after {} timesteps".format(t+1))
break
bprev = np.array([basis(s,a).flatten() for (s,a) in zip(observations[:-1],actions[1:])])
bnext = np.array([basis(s,a+np.random.randn()*0.2).flatten() for (s,a) in zip(observations[1:],actions[1:])])
rewards = np.array(rewards)[:-1]
constraints += [(bprev@alpha) >= rewards + gamma*(bnext)@alpha]

bcost = np.array([basis(s,env.action_space.sample()).flatten() for s in observations])
objective += cvx.sum(bcost@alpha) # np.sum(bnext,axis=0) * alpha
loss = 0.1*cvx.sum(cvx.abs(alpha-alpha.value)) + objective
prob = cvx.Problem(cvx.Minimize(loss), constraints)

print("solving problem")
result = prob.solve(verbose=True)
print(result)
print(alpha.value)
#inspection loop
for i_episode in range(4):
observation = env.reset()
#env.env.state = np.random.randn()*sigma
for t in range(200):
env.render()
#print(observation)
prev_obs = observation
action = maxaction(alpha, observation)
#print(action)

observation, reward, done, info = env.step(action)
if done:
print("Episode finished after {} timesteps".format(t+1))
break


## Deriving the Chebyshev Polynomials using Sum of Squares optimization with Sympy and Cvxpy

Least squares fitting $\sum (f(x_i)-y_i)^2$ is very commonly used and well loved. Sum of squared fitting can be solved using just linear algebra. One of the most convincing use cases to me of linear programming is doing sum of absolute value fits $\sum |f(x_i)-y_i|$  and maximum deviation fits $\max_i |f(x_i)-y_i|$. These two quality of fits are basically just as tractable as least squares, which is pretty cool.

The trick to turning an absolute value into an LP is to look at the region above the graph of absolute value. This region is defined by $y \ge x$ and $y \ge -x$. So you introduce a new variable y. Then the LP $\min y$ subject to those constraints will minimize the absolute value. For a sum of absolute values, introduce a variable $y_i$ for each absolute value you have. Then minimize $\sum_i y_i$. If you want to do min max optimization, use the same y value for every absolute value function. $\min y$ $\forall i. -y \le x_i \le y$

Let’s change topic a bit. Chebyshev polynomials are awesome. They are basically the polynomials you want to use in numerics.

Chebyshev polynomials are sines and cosines in disguise. They inherit tons of properties from them. One very important property is the equioscillation property. The Chebyshev polynomials are the polynomials that stay closest to zero while keeping the x^n coefficient nonzero (2^(n-2) by convention). They oscillate perfectly between -1 and 1 on the interval $x \in [-1,1]$ just like sort of a stretched out sine. It turns out this equioscillation property defines the Chebyshev polynomials

We can approximate the Chebyshev polynomials via sampling many points between [-1,1]. Then we do min of the max absolute error optimization using linear programming. What we get out does approximate the Chebyshev polynomials.

import cvxpy as cvx
import numpy as np
import matplotlib.pyplot as plt

# try finding the 3 through 5 chebyshev polynomial
for N in range(3,6):
a = cvx.Variable(N) #polynomial coefficients
t = cvx.Variable()
n = np.arange(N) #exponents

xs = np.linspace(-1,1,100)
chebcoeff = np.zeros(N)
chebcoeff[-1] = 1
plt.plot(xs, np.polynomial.chebyshev.chebval(xs, chebcoeff), color='r')

constraints = [a[-1]==2**(N-2)] # have to have highest power
for k in range(100):
x = np.random.rand()*2-1 #pick random x in [-1,1]
c = cvx.sum(a * x**n) #evaluate polynomial

constraints.append(c <= t)
constraints.append(-t <= c)

obj = cvx.Minimize(t) #minimize maximum aboslute value
prob = cvx.Problem(obj,constraints)
prob.solve()
plt.plot(xs, np.polynomial.polynomial.polyval(xs, a.value), color='g')
print(a.value)

plt.show() Found Coefficients:
[-9.95353054e-01  1.33115281e-03  1.99999613e+00]
[-0.01601964 -2.83172979  0.05364805  4.00000197]
[ 0.86388003 -0.33517716 -7.4286604   0.6983382   8.00000211]

Red is the actual Chebyshev polynomials and green is the solved for polynomials. It does a decent job. More samples will do even better, and if we picked the Chebyshev points it would be perfect.

Can we do better? Yes we can. Let’s go on a little optimization journey.

Semidefinite programming allows you to optimize matrix variables with the constraint that they have all positive eigenvalues. In a way it lets you add an infinite number of linear constraints. Another way of stating the eigenvalue constraint is that $\forall q. q^T X q \ge 0$

You could sample a finite number of random q vectors and take the conjunction of all these constraints. Once you had enough, this is probably a pretty good approximation of the Semidefinite constraint. But semidefinite programming let’s you have an infinite number of the constraints in the sense that $\forall q$ is referencing an infinite number of possible q , which is pretty remarkable.

Finite Sampling the qs has similarity to the previously discussed sampling method for absolute value minimization.

Sum of Squares optimization allows you to pick optimal polynomials with the constraint that they can be written as a sum of squares polynomials. In this form, the polynomials are manifestly positive everywhere. Sum of Squares programming is a perspective to take on Semidefinite programming. They are equivalent in power. You solve SOS programs under the hood by transforming them into semidefinite ones.

You can write a polynomial as a vector of coefficients $\tilde{a}$. $\tilde{x} = \begin{bmatrix} 1 \\ x \\ x^2 \\ x^3 \\ \vdots \end{bmatrix}$ $\tilde{a} = \begin{bmatrix} a_0 \\ a_1 \\ a_2 \\ a_3 \\ \vdots \end{bmatrix}$ $p(x)=\tilde{a}^T \tilde{x}$

Instead we represent the polynomial with the matrix $Q$ $p(x) = \tilde{x}^T Q \tilde{x}$

If the matrix is positive semidefinite, then it can be diagonalized into the sum of squares form.

In all honesty, this all sounds quite esoteric, and it kind of is. I struggle to find problems to solve with this stuff. But here we are! We’ve got one! We’re gonna find the Chebyshev polynomials exactly by translating the previous method to SOS.

The formulation is a direct transcription of the above tricks. $\min t$ $-t \le p(x) \le t$  by which I mean $p(x) + t$ is SOS and $t - p(x)$ is SOS.

There are a couple packages available for python already that already do SOS, .

SumofSquares.jl for Julia and SOSTools for Matlab. YalMip too I think. Instead of using those packages, I want to roll my own, like a doofus.

Sympy already has very useful polynomial manipulation functionality. What we’re going to do is form up the appropriate expressions by collecting powers of x, and then turn them into cvxpy expressions term by term. The transcription from sympy to cvxpy isn’t so bad, especially with a couple helper functions.

One annoying extra thing we have to do is known as the S-procedure. We don’t care about regions outside of $x \in [-1,1]$. We can specify this with a polynomial inequality $(x+1)(x-1) \ge 0$. If we multiply this polynomial by any manifestly positive polynomial (a SOS polynomial in particular will work), it will remain positive in the region we care about. We can then add this function into all of our SOS inequalities to make them easier to satisfy. This is very similar to a Lagrange multiplier procedure.

Now all of this seems reasonable. But it is not clear to me that we have the truly best polynomial in hand with this s-procedure business. But it seems to works out.

from sympy import *
import cvxpy as cvx
import matplotlib.pyplot as plt
import numpy as np

#build corresponding cvx variable for sympy variable
def cvxvar(expr, PSD=True):
if expr.func == MatrixSymbol:
i = int(expr.shape.evalf())
j = int(expr.shape.evalf())
return cvx.Variable((i,j), PSD=PSD)
elif expr.func == Symbol:
return cvx.Variable()

def cvxify(expr, cvxdict): # replaces sympy variables with cvx variables
return lambdify(tuple(cvxdict.keys()), expr)(*cvxdict.values())

xs = np.linspace(-1,1,100)

for N in range(3,6):
#plot actual chebyshev
chebcoeff = np.zeros(N)
chebcoeff[-1] = 1
plt.plot(xs, np.polynomial.chebyshev.chebval(xs, chebcoeff), color='r')

cvxdict = {}
# Going to need symbolic polynomials in x
x = Symbol('x')
xn = Matrix([x**n for n in range(N)]);

def sospoly(varname):
Q = MatrixSymbol(varname, N,N)
p = (xn.T * Matrix(Q) * xn)
return p, Q

t = Symbol('t')
cvxdict[t] = cvxvar(t)

#lagrange multiplier polynomial 1
pl1, l1 = sospoly('l1')
cvxdict[l1] = cvxvar(l1)

#lagrange multiplier polynomial 2
pl2, l2 = sospoly('l2')
cvxdict[l2] = cvxvar(l2)

#Equation SOS Polynomial 1
peq1, eq1 = sospoly('eq1')
cvxdict[eq1] = cvxvar(eq1)

#Equation SOS Polynomial 2
peq2, eq2 = sospoly('eq2')
cvxdict[eq2] = cvxvar(eq2)

a = MatrixSymbol("a", N,1)
pa = Matrix(a).T*xn #sum([polcoeff[k] * x**k for k in range(n)]);
pa = pa
cvxdict[a] = cvxvar(a, PSD=False)

constraints = []

# Rough Derivation for upper constraint
# pol <= t
# 0 <= t - pol + lam * (x+1)(x-1)  # relax constraint with lambda
# eq1 = t - pol + lam
# 0 = t - pol + lam - eq1
z1 = t - pa + pl1 * (x+1)*(x-1) - peq1
z1 = Poly(z1, x).all_coeffs()
constraints += [cvxify(expr, cvxdict) == 0 for expr in z1]

# Derivation for lower constraint
# -t <= pol
# 0 <= pol + t + lam * (x+1)(x-1) # relax constraint with lambda
# eq2 = pol + t + lam     # eq2 is SOS
# 0 = t - pol + lam - eq2     #Rearrange to equal zero.
z2 = pa + t + pl2 * (x+1)*(x-1) - peq2
z2 = Poly(z2, x).all_coeffs()
constraints += [cvxify(expr, cvxdict) == 0 for expr in z2]

constraints += [cvxdict[a][N-1,0] == 2**(N-2) ]
obj = cvx.Minimize(cvxdict[t]) #minimize maximum absolute value
prob = cvx.Problem(obj,constraints)
prob.solve()

print(cvxdict[a].value.flatten())
plt.plot(xs, np.polynomial.polynomial.polyval(xs, cvxdict[a].value.flatten()), color='g')

plt.show()


Coefficients:
[-1.00000000e+00 -1.02219773e-15  2.00000001e+00]
[-1.23103133e-13 -2.99999967e+00  1.97810058e-13  4.00001268e+00]
[ 1.00000088e+00 -1.39748880e-15 -7.99999704e+00 -3.96420452e-15
7.99999691e+00] Ooooooh yeah. Those curves are so similar you can’t even see the difference. NICE. JUICY.

There are a couple interesting extension to this. We could find global under or over approximating polynomials. This might be nice for a verified compression of a big polynomial to smaller, simpler polynomials for example. We could also similar form the pointwise best approximation of any arbitrary polynomial f(x) rather than the constant 0 like we did above (replace $-t \le p(x) \le t$ for $-t \le p(x) - f(x) \le t$ in the above). Or perhaps we could use it to find a best polynomial fit for some differential equation according to a pointwise error.

I think we could also extend this method to minimizing the mean absolute value integral just like we did in the sampling case. $\min \int_0^1 t(x)dx$ $-t(x) \le p(x) \le t(x)$

More references on Sum of Squares optimization:

http://www.mit.edu/~parrilo/

## Solving the Ising Model using a Mixed Integer Linear Program Solver (Gurobi)

I came across an interesting thing, that finding the minimizer of the Ising model is encodable as a mixed integer linear program.

The Ising model is a simple model of a magnet. A lattice of spins that can either be up or down. They want to align with an external magnetic field, but also with their neighbors (or anti align, depending on the sign of the interaction). At low temperatures they can spontaneously align into a permanent magnet. At high temperatures, they are randomized. It is a really great model that contains the essence of many physics topics.

Linear Programs minimize linear functions subject to linear equality and inequality constraints. It just so happens this is a very solvable problem (polynomial time).

MILP also allow you to add the constraint that variables take on integer values. This takes you into NP territory. Through fiendish tricks, you can encode very difficult problems. MILP solvers use LP solvers as subroutines, giving them clues where to search, letting them step early if the LP solver returns integer solutions, or for bounding branches of the search tree.

How this all works is very interesting (and very, very roughly explained), but barely matters practically since other people have made fiendishly impressive implementations of this that I can’t compete with. So far as I can tell, Gurobi is one of the best available implementations (Hans Mittelman has some VERY useful benchmarks here http://plato.asu.edu/bench.html), and they have a gimped trial license available (2000 variable limit. Bummer.). Shout out to CLP and CBC, the Coin-Or Open Source versions of this that still work pretty damn well.

Interesting Connection: Quantum Annealing (like the D-Wave machine) is largely based around mapping discrete optimization problems to an Ising model. We are traveling that road in the opposite direction.

So how do we encode the Ising model?

Each spin is a binary variable $s_i \in {0,1}$

We also introduce a variable for every edge. which we will constrain to actually be the product of the spins. $e_{ij} \in {0,1}$. This is the big trick.

We can compute the And/Multiplication (they coincide for 0/1 binary variables) of the spins using a couple linear constraints. I think this does work for the 4 cases of the two spins. $e_{ij} \ge s_i +s_j - 1$ $e_{ij} \le s_j$ $e_{ij} \le s_i$

The xor is usually what we care about for the Ising model, we want aligned vs unaligned spins to have different energy. It will have value 1 if they are aligned and 0 if they are anti-aligned. This is a linear function of the spins and the And. $s_i \oplus s_j = s_i + s_j - 2 e_{ij}$

Then the standard Hamiltonian is $H=\sum B_i s_i + \sum J_{ij} (s_i + s_j - 2 e_{ij})$

Well, modulo some constant offset. You may prefer making spins $\pm 1$, but that leads to basically the same Hamiltonian.

The Gurobi python package actually let’s us directly ask for AND constraints, which means I don’t actually have to code much of this.

We are allowed to use spatially varying external field B and coupling parameter J. The Hamiltonian is indeed linear in the variables as promised.

After already figuring this out, I found this chapter where they basically do what I’ve done here (and more probably). There is nothing new under the sun. The spatially varying fields B and J are very natural in the field of spin glasses.

https://onlinelibrary.wiley.com/doi/10.1002/3527603794.ch4

For a while I thought this is all we could do, find the lowest energy solution, but there’s more! Gurobi is one of the few solvers that support iteration over the lowest optimal solutions, which means we can start to talk about a low energy expansion. https://www.gurobi.com/documentation/8.0/refman/poolsearchmode.html#parameter:PoolSearchMode

Here we’ve got the basic functionality. Getting 10,000 takes about a minute. This is somewhat discouraging when I can see that we haven’t even got to very interesting ones yet, just single spin and double spin excitations. But I’ve got some ideas on how to fix that. Next time baby-cakes.

(A hint: recursion with memoization leads to some brother of a cluster expansion.)

from gurobipy import *
import matplotlib.pyplot as plt
import numpy as np

# Create a new model
m = Model("mip1")

m.Params.PoolSearchMode = 2
m.Params.PoolSolutions = 10000

# Create variables
N = 10

xor = {}
B = np.ones((N,N)) #np.random.randn(N,N)
J = 1. #antialigned
H = 0.
for i in range(N-1):
for j in range(N-1):
#for d in range(2)
xor[i,j,0] = spins[i,j] + spins[i+1,j] - 2*links[i,j,0]
xor[i,j,1] = spins[i,j] + spins[i,j+1] - 2*links[i,j,1]
H += J*xor[i,j,0] + J*xor[i,j,1]

for i in range(N):
for j in range(N):
H += B[i,j]*spins[i,j]

#B[i,j] = 1.
#quicksum([2*x, 3*y+1, 4*z*z])

#LinExpr([1.]*N*N,spins)

# Set objective
m.setObjective(H, GRB.MINIMIZE)

# Add constraint: x + 2 y + 3 z <= 4
#m.addConstr(x + 2 * y + 3 * z <= 4, "c0")

# Add constraint: x + y >= 1
#m.addConstr(x + y >= 1, "c1")

m.optimize()

#for v in m.getVars():
#    print(v.varName, v.x)

print('Obj:', m.objVal)
print('Solcount:', m.SolCount)
for i in range(m.SolCount):
m.Params.SolutionNumber = i #set solution numbers
print("sol val:", m.Xn)
print("sol energy:", m.PoolObjVal)
print(spins[0,0].Xn)

ising = np.zeros((N,N))
for i in range(N):
for j in range(N):
ising[i,j] = spins[i,j].Xn
plt.matshow(ising)

plt.show()

Here’s the ground antiferromagnetic state. Cute. ## Extracting a Banded Hessian in PyTorch

So pytorch does have some capability towards higher derivatives, with the caveat that you have to dot the gradients to turn them back into scalars before continuing. What this means is that you can sample a single application of the  Hessian (the matrix of second derivatives) at a time.

One could sample out every column of the hessian for example. Performance-wise I don’t know how bad this might be.

For a banded hessian, which will occur in a trajectory optimization problem (the bandedness being a reflection of the finite difference scheme), you don’t need that many samples. This feels more palatable. You only need to sample the hessian roughly the bandwidth number of times, which may be quite small. Plus, then you can invert that banded hessian very quickly using special purpose banded matrix solvers, which are also quite fast. I’m hoping that once I plug this into the trajectory optimization, I can use a Newton method (or SQP?) which will perform better than straight gradient descent.

If you pulled just a single column using [1,0,0,0,0,0..] for example, that would be wasteful, since there are so many known zeros in the banded matrix. Instead something like [1,0,0,1,0,0,1,0,0..] will not have any zeros in the result. This gets us every 3rd row of the matrix. Then we can sample with shifted versions like [0,1,0,0,1,0,0,1,0,0..]. until we have all the rows somewhere. Then there is some index shuffling to put the thing into a sane ordering, especially so that we can use https://docs.scipy.org/doc/scipy/reference/generated/scipy.linalg.solveh_banded.html which requires the banded matrix to be given in a particular form.

An alternative approach might be to use an fft with some phase twiddling. Also it feels like since the Hessian is hermitian we ought to be able to use about half the samples, since half are redundant, but I haven’t figured out a clean way to do this yet. I think that perhaps sampling with random vectors and then solving for the coefficients would work, but again how to organize the code for such a thing?

Here’s a snippet simulatng extracting the band matrix from matrix products.

import numpy as np

N = 6
B = 5
h = np.diag(np.random.randn(N))
h = h + h.T # symmetrize our matrix
print(h)
band = y = np.zeros((B, N))
for i in range(B):
y = np.zeros(N)
y[i::B]=1
band[i,:] = h @ y
print(band)
for i in range(B):
band[:,i::B] = np.roll(band[:,i::B], -i, axis=0) #B//2

print(band)
print(band[:B//2+1,:])

and here is the full pytorch implementation including a linear banded solve.

import torch
import matplotlib.pyplot as plt
import numpy as np
import torch.optim
from scipy import linalg
import matplotlib.pyplot as plt

N = 12

L = torch.sum((x[1:] - x[ :-1])**2)/2 + x**2/2 + x[-1]**2/2 #torch.sum((x**2))

#L.backward()
B = 3

print(delL)

for i in range(3):
y[i::3]=1
delLy = delL @ y

delLy.backward(retain_graph=True)
print(hess)
nphess = hess.detach().numpy()
print(nphess)
for i in range(B):
nphess[:,i::B] = np.roll(nphess[:,i::B], -i, axis=0)

print(nphess)
print(nphess[:B//2+1,:])
hessband = nphess[:B//2+1,:]
b = np.zeros(N)
b=1
x = linalg.solveh_banded(hessband, b, lower=True)
print(x)

plt.plot(x)
plt.show()


Output:

tensor([ 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.])
None
tensor([ 2., -1., -1.,  2., -1., -1.,  2., -1., -1.,  2., -1.,  0.])
tensor([-1.,  2., -1., -1.,  2., -1., -1.,  2., -1., -1.,  2., -1.])
tensor([ 0., -1.,  2., -1., -1.,  2., -1., -1.,  2., -1., -1.,  2.])
tensor([[ 2., -1., -1.,  2., -1., -1.,  2., -1., -1.,  2., -1.,  0.],
[-1.,  2., -1., -1.,  2., -1., -1.,  2., -1., -1.,  2., -1.],
[ 0., -1.,  2., -1., -1.,  2., -1., -1.,  2., -1., -1.,  2.]])
[[ 2. -1. -1.  2. -1. -1.  2. -1. -1.  2. -1.  0.]
[-1.  2. -1. -1.  2. -1. -1.  2. -1. -1.  2. -1.]
[ 0. -1.  2. -1. -1.  2. -1. -1.  2. -1. -1.  2.]]
[[ 2.  2.  2.  2.  2.  2.  2.  2.  2.  2.  2.  2.]
[-1. -1. -1. -1. -1. -1. -1. -1. -1. -1. -1.  0.]
[ 0. -1. -1. -1. -1. -1. -1. -1. -1. -1. -1. -1.]]
[[ 2.  2.  2.  2.  2.  2.  2.  2.  2.  2.  2.  2.]
[-1. -1. -1. -1. -1. -1. -1. -1. -1. -1. -1.  0.]]
[0.61538462 1.23076923 1.84615385 2.46153846 3.07692308 2.69230769
2.30769231 1.92307692 1.53846154 1.15384615 0.76923077 0.38461538] 