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

Merge pull request #6 from Shunichi09/develop 

Add: TwoWheeledTrack Env
This commit is contained in:
Shunichi09 2020-05-02 21:11:37 +09:00 committed by GitHub
parent 1e768f75e1 dd8880cce7
commit 014a31296e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
23 changed files with 732 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
PythonLinearNonlinearControl/common/utils.py
View File

@ -1 +1,44 @@
import numpy as np
def rotate_pos(pos, angle):
""" Transformation the coordinate in the angle
Args:
pos (numpy.ndarray): local state, shape(data_size, 2)
angle (float): rotate angle, in radians
Returns:
rotated_pos (numpy.ndarray): shape(data_size, 2)
"""
rot_mat = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
return np.dot(pos, rot_mat.T)
def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi):
""" Check angle range and correct the range
Args:
angle (numpy.ndarray): in radians
min_angle (float): maximum of range in radians, default -pi
max_angle (float): minimum of range in radians, default pi
Returns:
fitted_angle (numpy.ndarray): range angle in radians
"""
if max_angle < min_angle:
raise ValueError("max angle must be greater than min angle")
if (max_angle - min_angle) < 2.0 * np.pi:
raise ValueError("difference between max_angle \
and min_angle must be greater than 2.0 * pi")
output = np.array(angles)
output_shape = output.shape
output = output.flatten()
output -= min_angle
output %= 2 * np.pi
output += 2 * np.pi
output %= 2 * np.pi
output += min_angle
output = np.minimum(max_angle, np.maximum(min_angle, output))
return output.reshape(output_shape)

21
PythonLinearNonlinearControl/configs/cartpole.py
View File

@ -3,6 +3,7 @@ import numpy as np
class CartPoleConfigModule():
# parameters
ENV_NAME = "CartPole-v0"
PLANNER_TYPE = "Const"
TYPE = "Nonlinear"
TASK_HORIZON = 500
PRED_LEN = 50
@ -10,9 +11,9 @@ class CartPoleConfigModule():
INPUT_SIZE = 1
DT = 0.02
# cost parameters
R = np.diag([1.]) # 0.01 is worked for MPPI and CEM and MPPIWilliams
R = np.diag([0.01]) # 0.01 is worked for MPPI and CEM and MPPIWilliams
# 1. is worked for iLQR
Terminal_Weight = 1.
TERMINAL_WEIGHT = 1.
Q = None
Sf = None
# bounds
@ -23,6 +24,7 @@ class CartPoleConfigModule():
MC = 1.
L = 0.5
G = 9.81
CART_SIZE = (0.15, 0.1)
def __init__(self):
"""
@ -76,6 +78,7 @@ class CartPoleConfigModule():
@staticmethod
def input_cost_fn(u):
""" input cost functions
Args:
u (numpy.ndarray): input, shape(pred_len, input_size)
or shape(pop_size, pred_len, input_size)
@ -88,6 +91,7 @@ class CartPoleConfigModule():
@staticmethod
def state_cost_fn(x, g_x):
""" state cost function
Args:
x (numpy.ndarray): state, shape(pred_len, state_size)
or shape(pop_size, pred_len, state_size)
@ -118,6 +122,7 @@ class CartPoleConfigModule():
@staticmethod
def terminal_state_cost_fn(terminal_x, terminal_g_x):
"""
Args:
terminal_x (numpy.ndarray): terminal state,
shape(state_size, ) or shape(pop_size, state_size)
@ -133,13 +138,13 @@ class CartPoleConfigModule():
+ 12. * ((np.cos(terminal_x[:, 2]) + 1.)**2) \
+ 0.1 * (terminal_x[:, 1]**2) \
+ 0.1 * (terminal_x[:, 3]**2))[:, np.newaxis] \
* CartPoleConfigModule.Terminal_Weight
* CartPoleConfigModule.TERMINAL_WEIGHT
return (6. * (terminal_x[0]**2) \
+ 12. * ((np.cos(terminal_x[2]) + 1.)**2) \
+ 0.1 * (terminal_x[1]**2) \
+ 0.1 * (terminal_x[3]**2)) \
* CartPoleConfigModule.Terminal_Weight
* CartPoleConfigModule.TERMINAL_WEIGHT
@staticmethod
def gradient_cost_fn_with_state(x, g_x, terminal=False):
@ -168,7 +173,7 @@ class CartPoleConfigModule():
cost_dx3 = 0.2 * x[3]
cost_dx = np.array([[cost_dx0, cost_dx1, cost_dx2, cost_dx3]])
return cost_dx * CartPoleConfigModule.Terminal_Weight
return cost_dx * CartPoleConfigModule.TERMINAL_WEIGHT
@staticmethod
def gradient_cost_fn_with_input(x, u):
@ -177,7 +182,6 @@ class CartPoleConfigModule():
Args:
x (numpy.ndarray): state, shape(pred_len, state_size)
u (numpy.ndarray): goal state, shape(pred_len, input_size)
Returns:
l_u (numpy.ndarray): gradient of cost, shape(pred_len, input_size)
"""
@ -190,7 +194,6 @@ class CartPoleConfigModule():
Args:
x (numpy.ndarray): state, shape(pred_len, state_size)
g_x (numpy.ndarray): goal state, shape(pred_len, state_size)
Returns:
l_xx (numpy.ndarray): gradient of cost,
shape(pred_len, state_size, state_size) or
@ -220,7 +223,7 @@ class CartPoleConfigModule():
* -np.cos(x[2])
hessian[3, 3] = 0.2
return hessian[np.newaxis, :, :] * CartPoleConfigModule.Terminal_Weight
return hessian[np.newaxis, :, :] * CartPoleConfigModule.TERMINAL_WEIGHT
@staticmethod
def hessian_cost_fn_with_input(x, u):
@ -229,7 +232,6 @@ class CartPoleConfigModule():
Args:
x (numpy.ndarray): state, shape(pred_len, state_size)
u (numpy.ndarray): goal state, shape(pred_len, input_size)
Returns:
l_uu (numpy.ndarray): gradient of cost,
shape(pred_len, input_size, input_size)
@ -245,7 +247,6 @@ class CartPoleConfigModule():
Args:
x (numpy.ndarray): state, shape(pred_len, state_size)
u (numpy.ndarray): goal state, shape(pred_len, input_size)
Returns:
l_ux (numpy.ndarray): gradient of cost ,
shape(pred_len, input_size, state_size)

2
PythonLinearNonlinearControl/configs/make_configs.py
View File

@ -9,7 +9,7 @@ def make_config(args):
"""
if args.env == "FirstOrderLag":
return FirstOrderLagConfigModule()
elif args.env == "TwoWheeledConst" or args.env == "TwoWheeled":
elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack":
return TwoWheeledConfigModule()
elif args.env == "CartPole":
return CartPoleConfigModule()

