Some random links on uses of Convex relaxations, in particular Semidefinite programming


Problems involving outer products of vector variables can be relaxed into semidefinite programs. That’s a general trick. Then the low rank bit from SVD is an approixmate solution for the vector


convex relaxation for distributed optimal control



graph matching in relation to Image correspondence

Permutation matrices have sum of rows and columns must be 1 constraint, is one relaxation.

quickMatch. Actually, not convex programming but was the root of the chain of references I ‘m digging through

Click to access Tron_Fast_Multi-Image_Matching_ICCV_2017_paper.pdf



Finding MaxCut approximation of a graph is a classic one



Quantum Semidefinite programming course

Density matrices have a semidefinite constrina (non negative probabilities)



Sum of Squares is a semidefinite program that can guarantee that lyapunov functions actually work



Cart Pole Trajectory optimization using Cvxpy

I’ve been watching Russ Tedrake’s underactuated robotics and been trying some stuff out. The Drake package probably does this stuff better. Also IpOpt and Snopt are the libraries that get mentioned when people want to do this stuff for real.

It’s  too slow to be practical. That is mostly the fault of the cvxpy overhead. The ECOS solver itself says that it solves each iteration in about 0.02 seconds on my macbook.

The idea is to use linearized dynamics as constraints. Then iteratively ask cvxpy to solve for us. Until hopefully it converges. This is Sequential Convex Programming

I used the discretization of the equations of motion using the mehotd described here

It is possible I did it right.

The Hermite Collocation for the trajectory and trapezoidal for the control

If we used just an absolute value cost, this all would be a linear program. Something to consider. Also CVXOPT would probably be faster.

It hurts me that the constraint and cost matrices are banded and could be solved so quickly. But the next level up in programming complexity takes a lot more work it seems to me.

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

g = 9.8
l = 1.0
dt = 0.1
lookahead = 100

def f(x, u):
    b = np.zeros_like(x)
    theta = x[0]
    dtheta = x[1]
    a = u[0]
    b[0] = dtheta
    b[1] = (a * np.cos(theta) - g * np.sin(theta)) / l
    return b

def df(x, u):
    A = np.zeros((x.shape[0], x.shape[0]))
    B = np.zeros((x.shape[0], u.shape[0]))
    theta = x[0]
    dtheta = x[1]
    a = u[0]
    # dthetadot / dtheta
    A[0,1] = 1 
    # dtheta derviatvie.
    A[1,0] = (- a * np.sin(theta) - g * np.cos(theta)) / l
    B[1,0] = np.cos(theta) / l
    #b[1,:] = (a * np.cos(theta) - g * np.sin(theta)) / l
    return A, B

def linf(x, u, x2, u2):
    b = f(x,u)
    A, B = df(x,u)
    return b + A @ (x2 - x) + B * (u2 - u)

np_x = np.zeros((2*lookahead+1, 2))

