raiot
/
PythonLinearNonlinearControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add iLQR

This commit is contained in:
Shunichi09 2019-04-12 00:10:53 +09:00
parent 44da5d3515
commit d733facb28
5 changed files with 108 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
iLQR/animation.py
View File

@ -141,7 +141,10 @@ class AnimDrawer():
lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref
"""
self.car_history_state = objects[0]
self.traget = objects[1]
self.target = objects[1]
self.history_target_x = [self.target[:, 0]]
self.history_target_y = [self.target[:, 1]]
self.history_xs = [self.car_history_state[:, 0]]
self.history_ys = [self.car_history_state[:, 1]]
@ -155,7 +158,7 @@ class AnimDrawer():
self.car_imgs = []
self.traj_imgs = []
def draw_anim(self, interval=50):
def draw_anim(self, interval=10):
"""draw the animation and save
Parameteres
@ -192,8 +195,8 @@ class AnimDrawer():
self.axis.set_ylabel(r'$\it{y}$ [m]')
self.axis.set_aspect('equal', adjustable='box')
LOW_MARGIN = 5
HIGH_MARGIN = 5
LOW_MARGIN = 2
HIGH_MARGIN = 2
self.axis.set_xlim(np.min(self.history_xs) - LOW_MARGIN, np.max(self.history_xs) + HIGH_MARGIN)
self.axis.set_ylim(np.min(self.history_ys) - LOW_MARGIN, np.max(self.history_ys) + HIGH_MARGIN)
@ -288,7 +291,7 @@ class AnimDrawer():
self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i])
# goal
self.traj_imgs[-1].set_data(self.traget[0], self.traget[1])
self.traj_imgs[-1].set_data(self.history_target_x[0][i-1], self.history_target_y[0][i-1])
# traj_ref
# self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :])
self.traj_imgs[1].set_data(self.history_target_x[0][:i], self.history_target_y[0][:i])

65
iLQR/goal_maker.py
View File

@ -2,7 +2,6 @@ import numpy as np
import math
import matplotlib.pyplot as plt
def make_trajectory(goal_type, N):
"""
Parameters
@ -26,45 +25,93 @@ def make_trajectory(goal_type, N):
return ref_trajectory
if goal_type == "sin":
# parameters
amplitude = 2.
num_period = 2.
ref_x_trajectory = np.linspace(0., 2 * math.pi * num_period, N)
ref_y_trajectory = amplitude * np.sin(ref_x_trajectory)
ref_xy_trajectory = np.stack((ref_x_trajectory, ref_y_trajectory))
# target of theta is always zero
ref_trajectory = np.vstack((ref_xy_trajectory, np.zeros((1, N))))
return ref_trajectory.T
class GoalMaker():
"""
Attributes
-----------
N : int
length of reference
goal_type : str
goal type
dt : float
sampling time
ref_traj : numpy.ndarray, shape(N, STATE_SIZE)
in this case the state size is 3
"""
def __init_(self, goal_type="const", N=500, dt=0.01):
def __init__(self, N=500, goal_type="const", dt=0.01):
"""
Parameters
--------------
N : int
length of reference
goal_type : str
goal type
dt : float
sampling time
"""
self.N = N
self.goal_type = goal_type
self.dt = dt
self.ref_traj = None
self.history_target = []
def preprocess(self):
"""preprocess of make goal
"""
goal_type_list = ["const", "sin"]
if self.goal_type not in goal_type_list:
raise ValueError("{0} is not in implemented goal type. please implement that!!".format(self.goal_type))
raise KeyError("{0} is not in implemented goal type. please implement that!!".format(self.goal_type))
self.ref_traj = make_trajectory(self.goal_type)
self.ref_traj = make_trajectory(self.goal_type, self.N)
def calc_goal(self, x):
"""
""" calclate nearest goal
Parameters
------------
x : numpy.ndarray, shape(STATE_SIZE, )
state of the system
Returns
------------
goal : numpy.ndarray, shape(STATE_SIZE, )
"""
# search nearest point
x_dis = (self.ref_traj[:, 0]-x[0])**2
y_dis = (self.ref_traj[:, 1]-x[1])**2
index = np.argmin(np.sqrt(x_dis + y_dis))
print(index)
MARGIN = 30
if not self.goal_type == "const":
index += MARGIN
if index > self.N-1:
index = self.N - 1
goal = self.ref_traj[index]
self.history_target.append(goal)
return goal
if __name__ == "__main__":
make_trajectory("sin", 400)

4
iLQR/ilqr.py
View File