56
PythonLinearNonlinearControl/configs/two_wheeled.py
View File

@ -1,21 +1,37 @@
import numpy as np
from matplotlib.axes import Axes
from ..plotters.plot_objs import square_with_angle, square
from ..common.utils import fit_angle_in_range
class TwoWheeledConfigModule():
# parameters
ENV_NAME = "TwoWheeled-v0"
TYPE = "Nonlinear"
N_AHEAD = 1
TASK_HORIZON = 1000
PRED_LEN = 20
STATE_SIZE = 3
INPUT_SIZE = 2
DT = 0.01
# cost parameters
# for Const goal
"""
R = np.diag([0.1, 0.1])
Q = np.diag([1., 1., 0.01])
Sf = np.diag([5., 5., 1.])
"""
# for track goal
R = np.diag([0.01, 0.01])
Q = np.diag([2.5, 2.5, 0.01])
Sf = np.diag([2.5, 2.5, 0.01])
# bounds
INPUT_LOWER_BOUND = np.array([-1.5, 3.14])
INPUT_LOWER_BOUND = np.array([-1.5, -3.14])
INPUT_UPPER_BOUND = np.array([1.5, 3.14])
# parameters
CAR_SIZE = 0.2
WHEELE_SIZE = (0.075, 0.015)
def __init__(self):
"""
@ -78,6 +94,27 @@ class TwoWheeledConfigModule():
"""
return (u**2) * np.diag(TwoWheeledConfigModule.R)
@staticmethod
def fit_diff_in_range(diff_x):
""" fit difference state in range(angle)
Args:
diff_x (numpy.ndarray):
shape(pop_size, pred_len, state_size) or
shape(pred_len, state_size) or
shape(state_size, )
Returns:
fitted_diff_x (numpy.ndarray): same shape as diff_x
"""
if len(diff_x.shape) == 3:
diff_x[:, :, -1] = fit_angle_in_range(diff_x[:, :, -1])
elif len(diff_x.shape) == 2:
diff_x[:, -1] = fit_angle_in_range(diff_x[:, -1])
elif len(diff_x.shape) == 1:
diff_x[-1] = fit_angle_in_range(diff_x[-1])
return diff_x
@staticmethod
def state_cost_fn(x, g_x):
""" state cost function
@ -90,7 +127,8 @@ class TwoWheeledConfigModule():
cost (numpy.ndarray): cost of state, shape(pred_len, state_size) or
shape(pop_size, pred_len, state_size)
"""
return ((x - g_x)**2) * np.diag(TwoWheeledConfigModule.Q)
diff = TwoWheeledConfigModule.fit_diff_in_range(x - g_x)
return ((diff)**2) * np.diag(TwoWheeledConfigModule.Q)
@staticmethod
def terminal_state_cost_fn(terminal_x, terminal_g_x):
@ -104,8 +142,10 @@ class TwoWheeledConfigModule():
cost (numpy.ndarray): cost of state, shape(pred_len, ) or
shape(pop_size, pred_len)
"""
return ((terminal_x - terminal_g_x)**2) \
* np.diag(TwoWheeledConfigModule.Sf)
terminal_diff = TwoWheeledConfigModule.fit_diff_in_range(terminal_x \
- terminal_g_x)
return ((terminal_diff)**2) * np.diag(TwoWheeledConfigModule.Sf)
@staticmethod
def gradient_cost_fn_with_state(x, g_x, terminal=False):
@ -119,10 +159,12 @@ class TwoWheeledConfigModule():
l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size)
or shape(1, state_size)
"""
if not terminal:
return 2. * (x - g_x) * np.diag(TwoWheeledConfigModule.Q)
diff = TwoWheeledConfigModule.fit_diff_in_range(x - g_x)
return (2. * (x - g_x) \
if not terminal:
return 2. * (diff) * np.diag(TwoWheeledConfigModule.Q)
return (2. * (diff) \
* np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :]
@staticmethod

2
PythonLinearNonlinearControl/controllers/make_controllers.py
View File

@ -22,3 +22,5 @@ def make_controller(args, config, model):
return iLQR(config, model)
elif args.controller_type == "DDP":
return DDP(config, model)
raise ValueError("No controller: {}".format(args.controller_type))

27
PythonLinearNonlinearControl/controllers/mpc.py
View File

@ -1,7 +1,8 @@
from logging import getLogger
import numpy as np
from cvxopt import matrix, solvers
from scipy.optimize import minimize
from scipy.optimize import LinearConstraint
from .controller import Controller
from ..envs.cost import calc_cost
@ -61,6 +62,7 @@ class LinearMPC(Controller):
self.F = None
self.f = None
self.setup()
self.prev_sol = np.zeros(self.input_size*self.pred_len)
# history
self.history_u = [np.zeros(self.input_size)]
@ -183,6 +185,22 @@ class LinearMPC(Controller):
ub = np.array(b).flatten()
# using cvxopt
def optimized_func(dt_us):
return (np.dot(dt_us, np.dot(H, dt_us.reshape(-1, 1))) \
- np.dot(G.T, dt_us.reshape(-1, 1)))[0]
# constraint
lb = np.array([-np.inf for _ in range(len(ub))]) # one side cons
cons = LinearConstraint(A, lb, ub)
# solve
opt_sol = minimize(optimized_func, self.prev_sol.flatten(),\
constraints=[cons])
opt_dt_us = opt_sol.x
""" using cvxopt ver,
if you want to solve more quick please use cvxopt instead of scipy
# make cvxpy problem formulation
P = 2*matrix(H)
q = matrix(-1 * G)
@ -190,12 +208,15 @@ class LinearMPC(Controller):
b = matrix(ub)
# solve the problem
opt_result = solvers.qp(P, q, G=A, h=b)
opt_dt_us = np.array(list(opt_result['x']))
opt_sol = solvers.qp(P, q, G=A, h=b)
opt_dt_us = np.array(list(opt_sol['x']))
"""
# to dt form
opt_dt_u_seq = np.cumsum(opt_dt_us.reshape(self.pred_len,\
self.input_size),
axis=0)
self.prev_sol = opt_dt_u_seq.copy()
opt_u_seq = opt_dt_u_seq + self.history_u[-1]

63
PythonLinearNonlinearControl/envs/cartpole.py
View File

@ -1,6 +1,8 @@
import numpy as np
from matplotlib.axes import Axes
from .env import Env
from ..plotters.plot_objs import square
class CartPoleEnv(Env):
""" Cartpole Environment
@ -24,6 +26,7 @@ class CartPoleEnv(Env):
"mc": 1.,
"l": 0.5,
"g": 9.81,
"cart_size": (0.15, 0.1),
}
super(CartPoleEnv, self).__init__(self.config)
@ -113,3 +116,63 @@ class CartPoleEnv(Env):
return next_x.flatten(), costs, \
self.step_count > self.config["max_step"], \
{"goal_state" : self.g_x}
def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
""" plot cartpole object function
Args:
to_plot (axis or imgs): plotted objects
i (int): frame count
history_x (numpy.ndarray): history of state, shape(iters, state)
history_g_x (numpy.ndarray): history of goal state,
shape(iters, state)
Returns:
None or imgs : imgs order is ["cart_img", "pole_img"]
"""
if isinstance(to_plot, Axes):
imgs = {} # create new imgs
imgs["cart"] = to_plot.plot([], [], c="k")[0]
imgs["pole"] = to_plot.plot([], [], c="k", linewidth=5)[0]
imgs["center"] = to_plot.plot([], [], marker="o", c="k",\
markersize=10)[0]
# centerline
to_plot.plot(np.linspace(-1., 1., num=50), np.zeros(50),\
c="k", linestyle="dashed")
# set axis
to_plot.set_xlim([-1., 1.])
to_plot.set_ylim([-0.55, 1.5])
return imgs
# set imgs
cart_x, cart_y, pole_x, pole_y = \
self._plot_cartpole(history_x[i])
to_plot["cart"].set_data(cart_x, cart_y)
to_plot["pole"].set_data(pole_x, pole_y)
to_plot["center"].set_data(history_x[i][0], 0.)
def _plot_cartpole(self, curr_x):
""" plot cartpole fucntions
Args:
curr_x (numpy.ndarray): current catpole state
Returns:
cart_x (numpy.ndarray): x data of cart
cart_y (numpy.ndarray): y data of cart
pole_x (numpy.ndarray): x data of pole
pole_y (numpy.ndarray): y data of pole
"""
# cart
cart_x, cart_y = square(curr_x[0], 0.,\
self.config["cart_size"], 0.)
# pole
pole_x = np.array([curr_x[0], curr_x[0] + self.config["l"] \
* np.cos(curr_x[2]-np.pi/2)])
pole_y = np.array([0., self.config["l"] \
* np.sin(curr_x[2]-np.pi/2)])
return cart_x, cart_y, pole_x, pole_y

5
PythonLinearNonlinearControl/envs/env.py
View File

@ -1,8 +1,9 @@
import numpy as np
class Env():
"""
""" Environments class
Attributes:
curr_x (numpy.ndarray): current state
history_x (list[numpy.ndarray]): historty of state, shape(step_count*state_size)
step_count (int): step count
@ -48,7 +49,7 @@ class Env():
"""
raise NotImplementedError("Implement step function")
def __str__(self):
def __repr__(self):
"""
"""
return self.config

5
PythonLinearNonlinearControl/envs/first_order_lag.py
View File

@ -113,3 +113,8 @@ class FirstOrderLagEnv(Env):
return next_x.flatten(), cost, \
self.step_count > self.config["max_step"], \
{"goal_state" : self.g_x}
def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
"""
"""
raise ValueError("FirstOrderLag does not have animation")

3
PythonLinearNonlinearControl/envs/make_envs.py
View File

@ -1,5 +1,6 @@
from .first_order_lag import FirstOrderLagEnv
from .two_wheeled import TwoWheeledConstEnv
from .two_wheeled import TwoWheeledTrackEnv
from .cartpole import CartPoleEnv
def make_env(args):
@ -8,6 +9,8 @@ def make_env(args):
return FirstOrderLagEnv()
elif args.env == "TwoWheeledConst":
return TwoWheeledConstEnv()
elif args.env == "TwoWheeledTrack":
return TwoWheeledTrackEnv()
elif args.env == "CartPole":
return CartPoleEnv()

286
PythonLinearNonlinearControl/envs/two_wheeled.py
View File

@ -1,6 +1,9 @@
import numpy as np
from matplotlib.axes import Axes
import matplotlib.pyplot as plt
from .env import Env
from ..plotters.plot_objs import circle_with_angle, square, circle
def step_two_wheeled_env(curr_x, u, dt, method="Oylar"):
""" step two wheeled enviroment
@ -34,9 +37,11 @@ class TwoWheeledConstEnv(Env):
self.config = {"state_size" : 3,\
"input_size" : 2,\
"dt" : 0.01,\
"max_step" : 1000,\
"input_lower_bound": [-1.5, -3.14],\
"input_upper_bound": [1.5, 3.14],
"max_step" : 500,\
"input_lower_bound": (-1.5, -3.14),\
"input_upper_bound": (1.5, 3.14),\
"car_size": 0.2,\
"wheel_size": (0.075, 0.015)
}
super(TwoWheeledConstEnv, self).__init__(self.config)
@ -100,3 +105,278 @@ class TwoWheeledConstEnv(Env):
return next_x.flatten(), costs, \
self.step_count > self.config["max_step"], \
{"goal_state" : self.g_x}
def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
""" plot cartpole object function
Args:
to_plot (axis or imgs): plotted objects
i (int): frame count
history_x (numpy.ndarray): history of state, shape(iters, state)
history_g_x (numpy.ndarray): history of goal state,
shape(iters, state)
Returns:
None or imgs : imgs order is ["cart_img", "pole_img"]
"""
if isinstance(to_plot, Axes):
imgs = {} # create new imgs
imgs["car"] = to_plot.plot([], [], c="k")[0]
imgs["car_angle"] = to_plot.plot([], [], c="k")[0]
imgs["left_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
imgs["right_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
imgs["goal"] = to_plot.plot([], [], marker="*",
c="b", markersize=10)[0]
imgs["traj"] = to_plot.plot([], [], c="b", linestyle="dashed")[0]
# set axis
to_plot.set_xlim([-1., 3.])
to_plot.set_ylim([-1., 3.])
return imgs
# set imgs
# car imgs
car_x, car_y, car_angle_x, car_angle_y, \
left_tire_x, left_tire_y, right_tire_x, right_tire_y = \
self._plot_car(history_x[i])
to_plot["car"].set_data(car_x, car_y)
to_plot["car_angle"].set_data(car_angle_x, car_angle_y)
to_plot["left_tire"].set_data(left_tire_x, left_tire_y,)
to_plot["right_tire"].set_data(right_tire_x, right_tire_y)
# goal and trajs
to_plot["goal"].set_data(history_g_x[i, 0], history_g_x[i, 1])
to_plot["traj"].set_data(history_x[:i, 0], history_x[:i, 1])
def _plot_car(self, curr_x):
""" plot car fucntions
"""
# cart
car_x, car_y, car_angle_x, car_angle_y = \
circle_with_angle(curr_x[0], curr_x[1],
self.config["car_size"], curr_x[2])
# left tire
center_x = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.cos(curr_x[2]-np.pi/2.) + curr_x[0]
center_y = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]-np.pi/2.) + curr_x[1]
left_tire_x, left_tire_y = \
square(center_x, center_y,
self.config["wheel_size"], curr_x[2])
# right tire
center_x = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.cos(curr_x[2]+np.pi/2.) + curr_x[0]
center_y = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]+np.pi/2.) + curr_x[1]
right_tire_x, right_tire_y = \
square(center_x, center_y,
self.config["wheel_size"], curr_x[2])
return car_x, car_y, car_angle_x, car_angle_y,\
left_tire_x, left_tire_y,\
right_tire_x, right_tire_y
class TwoWheeledTrackEnv(Env):
""" Two wheeled robot with constant goal Env
"""
def __init__(self):
"""
"""
self.config = {"state_size" : 3,\
"input_size" : 2,\
"dt" : 0.01,\
"max_step" : 1000,\
"input_lower_bound": (-1.5, -3.14),\
"input_upper_bound": (1.5, 3.14),\
"car_size": 0.2,\
"wheel_size": (0.075, 0.015)
}
super(TwoWheeledTrackEnv, self).__init__(self.config)
@staticmethod
def make_road(linelength=3., circle_radius=1.):
""" make track
Returns:
road (numpy.ndarray): road info, shape(n_point, 3) x, y, angle
"""
# line
# not include start points
line = np.linspace(-1.5, 1.5, num=51, endpoint=False)[1:]
line_1 = np.stack((line, np.zeros(50)), axis=1)
line_2 = np.stack((line[::-1], np.zeros(50)+circle_radius*2.), axis=1)
# circle
circle_1_x, circle_1_y = circle(linelength/2., circle_radius,
circle_radius, start=-np.pi/2., end=np.pi/2., n_point=50)
circle_1 = np.stack((circle_1_x , circle_1_y), axis=1)
circle_2_x, circle_2_y = circle(-linelength/2., circle_radius,
circle_radius, start=np.pi/2., end=3*np.pi/2., n_point=50)
circle_2 = np.stack((circle_2_x , circle_2_y), axis=1)
road_pos = np.concatenate((line_1, circle_1, line_2, circle_2), axis=0)
# calc road angle
road_diff = road_pos[1:] - road_pos[:-1]
road_angle = np.arctan2(road_diff[:, 1], road_diff[:, 0])
road_angle = np.concatenate((np.zeros(1), road_angle))
road = np.concatenate((road_pos, road_angle[:, np.newaxis]), axis=1)
return np.tile(road, (3, 1))
def reset(self, init_x=None):
""" reset state
Returns:
init_x (numpy.ndarray): initial state, shape(state_size, )
info (dict): information
"""
self.step_count = 0
self.curr_x = np.zeros(self.config["state_size"])
if init_x is not None:
self.curr_x = init_x
# goal
self.g_traj = self.make_road()
# clear memory
self.history_x = []
self.history_g_x = []
return self.curr_x, {"goal_state": self.g_traj}
def step(self, u):
""" step environments
Args:
u (numpy.ndarray) : input, shape(input_size, )
Returns:
next_x (numpy.ndarray): next state, shape(state_size, )
cost (float): costs
done (bool): end the simulation or not
info (dict): information
"""
# clip action
u = np.clip(u,
self.config["input_lower_bound"],
self.config["input_upper_bound"])
# step
next_x = step_two_wheeled_env(self.curr_x, u, self.config["dt"])
costs = 0.
costs += 0.1 * np.sum(u**2)
costs += np.min(np.linalg.norm(self.curr_x - self.g_traj, axis=1))
# save history
self.history_x.append(next_x.flatten())
# update
self.curr_x = next_x.flatten()
# update costs
self.step_count += 1
return next_x.flatten(), costs, \
self.step_count > self.config["max_step"], \
{"goal_state" : self.g_traj}
def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
""" plot cartpole object function
Args:
to_plot (axis or imgs): plotted objects
i (int): frame count
history_x (numpy.ndarray): history of state, shape(iters, state)
history_g_x (numpy.ndarray): history of goal state,
shape(iters, state)
Returns:
None or imgs : imgs order is ["cart_img", "pole_img"]
"""
if isinstance(to_plot, Axes):
imgs = {} # create new imgs
imgs["car"] = to_plot.plot([], [], c="k")[0]
imgs["car_angle"] = to_plot.plot([], [], c="k")[0]
imgs["left_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
imgs["right_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
imgs["goal"] = to_plot.plot([], [], marker="*",
c="b", markersize=10)[0]
imgs["traj"] = to_plot.plot([], [], c="b", linestyle="dashed")[0]
# set axis
to_plot.set_xlim([-3., 3.])
to_plot.set_ylim([-1., 3.])
# plot road
to_plot.plot(self.g_traj[:, 0], self.g_traj[:, 1],
c="k", linestyle="dashed")
return imgs
# set imgs
# car imgs
car_x, car_y, car_angle_x, car_angle_y, \
left_tire_x, left_tire_y, right_tire_x, right_tire_y = \
self._plot_car(history_x[i])
to_plot["car"].set_data(car_x, car_y)
to_plot["car_angle"].set_data(car_angle_x, car_angle_y)
to_plot["left_tire"].set_data(left_tire_x, left_tire_y,)
to_plot["right_tire"].set_data(right_tire_x, right_tire_y)
# goal and trajs
to_plot["goal"].set_data(history_g_x[i, 0], history_g_x[i, 1])
to_plot["traj"].set_data(history_x[:i, 0], history_x[:i, 1])
def _plot_car(self, curr_x):
""" plot car fucntions
"""
# cart
car_x, car_y, car_angle_x, car_angle_y = \
circle_with_angle(curr_x[0], curr_x[1],
self.config["car_size"], curr_x[2])
# left tire
center_x = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.cos(curr_x[2]-np.pi/2.) + curr_x[0]
center_y = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]-np.pi/2.) + curr_x[1]
left_tire_x, left_tire_y = \
square(center_x, center_y,
self.config["wheel_size"], curr_x[2])
# right tire
center_x = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.cos(curr_x[2]+np.pi/2.) + curr_x[0]
center_y = (self.config["car_size"] \
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]+np.pi/2.) + curr_x[1]
right_tire_x, right_tire_y = \
square(center_x, center_y,
self.config["wheel_size"], curr_x[2])
return car_x, car_y, car_angle_x, car_angle_y,\
left_tire_x, left_tire_y,\
right_tire_x, right_tire_y

2
PythonLinearNonlinearControl/models/make_models.py
View File

@ -6,7 +6,7 @@ def make_model(args, config):
if args.env == "FirstOrderLag":
return FirstOrderLagModel(config)
elif args.env == "TwoWheeledConst" or args.env == "TwoWheeled":
elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack":
return TwoWheeledModel(config)
elif args.env == "CartPole":
return CartPoleModel(config)

39
PythonLinearNonlinearControl/planners/closest_point_planner.py Normal file
View File

@ -0,0 +1,39 @@
import numpy as np
from .planner import Planner
class ClosestPointPlanner(Planner):
""" This planner make goal state according to goal path
"""
def __init__(self, config):
"""
"""
super(ClosestPointPlanner, self).__init__()
self.pred_len = config.PRED_LEN
self.state_size = config.STATE_SIZE
self.n_ahead = config.N_AHEAD
def plan(self, curr_x, g_traj):
"""
Args:
curr_x (numpy.ndarray): current state, shape(state_size)
g_x (numpy.ndarray): goal state, shape(state_size),
this state should be obtained from env
Returns:
g_xs (numpy.ndarrya): goal state, shape(pred_len+1, state_size)
"""
min_idx = np.argmin(np.linalg.norm(curr_x[:-1] - g_traj[:, :-1],
axis=1))
start = (min_idx+self.n_ahead)
if start > len(g_traj):
start = len(g_traj)
end = min_idx+self.n_ahead+self.pred_len+1
if (min_idx+self.n_ahead+self.pred_len+1) > len(g_traj):
end = len(g_traj)
if abs(start - end) != self.pred_len + 1:
return np.tile(g_traj[-1], (self.pred_len+1, 1))
return g_traj[start:end]

2
PythonLinearNonlinearControl/planners/const_planner.py
View File

@ -1,7 +1,7 @@
import numpy as np
from .planner import Planner
class ConstantPlanner():
class ConstantPlanner(Planner):
""" This planner make constant goal state
"""
def __init__(self, config):

9
PythonLinearNonlinearControl/planners/make_planners.py
View File

@ -1,8 +1,15 @@
from .const_planner import ConstantPlanner
from .closest_point_planner import ClosestPointPlanner
def make_planner(args, config):
if args.planner_type == "const":
if args.env == "FirstOrderLag":
return ConstantPlanner(config)
elif args.env == "TwoWheeledConst":
return ConstantPlanner(config)
elif args.env == "TwoWheeledTrack":
return ClosestPointPlanner(config)
elif args.env == "CartPole":
return ConstantPlanner(config)
raise NotImplementedError("There is not {} Planner".format(args.planner_type))

74
PythonLinearNonlinearControl/plotters/animator.py Normal file
View File

@ -0,0 +1,74 @@
import os
from logging import getLogger
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
logger = getLogger(__name__)
class Animator():
""" animation class
"""
def __init__(self, args, env):
"""
"""
self.env_name = args.env
self.result_dir = args.result_dir
self.controller_type = args.controller_type
self.interval = env.config["dt"] * 1000. # to ms
self.plot_func = env.plot_func
self.imgs = []
def _setup(self):
""" set up figure of animation
"""
# make fig
self.anim_fig = plt.figure()
# axis
self.axis = self.anim_fig.add_subplot(111)
self.axis.set_aspect('equal', adjustable='box')
self.imgs = self.plot_func(self.axis)
def _update_img(self, i, history_x, history_g_x):
""" update animation
Args:
i (int): frame count
history_x (numpy.ndarray): history of state,
shape(iters, state_size)
history_g (numpy.ndarray): history of goal state,
shape(iters, input_size)
"""
self.plot_func(self.imgs, i, history_x, history_g_x)
def draw(self, history_x, history_g_x=None):
"""draw the animation and save
Args:
history_x (numpy.ndarray): history of state,
shape(iters, state_size)
history_g (numpy.ndarray): history of goal state,
shape(iters, input_size)
Returns:
None
"""
# set up animation figures
self._setup()
_update_img = lambda i: self._update_img(i, history_x, history_g_x)
# call funcanimation
animation = FuncAnimation(
self.anim_fig,
_update_img, interval=self.interval, frames=len(history_x)-1)
# save animation
path = os.path.join(self.result_dir, self.controller_type,
"animation-" + self.env_name + ".mp4")
logger.info("Saved Animation to {} ...".format(path))
animation.save(path, writer="ffmpeg")

99
PythonLinearNonlinearControl/plotters/plot_objs.py Normal file
View File

@ -0,0 +1,99 @@
import os
import numpy as np
import matplotlib.pyplot as plt
from ..common.utils import rotate_pos
def circle(center_x, center_y, radius, start=0., end=2*np.pi, n_point=100):
""" Create circle matrix
Args:
center_x (float): the center x position of the circle
center_y (float): the center y position of the circle
radius (float): in meters
start (float): start angle
end (float): end angle
Returns:
circle x : numpy.ndarray
circle y : numpy.ndarray
"""
diff = end - start
circle_xs = []
circle_ys = []
for i in range(n_point + 1):
circle_xs.append(center_x + radius * np.cos(i*diff/n_point + start))
circle_ys.append(center_y + radius * np.sin(i*diff/n_point + start))
return np.array(circle_xs), np.array(circle_ys)
def circle_with_angle(center_x, center_y, radius, angle):
""" Create circle matrix with angle line matrix
Args:
center_x (float): the center x position of the circle
center_y (float): the center y position of the circle
radius (float): in meters
angle (float): in radians
Returns:
circle_x (numpy.ndarray): x data of circle
circle_y (numpy.ndarray): y data of circle
angle_x (numpy.ndarray): x data of circle angle
angle_y (numpy.ndarray): y data of circle angle
"""
circle_x, circle_y = circle(center_x, center_y, radius)
angle_x = np.array([center_x, center_x + np.cos(angle) * radius])
angle_y = np.array([center_y, center_y + np.sin(angle) * radius])
return circle_x, circle_y, angle_x, angle_y
def square(center_x, center_y, shape, angle):
""" Create square
Args:
center_x (float): the center x position of the square
center_y (float): the center y position of the square
shape (tuple): the square's shape(width/2, height/2)
angle (float): in radians
Returns:
square_x (numpy.ndarray): shape(5, ), counterclockwise from right-up
square_y (numpy.ndarray): shape(5, ), counterclockwise from right-up
"""
# start with the up right points
# create point in counterclockwise, local
square_xy = np.array([[shape[0], shape[1]],
[-shape[0], shape[1]],
[-shape[0], -shape[1]],
[shape[0], -shape[1]],
[shape[0], shape[1]]])
# translate position to world
# rotation
trans_points = rotate_pos(square_xy, angle)
# translation
trans_points += np.array([center_x, center_y])
return trans_points[:, 0], trans_points[:, 1]
def square_with_angle(center_x, center_y, shape, angle):
""" Create square with angle line
Args:
center_x (float): the center x position of the square
center_y (float): the center y position of the square
shape (tuple): the square's shape(width/2, height/2)
angle (float): in radians
Returns:
square_x (numpy.ndarray): shape(5, ), counterclockwise from right-up
square_y (numpy.ndarray): shape(5, ), counterclockwise from right-up
angle_x (numpy.ndarray): x data of square angle
angle_y (numpy.ndarray): y data of square angle
"""
square_x, square_y = square(center_x, center_y, shape, angle)
angle_x = np.array([center_x, center_x + np.cos(angle) * shape[0]])
angle_y = np.array([center_y, center_y + np.sin(angle) * shape[1]])
return square_x, square_y, angle_x, angle_y

2
PythonLinearNonlinearControl/runners/runner.py
View File

@ -29,7 +29,7 @@ class ExpRunner():
while not done:
logger.debug("Step = {}".format(step_count))
# plan
g_xs = planner.plan(curr_x, g_x=info["goal_state"])
g_xs = planner.plan(curr_x, info["goal_state"])
# obtain sol
u = controller.obtain_sol(curr_x, g_xs)

16
README.md
View File

@ -5,8 +5,11 @@
# PythonLinearNonLinearControl
PythonLinearNonLinearControl is a library implementing the linear and nonlinear control theories in python.
Due to use only basic libralies (scipy, numpy), this library is also easily to extend for your own situations.
<img src="assets/concept.png" width="500">
<div><img src="assets/concept.png" width="500"/></div>
<img src="assets/cartpole.gif" width="350"> <img src="assets/twowheeledconst.gif" width="350"> <img src="assets/twowheeledtrack.gif" width="350">
# Algorithms
@ -24,7 +27,6 @@ PythonLinearNonLinearControl is a library implementing the linear and nonlinear
| Constrained Nonlinear Model Predictive Control Newton (NMPC-Newton) | x | ✓ | x | x | x |
"Need Gradient" means that you have to implement the gradient of the model or the gradient of hamiltonian.
This library is also easily to extend for your own situations.
Following algorithms are implemented in PythonLinearNonlinearControl
@ -61,11 +63,13 @@ Following algorithms are implemented in PythonLinearNonlinearControl
# Environments
There are 4 example environments, "FirstOrderLag", "TwoWheeledConst", "TwoWheeledTrack" and "Cartpole".
| Name | Linear | Nonlinear | State Size | Input size |
|:----------|:---------------:|:----------------:|:----------------:|:----------------:|
| First Order Lag System | ✓ | x | 4 | 2 |
| Two wheeled System (Constant Goal) | x | ✓ | 3 | 2 |
| Two wheeled System (Moving Goal) (Coming soon) | x | ✓ | 3 | 2 |
| Two wheeled System (Moving Goal) | x | ✓ | 3 | 2 |
| Cartpole (Swing up) | x | ✓ | 4 | 1 |
All states and inputs of environments are continuous.
@ -104,7 +108,7 @@ pip install -e .
You can run the experiments as follows:
```
python scripts/simple_run.py --env first-order_lag --controller CEM
python scripts/simple_run.py --env FirstOrderLag --controller CEM
```
**figures and animations are saved in the ./result folder.**
@ -147,9 +151,9 @@ Coming soon !!
# Requirements
- numpy
- matplotlib
- cvxopt
- scipy
- matplotlib (for figures and animations)
- ffmpeg (for animations)
# License

BIN
assets/cartpole.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 58 KiB

BIN
assets/twowheeledconst.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 59 KiB

BIN
assets/twowheeledtrack.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 143 KiB

11
scripts/simple_run.py
View File

@ -9,6 +9,7 @@ from PythonLinearNonlinearControl.envs.make_envs import make_env
from PythonLinearNonlinearControl.runners.make_runners import make_runner
from PythonLinearNonlinearControl.plotters.plot_func import plot_results, \
save_plot_data
from PythonLinearNonlinearControl.plotters.animator import Animator
def run(args):
# logger
@ -39,12 +40,16 @@ def run(args):
plot_results(args, history_x, history_u, history_g=history_g)
save_plot_data(args, history_x, history_u, history_g=history_g)
if args.save_anim:
animator = Animator(args, env)
animator.draw(history_x, history_g)
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--controller_type", type=str, default="DDP")
parser.add_argument("--planner_type", type=str, default="const")
parser.add_argument("--env", type=str, default="CartPole")
parser.add_argument("--controller_type", type=str, default="CEM")
parser.add_argument("--env", type=str, default="TwoWheeledTrack")
parser.add_argument("--save_anim", type=bool_flag, default=1)
parser.add_argument("--result_dir", type=str, default="./result")
args = parser.parse_args()