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)