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 https://web.stanford.edu/class/ee364b/lectures/seq_slides.pdf
I used the discretization of the equations of motion using the mehotd described here https://epubs.siam.org/doi/pdf/10.1137/16M1062569
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): #print(x) b = np.zeros_like(x) theta = x dtheta = x a = u b = dtheta b = (a * np.cos(theta) - g * np.sin(theta)) / l return b def df(x, u): A = np.zeros((x.shape, x.shape)) B = np.zeros((x.shape, u.shape)) theta = x dtheta = x a = u # 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) constraints.append(x == 0) xs.append(x) u = cvx.Variable(1) constraints.append(u <= 13.0) constraints.append(u >= -13.0) controls.append(u) for i in range(lookahead): next_u = cvx.Variable(1) controls.append(next_u) # next time step variables next_x = cvx.Variable(2) half_x = cvx.Variable(2) #delthetan = cvx.Variable() #deldthetan = cvx.Variable() #delthetas.append(delthetan) #deldthetas.append(deldthetan) xs.append(half_x) xs.append(next_x) #lin = linearApproxAlpha(a[i], theta[i]) #Dynamics 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 - np.pi, M=0.5) + 0.01 * cvx.huber(u)#+ (np.cos(np_x[2*i,:]) + 1) * (x - np_x[2*i,:]) #+ cvx.square( x - 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 - np.pi ) # cvx.huber( x - np.pi, M=0.4) objective = cvx.Minimize(cost) #print(objective) #print(constraints) prob = cvx.Problem(objective, constraints) sol = prob.solve(verbose=True) #print(sol) #update by the del #theta += np.array(list(map( lambda x: x.value, delthetas))) #print(x.value) #print(constraints) np_x = np.array(list(map( lambda x: x.value, xs))) print(np_x.shape) np_x = np_x.reshape((-1,2)) print(np_x.shape) np_u = np.array(list(map( lambda x: x.value, controls))).reshape((-1,1)) ''' plt.plot(np_x[::2,0]) plt.plot(np_x[::2,1]) plt.plot(np_u[:,0]) plt.show() ''' #dtheta += np.array(list(map( lambda x: x.value, deldthetas))) #a += np.array(list(map( lambda x: x.value, controls))) #print(np_u) plt.plot(np_x[::2,0]) plt.plot(np_x[::2,1]) plt.plot(np_u[:,0]) plt.show() #TODO ''' 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.