np_u = np.zeros((lookahead+1, 1))
for j in range(15):
    controls = []
    constraints = []
    thetas = []
    dthetas = []
    xs = []
    cost = 0

    #initiial condition constraints

    x = cvx.Variable(2)
    constraints.append(x[0] == 0)
    constraints.append(x[1] == 0)


    u = cvx.Variable(1)
    constraints.append(u <= 13.0)   
    constraints.append(u >= -13.0)

    for i in range(lookahead):
        next_u = cvx.Variable(1)

        # next time step variables
        next_x = cvx.Variable(2)
        half_x = cvx.Variable(2)

        #delthetan = cvx.Variable()
        #deldthetan = cvx.Variable()    
        #lin = linearApproxAlpha(a[i], theta[i])


        constraints.append(half_x == next_x/2 + x/2 + dt/8 * (linf(np_x[2*i,:], np_u[i,:],x, u ) - linf(np_x[2*i+2,:], np_u[i+1,:],next_x, next_u )))

        constraints.append(next_x - x ==  (linf(np_x[2*i,:], np_u[i,:], x, u ) + 4 * linf(np_x[2*i+1,:], (np_u[i,:] + np_u[i+1,:]) / 2, half_x, (u + next_u)/2) + linf(np_x[2*i+2,:], np_u[i+1,:], next_x, next_u )  ) * dt / 6)
        #constraints.append(deldthetan == deldtheta + lin(at, deltheta) * dt)

        #conditions on allowable control
        constraints.append(next_u <= 8.0)   
        constraints.append(next_u >= -8.0)
        #trust regions

        #Cost calculation  
        cost = cost + cvx.huber( x[0] - np.pi, M=0.5) + 0.01 * cvx.huber(u)#+ (np.cos(np_x[2*i,:]) + 1) * (x[0] - np_x[2*i,:])  #+ cvx.square( x[0] - np.pi ) #+ cvx.square(u) #+ 0.1 * cvx.square(ut)
        # + cvx.square(np.cos(np_x[2*i,:])*(x - np_x[2*i,:]))  
        x = next_x
        u = next_u

    cost = cost + 100 * cvx.square( x[0] - np.pi )  # cvx.huber( x[0] - np.pi, M=0.4)
    objective = cvx.Minimize(cost)
    prob = cvx.Problem(objective, constraints)
    sol = prob.solve(verbose=True)
    #update by the del
    #theta += np.array(list(map( lambda x: x.value, delthetas))) 
    np_x = np.array(list(map( lambda x: x.value, xs)))
    np_x = np_x.reshape((-1,2))
    np_u = np.array(list(map( lambda x: x.value, controls))).reshape((-1,1))
    #dtheta += np.array(list(map( lambda x: x.value, deldthetas))) 
    #a += np.array(list(map( lambda x: x.value, controls)))


We need to add cart position contraints
Parameters to hopefully speed up
Better initial guess.
Derivative of Cost
Is this actually working?


This is the result

Green is cart acceleration. You can see it slamming into the maximum acceleration constraint

Blue is pole angle and orange is angular velocity. So it does a pretty good job. For some settings it is just awful though.


Making a Van Der Graaf Generator

Will has been juicin’ to make a Van der Graaf generator. Similar to the Stirling engines, we followed youtube instructions to make it out of trash (soda bottles and soda cans and tape) and failed miserably.

We were mostly following this video

Yesterday, we took the gloves off and went to home depot. We used a trash fan motor which is awesome. 3d Printed some pieces for mounting the motor and for mounting the roller on the shaft with a press fit. 

Big ole 4″ pvc is the stalk. A flange to mount to the base which is a nice circle wood from the nice wood section of . Used two ikea bowls as the globe. I think it is important to have the upper roller somewhat deeply inside the globe?

We’re using pantyhose as a belt. Sort of the waist area stretched out to be a loop. It is just rubbing on the fixed top pvc roller. Maybe wrapping that in teflon tape would make it even better? The pantyhose is pure nylon. Nylon and pvc are towards opposite ends of the triboelectric series, which is important for the thing to work, but teflon is even farther down.

We used some brushes from home depot as electrical brushes. Might be overkill.

We wrapped the lower roller in more pantyhose to make a bulge. Counterintuitively, this bulge is supposed to keep the belt centered?

We were getting pretty wimpy sparks until we installed to lower brush and properly grounded it. I guess that is really important. Now they are pretty beefy maybe a couple centimeters. You can definitely see them and they sting a bit.

All in all a success!

img_0231 img_5799 img_4759 img_6389



Holy crap this was easy.

And I was able to easily add a constraint on the available force. Hot damn. It is a ridiculously tiny problem I guess, but still pretty damn cool. 0.002 second runtime.

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

lookahead = 50
dt = 0.1
F = 1.0
objective = 0
A = np.array([[1,dt],[0,1]])
B = np.array([0,dt*F])
x0 = np.array([1,0])
xt = cvx.Variable(2)
state = [xt]
cost = 0
constraints = [xt == x0]
controls = []
for i in range(lookahead):
	ut = cvx.Variable()
	xtn = cvx.Variable(2)

	constraints.append(xtn == A*xt + B * ut )
	constraints.append(ut <= 1.0)   
	constraints.append(ut >= -1.0)  
	cost = cost + cvx.square(xtn[0]) #+ 0.1 * cvx.square(ut)

	xt = xtn

objective = cvx.Minimize(cost)
prob = cvx.Problem(objective, constraints)
sol = prob.solve(verbose=True)
pos = np.array(list(map( lambda x: x.value, state)))
us = np.array(list(map( lambda x: x.value, controls)))



