def rkstep_jacobians(x1, u0, dt):
# Define constants
# return A, B
Nx = 2
# x1 = x0
t = 0.0
xdot1, A1, B1 = dynamics(t, x1, u0)
k1 = xdot1 * dt
x2 = x1 + 0.5 * k1
xdot2, A2, B2 = dynamics(t + dt * 0.5, x2, u0)
k2 = dt * xdot2
x3 = x1 + 0.5 * k2
xdot3, A3, B3 = dynamics(t + dt * 0.5, x3, u0)
k3 = dt * xdot3
x4 = x1 + k3
xdot4, A4, B4 = dynamics(t + dt, x4, u0)
# k4 = dt * xdot4
# x_tp1 = x1 + (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
# RK4 method
# A_d
dk1_dx1 = dt * A1
dx2_dx1 = np.eye(Nx) + 0.5 * dk1_dx1
dk2_dx1 = dt * mul2(A2, dx2_dx1)
dx3_dx1 = np.eye(Nx) + 0.5 * dk2_dx1
dk3_dx1 = dt * mul2(A3, dx3_dx1)
dx4_dx1 = np.eye(Nx) + dk3_dx1
dk4_dx1 = dt * mul2(A4, dx4_dx1)
A_d = np.eye(Nx) + (1 / 6) * (dk1_dx1 + 2 * dk2_dx1 + 2 * dk3_dx1 +
dk4_dx1)
# B_d
dk1_du = dt * B1
dx2_du = 0.5 * dk1_du
dk2_du = dt * mul2(A2, dx2_du) + dt * B2
dx3_du = 0.5 * dk2_du
dk3_du = dt * mul2(A3, dx3_du) + dt * B3
dx4_du = 1.0 * dk3_du
dk4_du = dt * mul2(A4, dx4_du) + dt * B4
B_d = (1 / 6) * (dk1_du + 2 * dk2_du + 2 * dk3_du + dk4_du)
return A_d, B_d
def diag(a: ulab.array) -> ulab.array:
assert isinstance(a, ulab.array), "Input should be of type ulab.array!"
m, n = a.shape()
itemsize = a.itemsize()
if itemsize == 1:
dtype = ulab.int8
elif itemsize == 2:
dtype = ulab.int16
elif itemsize == 8:
dtype = ulab.float
else:
raise Exception("Unknown datatype of the input array!")
if m <= 1 and n <= 1:
return a
elif (m == 1 and n > 1) or (m > 1 and n == 1):
r = m if m > n else n
b = ulab.eye(r, dtype=dtype)
for i in range(r):
b[i, i] = a[i]
return b
elif m == n:
b = ulab.zeros(m, dtype=dtype)
for i in range(m):
b[i] = a[i][i]
return b
else:
raise Exception("Input should be square matrix!")
def main2():
N = 75
Nx = 2
Nu = 1
Qf = np.eye(Nx) * 30
Q = np.eye(Nx) * 0.01
R = np.eye(1) * 0.03
x0 = np.zeros(2)
xg = np.array([math.pi, 0])
utraj0 = np.zeros((Nu, N - 1))
dt = 0.1
tol = 0.35
xtraj, utraj, K = iLQRsimple_py(x0, xg, utraj0, Q, R, Qf, dt, tol)
def rkstep_jacobians(x0, u0, dt):
# Define constants
A1, B1 = dynamics_jacobians(0, x0, u0)
xdot1 = dynamics_xdot(0, x0, u0)
A2, B2 = dynamics_jacobians(0, x0 + 0.5 * xdot1 * dt, u0)
A = np.eye(2) + dt * A2 + 0.5 * dt * dt * mul2(A2, A1)
B = dt * B2 + 0.5 * dt * dt * mul2(A2, B1)
return A, B
def main2():
N = 25
Nx = 6
Nu = 3
qp = 1
qw = .01
Q = np.array([[qp, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, qp, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, qp, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, qw, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, qw, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0,
qw]])
qpf = 100.0
qwf = 10 * qpf
Qf = np.array([[qpf, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, qpf, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, qpf, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, qwf, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, qwf, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, qwf]])
R = .1 * np.eye(3)
#x0 = np.array([0.07500539420980451, 0.7678669790425526, 0.3299131467179552, 0.0, 0.0, 0.0])
x0 = np.array([
-0.274710632045161, 0.6780834943980876, 0.4108832368302142, 0.0, 0.0,
0.0
])
xg = np.array([0, 0, 0, 0, 0, 0.0])
utraj0 = np.zeros((Nu, N - 1))
dt = 12.0
tol = 0.5
xtraj, utraj, K = iLQRsimple_py(x0, xg, utraj0, Q, R, Qf, dt, tol)
print(xtraj[-1])
from ulab import linalg import ulab print(ulab.ones(3)) print(ulab.ones((2,3))) print(ulab.zeros(3)) print(ulab.zeros((2,3))) print(ulab.eye(3)) print(ulab.ones(1, dtype=ulab.int8)) print(ulab.ones(2, dtype=ulab.uint8)) print(ulab.ones(3, dtype=ulab.int16)) print(ulab.ones(4, dtype=ulab.uint16)) print(ulab.ones(5, dtype=ulab.float))
def dcm_from_p(p):
sp = hat(p)
np2 = np.linalg.dot(p,p.transpose())[0]
return np.eye(3) + (8*mul2(sp,sp) + 4*(1-np2)*sp)/(1+np2)**2
def pdot_from_w(p,w):
np2 = np.linalg.dot(p,p.transpose())[0]
return mul2((.25*(1+np2))*(np.eye(3) + 2*(mul2(hat(p),hat(p)) + hat(p))/(1+np2)) ,w.transpose())
for i in range(3):
for j in range(3):
if i == j:
print(math.isclose(v[i][j], 1.0, rel_tol=1E-9, abs_tol=1E-9))
else:
print(math.isclose(v[i][j], 0.0, rel_tol=1E-9, abs_tol=1E-9))
a = np.array([[25, 15, -5], [15, 18, 0], [-5, 0, 11]])
result = (np.linalg.cholesky(a))
ref_result = np.array([[5., 0., 0.], [ 3., 3., 0.], [-1., 1., 3.]])
for i in range(3):
for j in range(3):
print(math.isclose(result[i][j], ref_result[i][j], rel_tol=1E-9, abs_tol=1E-9))
a = np.array([1,2,3,4,5], dtype=np.float)
result = (np.linalg.norm(a))
ref_result = 7.416198487095663
print(math.isclose(result, ref_result, rel_tol=1E-9, abs_tol=1E-9))
a = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
result = (np.linalg.norm(a)) ## Here is a problem
ref_result = 16.881943016134134
print(math.isclose(result, ref_result, rel_tol=1E-6, abs_tol=1E-6))
if use_ulab:
print(np.linalg.trace(np.eye(3)))
else:
print(np.trace(np.eye(3)))
import time
import board
import math
import ulab as np
def foo(a):
a[1, 1] = 69
a = np.eye(3)
#a = [1,2,3]
foo(a)
print(a)
print(np.arange(2, 10))
print(np.arange(2, 10, 3))
print(np.arange(2, 10, 3, dtype=np.float))
print("Array concatenation:")
a = np.array([1, 2, 3], dtype=np.float)
b = np.array([4, 5, 6], dtype=np.float)
print(np.concatenate((a, b)))
print(np.concatenate((a, b), axis=0))
a = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype=np.float)
b = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype=np.float)
print(np.concatenate((a, b), axis=0))
print(np.concatenate((a, b), axis=1))
print(np.concatenate((b, a), axis=0))
print(np.concatenate((b, a), axis=1))
print("Identity array creation:")
print(np.eye(3))
print(np.eye(3, M=4))
print(np.eye(3, M=4, k=0))
print(np.eye(3, M=4, k=-1))
print(np.eye(3, M=4, k=-2))
print(np.eye(3, M=4, k=-3))
print(np.eye(3, M=4, k=1))
print(np.eye(3, M=4, k=2))
print(np.eye(3, M=4, k=3))
print(np.eye(4, M=4))
print(np.eye(4, M=3, k=0))
print(np.eye(4, M=3, k=-1))
print(np.eye(4, M=3, k=-2))
print(np.eye(4, M=3, k=-3))
print(np.eye(4, M=3, k=1))
print(np.eye(4, M=3, k=2))
def smoke(n):
return ulab.eye(n)