def robotInit(self):
# optimization problem
T = 100 # horizon
x0 = array([0, 0, 0]) # initial state
u0 = .1 * random.randn(T, 2) # initial controls
#u0 = zeros((T, 2)) # initial controls
options = {}
# run the optimization
options["lims"] = array([[-1, 1], [-1, 1]])
start_time = time.time()
self.x, self.u, L, Vx, Vxx, cost = ilqg.ilqg(
lambda x, u: dynamics_func(x, u), cost_func, x0, u0, options)
self.i = 0
print(self.x[-1])
print("ilqg took {} seconds".format(time.time() - start_time))
cost_graph(self.x)
self.drive = wpilib.RobotDrive(0, 1)
self.joystick = wpilib.Joystick(0)
def robotInit(self):
# optimization problem
T = 100 # horizon
x0 = array([0, 0, 0]) # initial state
u0 = .1*random.randn(T, 2) # initial controls
#u0 = zeros((T, 2)) # initial controls
options = {}
# run the optimization
options["lims"] = array([[-1, 1],
[-1, 1]])
start_time = time.time()
self.x, self.u, L, Vx, Vxx, cost = ilqg.ilqg(lambda x, u: dynamics_func(x, u), cost_func, x0, u0, options)
self.i = 0
print(self.x[-1])
print("ilqg took {} seconds".format(time.time() - start_time))
cost_graph(self.x)
self.drive = wpilib.RobotDrive(0, 1)
self.joystick = wpilib.Joystick(0)
A = expm(h * A) # discrete time
B = h * random.randn(m, n)
#A = array([[1, -1.5e-4, -4.6e-5],
# [1.5e-4, 1, 1.1e-4],
# [4.5e-5, -1.1e-4, 1]])
#B = array([[-1.7e-5, 1.1e-4],
# [1.5e-4, 4.4e-6],
# [-1.4e-5, 3.7e-6]])
# quadratic costs
Q = h * eye(n)
R = .1 * h * eye(m)
# control limits
#Op.lims = ones(m,1)*[-1 1]*.6;
# optimization problem
dyncst = lambda x, u, i, want_all=False: lin_dyn_cst(x, u, A, B, Q, R, want_all
)
T = 100 # horizon
x0 = random.randn(n) # initial state
u0 = .1 * random.randn(T, m) # initial controls
#x0 = array([[ 0.07919485],
# [-0.39150426],
# [ 0.39676904]])
#u0 = array([[0.08702516, -0.10695812, -0.03761507, 0.00790764, 0.00532442, 0.08218673, -0.04070015, 0.05781017, 0.0340251, 0.15567711],
# [-0.16786378, 0.08034461, -0.23664327, 0.16031643, 0.15972222, 0.00393588, -0.01797945, -0.14965136, 0.13926328, -0.00071236]])
#u0 = tile(u0, (1, T/10))
# run the optimization
x, u, L, Vx, Vxx, cost = ilqg.ilqg(dyncst, x0, u0, {})
#print(L[:,:,-1])
# final convergence will be much faster (quadratic)
full_DDP = True
# optimization problem
DYNCST = lambda x, u, i, want_all=False: dyn_cst(x, u, want_all)
T = 500 # horizon
x0 = array([1, 1, pi*3/2, 0]) # initial state
u0 = .1*random.randn(T, 2) # initial controls
#u0 = zeros((T, 2))
options = {}
options["lims"] = array([[-.5, .5], # wheel angle limits (radians)
[ -2, 2]]) # acceleration limits (m/s^2)
# run the optimization
#options["maxIter"] = 5
x, u, L, Vx, Vxx, cost = ilqg(DYNCST, x0, u0, options)
print("done")
## ======== graphics functions ========
#function h = car_plot(x,u)
#
#body = [0.9 2.1 0.3] # body = [width length curvature]
#bodycolor = 0.5*[1 1 1]
#headlights = [0.25 0.1 .1 body(1)/2] # headlights [width length curvature x]
#lightcolor = [1 1 0]
#wheel = [0.15 0.4 .06 1.1*body(1) -1.1 .9] # wheels = [width length curvature x yb yf]
#wheelcolor = 'k'
#
#h = []
#
## make wheels
#for front = 1:2
A = A-A.T # skew-symmetric = pure imaginary eigenvalues
A = expm(h*A) # discrete time
B = h*random.randn(m, n)
#A = array([[1, -1.5e-4, -4.6e-5],
# [1.5e-4, 1, 1.1e-4],
# [4.5e-5, -1.1e-4, 1]])
#B = array([[-1.7e-5, 1.1e-4],
# [1.5e-4, 4.4e-6],
# [-1.4e-5, 3.7e-6]])
# quadratic costs
Q = h*eye(n)
R = .1*h*eye(m)
# control limits
#Op.lims = ones(m,1)*[-1 1]*.6;
# optimization problem
dyncst = lambda x, u, i, want_all=False: lin_dyn_cst(x, u, A, B, Q, R, want_all)
T = 100 # horizon
x0 = random.randn(n) # initial state
u0 = .1*random.randn(T, m) # initial controls
#x0 = array([[ 0.07919485],
# [-0.39150426],
# [ 0.39676904]])
#u0 = array([[0.08702516, -0.10695812, -0.03761507, 0.00790764, 0.00532442, 0.08218673, -0.04070015, 0.05781017, 0.0340251, 0.15567711],
# [-0.16786378, 0.08034461, -0.23664327, 0.16031643, 0.15972222, 0.00393588, -0.01797945, -0.14965136, 0.13926328, -0.00071236]])
#u0 = tile(u0, (1, T/10))
# run the optimization
x, u, L, Vx, Vxx, cost = ilqg.ilqg(dyncst, x0, u0, {})
#print(L[:,:,-1])
# optimization problem
DYNCST = lambda x, u, i, want_all=False: dyn_cst(x, u, want_all)
T = 500 # horizon
x0 = array([1, 1, pi * 3 / 2, 0]) # initial state
u0 = .1 * random.randn(T, 2) # initial controls
#u0 = zeros((T, 2))
options = {}
options["lims"] = array([
[-.5, .5], # wheel angle limits (radians)
[-2, 2]
]) # acceleration limits (m/s^2)
# run the optimization
#options["maxIter"] = 5
x, u, L, Vx, Vxx, cost = ilqg(DYNCST, x0, u0, options)
print("done")
## ======== graphics functions ========
#function h = car_plot(x,u)
#
#body = [0.9 2.1 0.3] # body = [width length curvature]
#bodycolor = 0.5*[1 1 1]
#headlights = [0.25 0.1 .1 body(1)/2] # headlights [width length curvature x]
#lightcolor = [1 1 0]
#wheel = [0.15 0.4 .06 1.1*body(1) -1.1 .9] # wheels = [width length curvature x yb yf]
#wheelcolor = 'k'
#
#h = []
#
## make wheels
