def getAlu(self, x, x0, v0, th0, thd0):
N = self.N
gt = np.zeros((2, N))
gt[0, :] = 0.15 # x is greaer than 0.15
gt[1, :] = -1 #veclotu is gt -1m/s
gt = gt.flatten()
lt = np.zeros((2, N))
lt[0, :] = 0.75 # x less than 0.75
lt[1, :] = 1 # velocity less than 1m/s
lt = lt.flatten()
z = sparse.bsr_matrix((N, N))
ineqA = sparse.bmat([[sparse.eye(N), z, z, z, z],
[z, sparse.eye(N), z, z, z]]) #.tocsc()
#print(ineqA.shape)
#print(ineqA)
cons = self.constraint(forward.seed_sparse_gradient(x), x0, v0, th0,
thd0)
A = sparse.vstack(map(lambda z: z.dvalue, cons)) # y.dvalue.tocsc()
#print(A.shape)
totval = np.concatenate(tuple(map(lambda z: z.value, cons)))
temp = A @ x - totval
A = sparse.vstack((A, ineqA)).tocsc()
#print(tuple(map(lambda z: z.value, cons)))
#print(temp.shape)
#print(lt.shape)
#print(gt.shape)
u = np.concatenate((temp, lt))
l = np.concatenate((temp, gt))
return A, l, u
def getAlu(self, x, x0, v0, th0, thd0):
N = self.N
gt = np.zeros((2, N))
gt[0, :] = -0.1 # 0.15 # x is greaer than 0.15
gt[1, :] = -3 # -1 #veclotu is gt -1m/s
# gt[4,:] = -10
control_n = max(3, int(0.1 / self.dt)) # I dunno. 4 seems to help
# print(control_n)
gt[:, :control_n] = -100
# gt[1,:2] = -100
# gt[1,:2] = -15
# gt[0,:3] = -10
gt = gt.flatten()
lt = np.zeros((2, N))
lt[0, :] = 1 # 0.75 # x less than 0.75
lt[1, :] = 3 # 1 # velocity less than 1m/s
# lt[4,:] = 10
lt[:, :control_n] = 100
# lt[1,:2] = 100
# lt[0,:3] = 10
# lt[1,:2] = 15
lt = lt.flatten()
z = sparse.bsr_matrix((N, N))
ineqA = sparse.bmat([[sparse.eye(N), z, z, z, z],
[z, sparse.eye(N), z, z, z]]) # .tocsc()
# print(ineqA.shape)
# print(ineqA)
cons = self.constraint(forward.seed_sparse_gradient(x), x0, v0, th0,
thd0)
A = sparse.vstack(map(lambda z: z.dvalue, cons)) # y.dvalue.tocsc()
# print(A.shape)
totval = np.concatenate(tuple(map(lambda z: z.value, cons)))
temp = A @ x - totval
A = sparse.vstack((A, ineqA)).tocsc()
# print(tuple(map(lambda z: z.value, cons)))
# print(temp.shape)
# print(lt.shape)
# print(gt.shape)
u = np.concatenate((temp, lt))
l = np.concatenate((temp, gt))
return A, l, u
dx_dot = (x_dot[0:-1] - x_dot[1:]) * dt1
dtheta = (theta[0:-1] - theta[1:]) * dt1
dtheta_dot = (theta_dot[0:-1] - theta_dot[1:]) * dt1
f = a[1:] #return v - f()
t = -np.sin(avg_theta) + a[1:] * np.cos(avg_theta)
x_res = dx - avg_x_dot
x_dot_res = dx_dot - f
theta_res = dtheta - avg_theta_dot
theta_dot_res = dtheta_dot - t
return x[0:1] - x0, x_dot[0:1] - x_dot0, theta[0:1] - theta0, theta_dot[
0:1] - theta_dot0, x_res, x_dot_res, theta_res, theta_dot_res
cons = constraint(forward.seed_sparse_gradient(x), 0, 0, 0, 0)
A = sparse.vstack(map(lambda z: z.dvalue, cons)).tocsc()
totval = np.concatenate(tuple(map(lambda z: z.value, cons)))
temp = A @ x - totval
u = temp
l = temp
m = osqp.OSQP()
m.setup(P=P, q=q, A=A, l=l, u=u)
results = m.solve()
print(results.x)
plt.plot(results.x[:N], label="x")
plt.plot(results.x[N:2 * N], label="x_dot")
plt.plot(results.x[2 * N:3 * N], label="theta")
def check_sparsity(msg, f, x):
df = f(forward.seed_sparse_gradient(x)**2).gradient
sdf = f(forward.seed_sparsity(x)**2).sparsity
assert sdf.shape
assert_almost_equal((sdf.tocsr().multiply(df) - df).data, 0.)