STM32F411 Discovery Board Getting started

Bought one of these discovery boards for 15$ from digikey. I like the built in stuff, like the 9axis mems device. I don’t enjoy wiring up little boards particularly and it makes every project so ephemeral.

I am concerned that I should have gotten the older board.

User manual has pin connectors

Click to access en.DM00148985.pdf

It’s in as


Huh it only supports STM32Cube? I kind of wanted to try libOpenCM3

start a project with

platformio init –board disco_f411ve

platformio run

The examples are invaluable

ok. There is a blink for the Hardware abstraction layer (HAL) and Low level (LL)

Hmm. Neither blink examples work off the bat. SYS_CLOCK is undefined for Low level

and board not supported in the HAL.



Alright let’s try libopencm3

make in top directory first

follow directions to init submodule

alirght I guess I need to download the arm toolchain

I put them on my path

export PATH=~/Downloads/gcc-arm-none-eabi-7-2017-q4-major/bin:$PATH

Then ran make

also need to apt install gawk

editted the Makefile of miniblink

stm32f411re stm32f4 ROM=512K RAM=128K

that’s the right config even though re is wrong

Okay was getting weird error

sudo make flash V=1

the V=1 gives verbose

I may have had to apt install openocd?

Need to run under sudo. Go fig.

Alright got some blinking! WOOO

Ah I see. PD12 is PortD12. Makes sense.



PlatformIO and Visual Studio Take over the World

Somehow I was not aware of this thing. It is a build tool for microcontrollers

Seems like people basically like it. 1000+ stars on github

python -m pip install -U platformio


make a folder

platformio init –board icestick

Holy crap. Is this thing going to download and setup the tools? THAT. IS. AWESOME. If it works.

Better yet clone this bad boy

go to the blink folder.

platformio run

platformio run –target upload

Holy. Hell. It worked. THAT IS NUTS.

The commands it ran to compile

yosys -p "synth_ice40 -blif .pioenvs/icestick/hardware.blif" -q src/counter.v
arachne-pnr -d 1k -P tq144 -p /home/philip/Documents/platformio-examples/lattice_ice40/lattice_ice40-counter/src/counter.pcf -o .pioenvs/icestick/hardware.asc .pioenvs/icestick/hardware.blif

Hmm. I’m puzzled. Where did this come from? How did it know counter.v?


Mecrisp has an icestick version. Intriguing (Mecrisp is a forth implementation)

had to sudo apt install libreadline6 and gtkwave to run simulation

I had to follow these instructions to get the FTDI device to work

and change the platformio.ini file to say icestick instead of icezum. Actually i don’t think that is necessary.




Cart Pole using Lyapunov and LQR control, OpenAI gym

We’re having a lot of trouble hacking together a reinforcement learning version of this, so we are taking an alternative approacg, inspired by wtaching the MIT underactuated robotics course.

It took some pen and paper to get the equations of motion (which are maybe right?).

openai gym has

We switch over to LQR when the y position of the pole is above a certain height

This scipy function solves the algebriac ricatti equation in the ocntinous time infite horizon section

Things that helped: Trying to balance pole first from upright position then from downright.

Tuning weights for theta and thetadot. Thetadot was too small made it unstable

Hacked in the LQR control by adjusting force_mag variable. Nasty.


Put it some slight compensation for a delayed observation, which reflects our actual sensor system


from custom_cartpole_delay import CartPoleEnv

import gym

import numpy as np
import scipy.linalg as linalg
lqr = linalg.solve_continuous_are

env = CartPoleEnv()
env.buffer_size = 5
gravity = 9.8
masscart = 1.0
masspole = 0.1
total_mass = (masspole + masscart)
length = 0.5 # actually half the pole's length
polemass_length = (masspole * length)
force_mag = 10.0
tau = 0.02 

def E(x):
	return 1/2 * masspole * (2 * length)**2 / 3 *  x[3]**2 + np.cos(x[2]) * polemass_length * gravity
def u(x):
	return 1.0 * (E(x)-Ed) * x[3] * np.cos(x[2])

