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.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 |
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[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) 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[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) #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[0]) 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.