diff --git a/PythonLinearNonlinearControl/common/utils.py b/PythonLinearNonlinearControl/common/utils.py
index a22b22b..8010737 100644
--- a/PythonLinearNonlinearControl/common/utils.py
+++ b/PythonLinearNonlinearControl/common/utils.py
@@ -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)
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/configs/cartpole.py b/PythonLinearNonlinearControl/configs/cartpole.py
index fff5cf5..bbcf99b 100644
--- a/PythonLinearNonlinearControl/configs/cartpole.py
+++ b/PythonLinearNonlinearControl/configs/cartpole.py
@@ -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)
diff --git a/PythonLinearNonlinearControl/configs/make_configs.py b/PythonLinearNonlinearControl/configs/make_configs.py
index 984df94..a48aedc 100644
--- a/PythonLinearNonlinearControl/configs/make_configs.py
+++ b/PythonLinearNonlinearControl/configs/make_configs.py
@@ -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()
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/configs/two_wheeled.py b/PythonLinearNonlinearControl/configs/two_wheeled.py
index 8b79c87..93de72a 100644
--- a/PythonLinearNonlinearControl/configs/two_wheeled.py
+++ b/PythonLinearNonlinearControl/configs/two_wheeled.py
@@ -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
diff --git a/PythonLinearNonlinearControl/controllers/make_controllers.py b/PythonLinearNonlinearControl/controllers/make_controllers.py
index 74d048c..d5ec8df 100644
--- a/PythonLinearNonlinearControl/controllers/make_controllers.py
+++ b/PythonLinearNonlinearControl/controllers/make_controllers.py
@@ -21,4 +21,6 @@ def make_controller(args, config, model):
elif args.controller_type == "iLQR":
return iLQR(config, model)
elif args.controller_type == "DDP":
- return DDP(config, model)
\ No newline at end of file
+ return DDP(config, model)
+
+ raise ValueError("No controller: {}".format(args.controller_type))
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/controllers/mpc.py b/PythonLinearNonlinearControl/controllers/mpc.py
index 766973c..ddf5840 100644
--- a/PythonLinearNonlinearControl/controllers/mpc.py
+++ b/PythonLinearNonlinearControl/controllers/mpc.py
@@ -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]
diff --git a/PythonLinearNonlinearControl/envs/cartpole.py b/PythonLinearNonlinearControl/envs/cartpole.py
index fb190e6..fd70d47 100644
--- a/PythonLinearNonlinearControl/envs/cartpole.py
+++ b/PythonLinearNonlinearControl/envs/cartpole.py
@@ -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)
@@ -112,4 +115,64 @@ class CartPoleEnv(Env):
return next_x.flatten(), costs, \
self.step_count > self.config["max_step"], \
- {"goal_state" : self.g_x}
\ No newline at end of file
+ {"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
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/envs/env.py b/PythonLinearNonlinearControl/envs/env.py
index c8ace4b..4f63903 100644
--- a/PythonLinearNonlinearControl/envs/env.py
+++ b/PythonLinearNonlinearControl/envs/env.py
@@ -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
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/envs/first_order_lag.py b/PythonLinearNonlinearControl/envs/first_order_lag.py
index 7da07b8..5597a07 100644
--- a/PythonLinearNonlinearControl/envs/first_order_lag.py
+++ b/PythonLinearNonlinearControl/envs/first_order_lag.py
@@ -112,4 +112,9 @@ class FirstOrderLagEnv(Env):
return next_x.flatten(), cost, \
self.step_count > self.config["max_step"], \
- {"goal_state" : self.g_x}
\ No newline at end of file
+ {"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")
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/envs/make_envs.py b/PythonLinearNonlinearControl/envs/make_envs.py
index 4b1adf7..253f3ed 100644
--- a/PythonLinearNonlinearControl/envs/make_envs.py
+++ b/PythonLinearNonlinearControl/envs/make_envs.py
@@ -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()
diff --git a/PythonLinearNonlinearControl/envs/two_wheeled.py b/PythonLinearNonlinearControl/envs/two_wheeled.py
index 8be0d36..43f5f0c 100644
--- a/PythonLinearNonlinearControl/envs/two_wheeled.py
+++ b/PythonLinearNonlinearControl/envs/two_wheeled.py
@@ -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)
@@ -99,4 +104,279 @@ class TwoWheeledConstEnv(Env):
return next_x.flatten(), costs, \
self.step_count > self.config["max_step"], \
- {"goal_state" : self.g_x}
\ No newline at end of file
+ {"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
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/models/make_models.py b/PythonLinearNonlinearControl/models/make_models.py
index fcb29ae..fb628ad 100644
--- a/PythonLinearNonlinearControl/models/make_models.py
+++ b/PythonLinearNonlinearControl/models/make_models.py
@@ -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)
diff --git a/PythonLinearNonlinearControl/planners/closest_point_planner.py b/PythonLinearNonlinearControl/planners/closest_point_planner.py
new file mode 100644
index 0000000..291aff3
--- /dev/null
+++ b/PythonLinearNonlinearControl/planners/closest_point_planner.py
@@ -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]
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/planners/const_planner.py b/PythonLinearNonlinearControl/planners/const_planner.py
index 5806c62..a0569a2 100644
--- a/PythonLinearNonlinearControl/planners/const_planner.py
+++ b/PythonLinearNonlinearControl/planners/const_planner.py
@@ -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):
diff --git a/PythonLinearNonlinearControl/planners/make_planners.py b/PythonLinearNonlinearControl/planners/make_planners.py
index 9da7f31..9f18e51 100644
--- a/PythonLinearNonlinearControl/planners/make_planners.py
+++ b/PythonLinearNonlinearControl/planners/make_planners.py
@@ -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))
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/plotters/animator.py b/PythonLinearNonlinearControl/plotters/animator.py
new file mode 100644
index 0000000..8446229
--- /dev/null
+++ b/PythonLinearNonlinearControl/plotters/animator.py
@@ -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")
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/plotters/plot_objs.py b/PythonLinearNonlinearControl/plotters/plot_objs.py
new file mode 100644
index 0000000..911cb5f
--- /dev/null
+++ b/PythonLinearNonlinearControl/plotters/plot_objs.py
@@ -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
\ No newline at end of file
diff --git a/PythonLinearNonlinearControl/runners/runner.py b/PythonLinearNonlinearControl/runners/runner.py
index 0c55bb9..4ef0c6b 100644
--- a/PythonLinearNonlinearControl/runners/runner.py
+++ b/PythonLinearNonlinearControl/runners/runner.py
@@ -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)
diff --git a/README.md b/README.md
index 7bb14b1..6729e77 100644
--- a/README.md
+++ b/README.md
@@ -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.
-
+
+
+
# 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
diff --git a/assets/cartpole.gif b/assets/cartpole.gif
new file mode 100644
index 0000000..0b40fb4
Binary files /dev/null and b/assets/cartpole.gif differ
diff --git a/assets/twowheeledconst.gif b/assets/twowheeledconst.gif
new file mode 100644
index 0000000..cedd5da
Binary files /dev/null and b/assets/twowheeledconst.gif differ
diff --git a/assets/twowheeledtrack.gif b/assets/twowheeledtrack.gif
new file mode 100644
index 0000000..0575f43
Binary files /dev/null and b/assets/twowheeledtrack.gif differ
diff --git a/scripts/simple_run.py b/scripts/simple_run.py
index f1d70fd..4181f8f 100644
--- a/scripts/simple_run.py
+++ b/scripts/simple_run.py
@@ -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()