H = np.array([
	[0,total_mass, 0, - polemass_length],
	[0,- polemass_length, 0, (2 * length)**2 * masspole /3]

Hinv = np.linalg.inv(H)

A = Hinv @ np.array([
    [0,0, - polemass_length * gravity, 0]
B = Hinv @ np.array([0,1.0,0,0]).reshape((4,1))
Q = np.diag([0.1, 1.0, 100.0, 5.0])
R = np.array([[0.1]])

P = lqr(A,B,Q,R)
Rinv = np.linalg.inv(R)
K = Rinv @ B.T @ P
def ulqr(x):
	x1 = np.copy(x)
	x1[2] = np.sin(x1[2])
	return, x1)

Ed = E([0,0,0,0])

observation = env.reset()
for _ in range(1000):
  observation = np.copy(observation)
  observation[0] += observation[1] * tau * env.buffer_size
  observation[2] += observation[3] * tau * env.buffer_size
  observation[3] += np.sin(observation[2]) * tau * env.buffer_size * gravity  / (length * 2) / 2
  #action = env.action_space.sample() # your agent here (this takes random actions)
  a = u(observation) - 0.3 * observation[0] -  0.1 * observation[1]
  if  abs(E(observation)-Ed) < 0.1 and np.cos(observation[2]) > 0.6: #abs(E(observation)-Ed) < 0.1 and
  	a = ulqr(observation)
  	env.force_mag = min(abs(a[0]), 10)
  	env.force_mag = 10.0
  if a < 0:
  	action = 0
  	action = 1

  observation, reward, done, info = env.step(action)

import math
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import logging
logger = logging.getLogger(__name__)
class CartPoleEnv(gym.Env):
    metadata = {
        'render.modes': ['human', 'rgb_array'],
        'video.frames_per_second' : 50
    def __init__(self):
        self.gravity = 9.8
        self.masscart = 1.0
        self.masspole = 0.1
        self.total_mass = (self.masspole + self.masscart)
        self.length = 0.5 # actually half the pole's length
        self.polemass_length = (self.masspole * self.length)
        self.force_mag = 3.0
        self.tau = 0.02  # seconds between state updates
        # Angle at which to fail the episode
        # we expect full swings
        self.theta_threshold_radians =  np.pi  #12 * 2 * math.pi / 360
        self.x_threshold = 2.4
        # Angle limit set to 2 * theta_threshold_radians so failing observation is still within bounds
        high = np.array([
            self.x_threshold * 2,
            self.theta_threshold_radians * 2,
        high2 = np.array([1])
        self.action_space = spaces.Discrete(2) #spaces.Box(-high2, high2)# 
        self.observation_space = spaces.Box(-high, high)
        self.viewer = None
        self.state = None
        self.buffer_size = 0

        self.steps_beyond_done = None
        self.steps = 0
        self.num_envs = 1
        self.viewer = None
    viewer = None
    def _seed(self, seed=None):
        self.np_random, seed = seeding.np_random(seed)
        return [seed]
    def _step(self, action):
        #assert self.action_space.contains(action), "%r (%s) invalid"%(action, type(action))
        #action =action[0] # max(-1, min(action[0],1))
        state = self.state
        x, x_dot, theta, theta_dot = state
        force = self.force_mag if action==1 else -self.force_mag
        #force = self.force_mag * action
        x  = x + self.tau * x_dot
        theta = theta + self.tau * theta_dot
        costheta = math.cos(theta)
        sintheta = math.sin(theta)
        temp = (force + self.polemass_length * theta_dot * theta_dot * sintheta) / self.total_mass
        thetaacc = (self.gravity * sintheta - costheta* temp) / (self.length * (4.0/3.0 - self.masspole * costheta * costheta / self.total_mass))
        xacc  = temp - self.polemass_length * thetaacc * costheta / self.total_mass
        x_dot = x_dot + self.tau * xacc
        theta_dot = theta_dot + self.tau * thetaacc
        self.state = (x,x_dot,theta,theta_dot)
        done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -np.pi * 5 \
                or theta > np.pi * 5 \
                or self.steps > 1024
        done = bool(done)
        self.steps += 1
        limit = 200
        reward = 0.0
        if  x < -self.x_threshold or x > self.x_threshold:
            reward -= 1.0
        reward += (np.cos(theta)+1)**2 / 4
        #reward -= 0.1 * action**2 
        reward += -0.1 * x**2
        if np.cos(theta) > 0.95:
            reward += 1
        #reward = reward/2048
        if not done:
            reward = 1.0
        elif self.steps_beyond_done is None:
            # Pole just fell!
            self.steps_beyond_done = 0
            reward = 1.0
            if self.steps_beyond_done == 0:
                logger.warning("You are calling 'step()' even though this environment has already returned done = True. You should always call 'reset()' once you receive 'done = True' -- any further steps are undefined behavior.")
            self.steps_beyond_done += 1
            reward = 0.0
        #print(np.array(self.state).reshape((1,-1)), reward, done, {})

        #obs = np.array([x,x_dot,np.cos(theta),np.sin(theta),theta_dot])# + self.action_buffer)
        obs2 = self.buffer.pop(0)
        return obs2, reward, done, {}
    def _reset(self):
        #self.state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
        #x, xdot, theta, thetadot
        self.state = np.array([0, 0, np.pi+0.2 , 0]) # np.pi+0.2 + self.np_random.uniform(low=-1.0, high=1.0, size=(4,))
        self.steps_beyond_done = None
        self.steps = 0
        self.buffer = []
        for i in range(self.buffer_size):
        return np.array(self.state)
    def _render(self, mode='human', close=False):
        if close:
            if self.viewer is not None:
                self.viewer = None
        screen_width = 600
        screen_height = 400
        world_width = self.x_threshold*2
        scale = screen_width/world_width
        carty = 100 # TOP OF CART
        polewidth = 10.0
        polelen = scale * 1.0
        cartwidth = 50.0
        cartheight = 30.0
        if self.viewer is None:
            from gym.envs.classic_control import rendering
            self.viewer = rendering.Viewer(screen_width, screen_height)
            l,r,t,b = -cartwidth/2, cartwidth/2, cartheight/2, -cartheight/2
            axleoffset =cartheight/4.0
            cart = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)])
            self.carttrans = rendering.Transform()
            l,r,t,b = -polewidth/2,polewidth/2,polelen-polewidth/2,-polewidth/2
            pole = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)])
            self.poletrans = rendering.Transform(translation=(0, axleoffset))
            self.axle = rendering.make_circle(polewidth/2)
            self.track = rendering.Line((0,carty), (screen_width,carty))
        if self.state is None: return None
        x = self.state
        cartx = x[0]*scale+screen_width/2.0 # MIDDLE OF CART
        self.carttrans.set_translation(cartx, carty)
        return self.viewer.render(return_rgb_array = mode=='rgb_array')