@ -127,8 +127,8 @@ class iLQRController():
"""
# total cost
R_11 = 0.01 # terminal u_v cost weight
R_22 = 0.01 # terminal u_th cost weight
R_11 = 1e-4 # terminal u_v cost weight
R_22 = 1e-4 # terminal u_th cost weight
l = np.dot(us.T, np.dot(np.diag([R_11, R_22]), us))

28
iLQR/main_dynamic.py
View File

@ -4,9 +4,9 @@ import math
from model import TwoWheeledCar
from ilqr import iLQRController
from goal_maker import GoalMaker
from animation import AnimDrawer
def main():
"""
"""
@ -19,37 +19,47 @@ def main():
car = TwoWheeledCar(init_x)
# make goal
goal_maker =
goal_maker = GoalMaker(goal_type="sin")
goal_maker.preprocess()
# controller
controller = iLQRController()
for iteration in range(NUM_ITERATIONS):
print("iteration num = {} / {}".format(iteration, NUM_ITERATIONS))
target = goal_maker.calc_goal(car.xs)
u = controller.calc_input(car, target)
car.update_state(u, dt) # update state
# figures and animation
history_states = np.array(car.history_xs)
history_targets = np.array(goal_maker.history_target)
time_fig = plt.figure(figsize=(3, 4))
time_fig = plt.figure()
x_fig = time_fig.add_subplot(311)
y_fig = time_fig.add_subplot(312)
th_fig = time_fig.add_subplot(313)
time = len(history_states)
x_fig.plot(np.arange(time), history_states[:, 0])
y_fig.plot(np.arange(time), history_states[:, 1])
th_fig.plot(np.arange(time), history_states[:, 2])
x_fig.plot(np.arange(time), history_states[:, 0], "r")
x_fig.plot(np.arange(1, time), history_targets[:, 0], "b", linestyle="dashdot")
x_fig.set_ylabel("x")
y_fig.plot(np.arange(time), history_states[:, 1], "r")
y_fig.plot(np.arange(1, time), history_targets[:, 1], "b", linestyle="dashdot")
y_fig.set_ylabel("y")
th_fig.plot(np.arange(time), history_states[:, 2], "r")
th_fig.plot(np.arange(1, time), history_targets[:, 2], "b", linestyle="dashdot")
th_fig.set_ylabel("th")
plt.show()
history_states = np.array(car.history_xs)
animdrawer = AnimDrawer([history_states, target])
animdrawer = AnimDrawer([history_states, history_targets])
animdrawer.draw_anim()
if __name__ == "__main__":

27
iLQR/main_static.py
View File

@ -4,8 +4,10 @@ import math
from model import TwoWheeledCar
from ilqr import iLQRController
from goal_maker import GoalMaker
from animation import AnimDrawer
def main():
"""
"""
@ -18,37 +20,48 @@ def main():
car = TwoWheeledCar(init_x)
# make goal
target = np.array([5., 3., 0.])
goal_maker = GoalMaker(goal_type="const")
goal_maker.preprocess()
# controller
controller = iLQRController()
for iteration in range(NUM_ITERATIONS):
print("iteration num = {} / {}".format(iteration, NUM_ITERATIONS))
target = goal_maker.calc_goal(car.xs)
u = controller.calc_input(car, target)
car.update_state(u, dt) # update state
# figures and animation
history_states = np.array(car.history_xs)
history_targets = np.array(goal_maker.history_target)
time_fig = plt.figure(figsize=(3, 4))
time_fig = plt.figure()
x_fig = time_fig.add_subplot(311)
y_fig = time_fig.add_subplot(312)
th_fig = time_fig.add_subplot(313)
time = len(history_states)
x_fig.plot(np.arange(time), history_states[:, 0])
y_fig.plot(np.arange(time), history_states[:, 1])
th_fig.plot(np.arange(time), history_states[:, 2])
x_fig.plot(np.arange(time), history_states[:, 0], "r")
x_fig.plot(np.arange(1, time), history_targets[:, 0], "b", linestyle="dashdot")
x_fig.set_ylabel("x")
y_fig.plot(np.arange(time), history_states[:, 1], "r")
y_fig.plot(np.arange(1, time), history_targets[:, 1], "b", linestyle="dashdot")
y_fig.set_ylabel("y")
th_fig.plot(np.arange(time), history_states[:, 2], "r")
th_fig.plot(np.arange(1, time), history_targets[:, 2], "b", linestyle="dashdot")
th_fig.set_ylabel("th")
plt.show()
history_states = np.array(car.history_xs)
animdrawer = AnimDrawer([history_states, target])
animdrawer = AnimDrawer([history_states, history_targets])
animdrawer.draw_anim()
if __name__ == "__main__":