Linksprite CNC machine

I’m impressed with the package in the box. Well organanized.

Putting it together took maybe 4 hours, half watching The Wire.

The x-axis screw is not fitting. I’m hoping the end is just ground incorrectly.

Nope. This has been a huge pain. They sent me the wrong screw. There are 8mm 4mm and 2mm pitch T8 rods. They have 1 2 and 4 starts to their threading. I have bought the maximal number of incorrect rods. I now have in posession the 4mm pitch rod I need. Figuring this out has set me back a month and another 30$. I am annoyed.

There is a crack in the z-axis printed part. Epoxy should fix it.

open up arduino serial monitor

115200 baud both NL & CR

$$ (view Grbl settings)
$# (view # parameters)
$G (view parser state)
$I (view build info)
$N (view startup blocks)
$x=value (save Grbl setting)
$Nx=line (save startup block)
$C (check gcode mode)
$X (kill alarm lock)
$H (run homing cycle)
~ (cycle start)
! (feed hold)
? (current status)
ctrl-x (reset Grbl)


Hmm. LinuxCNC is more complicated than I thought. It seems installing it on my regular ubuntu 14.04 is not an option without a lot of dangerous futzing. Uses a real-time special kernel.

whenever it didn’t work i apt-get installed whatever was missing.

I also had to add a non redistributable when that error came up.





Two Control Guys. One in python one in java


Pycam and Blendercam do 3d models. Pycam appears to be defunct

go to folder and make.

The website seems wrong. The links aren’t.

Build error

I added
#include <stddef.h>
to Edge.h.




Inkscape gcodetools.

An Intro to G-code and How to Generate It Using Inkscape


Openscam (now called camotics?) is a path simulator.


S1000 sets the spindle speed

M3 turns spindle on

M5 turns off

even 0.2 depth is a little deep

home. reset

turn on the spindle