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

Fix format

This commit is contained in:
Shunichi09 2021-02-13 17:18:42 +09:00
parent f49ed382a4
commit d64a799eda
37 changed files with 648 additions and 563 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
PythonLinearNonlinearControl/common/utils.py
View File

@ -1,5 +1,6 @@
import numpy as np
def rotate_pos(pos, angle):
""" Transformation the coordinate in the angle
@ -14,6 +15,7 @@ def rotate_pos(pos, 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
@ -43,6 +45,7 @@ def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi):
output = np.minimum(max_angle, np.maximum(min_angle, output))
return output.reshape(output_shape)
def update_state_with_Runge_Kutta(state, u, functions, dt=0.01):
""" update state in Runge Kutta methods
Args:

27
PythonLinearNonlinearControl/configs/cartpole.py
View File

@ -1,5 +1,6 @@
import numpy as np
class CartPoleConfigModule():
# parameters
ENV_NAME = "CartPole-v0"
@ -103,15 +104,15 @@ class CartPoleConfigModule():
"""
if len(x.shape) > 2:
return (6. * (x[:, :, 0]**2) \
+ 12. * ((np.cos(x[:, :, 2]) + 1.)**2) \
+ 0.1 * (x[:, :, 1]**2) \
return (6. * (x[:, :, 0]**2)
+ 12. * ((np.cos(x[:, :, 2]) + 1.)**2)
+ 0.1 * (x[:, :, 1]**2)
+ 0.1 * (x[:, :, 3]**2))[:, :, np.newaxis]
elif len(x.shape) > 1:
return (6. * (x[:, 0]**2) \
+ 12. * ((np.cos(x[:, 2]) + 1.)**2) \
+ 0.1 * (x[:, 1]**2) \
return (6. * (x[:, 0]**2)
+ 12. * ((np.cos(x[:, 2]) + 1.)**2)
+ 0.1 * (x[:, 1]**2)
+ 0.1 * (x[:, 3]**2))[:, np.newaxis]
return 6. * (x[0]**2) \
@ -134,15 +135,15 @@ class CartPoleConfigModule():
"""
if len(terminal_x.shape) > 1:
return (6. * (terminal_x[:, 0]**2) \
+ 12. * ((np.cos(terminal_x[:, 2]) + 1.)**2) \
+ 0.1 * (terminal_x[:, 1]**2) \
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))[:, np.newaxis] \
* CartPoleConfigModule.TERMINAL_WEIGHT
return (6. * (terminal_x[0]**2) \
+ 12. * ((np.cos(terminal_x[2]) + 1.)**2) \
+ 0.1 * (terminal_x[1]**2) \
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
@ -163,7 +164,7 @@ class CartPoleConfigModule():
cost_dx1 = 0.2 * x[:, 1]
cost_dx2 = 24. * (1 + np.cos(x[:, 2])) * -np.sin(x[:, 2])
cost_dx3 = 0.2 * x[:, 3]
cost_dx = np.stack((cost_dx0, cost_dx1,\
cost_dx = np.stack((cost_dx0, cost_dx1,
cost_dx2, cost_dx3), axis=1)
return cost_dx

3
PythonLinearNonlinearControl/configs/first_order_lag.py
View File

@ -1,5 +1,6 @@
import numpy as np
class FirstOrderLagConfigModule():
# parameters
ENV_NAME = "FirstOrderLag-v0"
@ -128,7 +129,7 @@ class FirstOrderLagConfigModule():
if not terminal:
return 2. * (x - g_x) * np.diag(FirstOrderLagConfigModule.Q)
return (2. * (x - g_x) \
return (2. * (x - g_x)
* np.diag(FirstOrderLagConfigModule.Sf))[np.newaxis, :]
@staticmethod

1
PythonLinearNonlinearControl/configs/make_configs.py
View File

@ -2,6 +2,7 @@ from .first_order_lag import FirstOrderLagConfigModule
from .two_wheeled import TwoWheeledConfigModule
from .cartpole import CartPoleConfigModule
def make_config(args):
"""
Returns:

5
PythonLinearNonlinearControl/configs/two_wheeled.py
View File

@ -4,6 +4,7 @@ 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"
@ -142,7 +143,7 @@ class TwoWheeledConfigModule():
cost (numpy.ndarray): cost of state, shape(pred_len, ) or
shape(pop_size, pred_len)
"""
terminal_diff = TwoWheeledConfigModule.fit_diff_in_range(terminal_x \
terminal_diff = TwoWheeledConfigModule.fit_diff_in_range(terminal_x
- terminal_g_x)
return ((terminal_diff)**2) * np.diag(TwoWheeledConfigModule.Sf)
@ -164,7 +165,7 @@ class TwoWheeledConfigModule():
if not terminal:
return 2. * (diff) * np.diag(TwoWheeledConfigModule.Q)
return (2. * (diff) \
return (2. * (diff)
* np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :]
@staticmethod

6
PythonLinearNonlinearControl/controllers/cem.py
View File

@ -8,6 +8,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class CEM(Controller):
""" Cross Entropy Method for linear and nonlinear method
@ -19,6 +20,7 @@ class CEM(Controller):
using probabilistic dynamics models.
In Advances in Neural Information Processing Systems (pp. 4754-4765).
"""
def __init__(self, config, model):
super(CEM, self).__init__(config, model)
@ -50,7 +52,7 @@ class CEM(Controller):
self.input_cost_fn = config.input_cost_fn
# init mean
self.init_mean = np.tile((config.INPUT_UPPER_BOUND \
self.init_mean = np.tile((config.INPUT_UPPER_BOUND
+ config.INPUT_LOWER_BOUND) / 2.,
self.pred_len)
self.prev_sol = self.init_mean.copy()
@ -86,7 +88,7 @@ class CEM(Controller):
# make distribution
X = stats.truncnorm(-1, 1,
loc=np.zeros_like(mean),\
loc=np.zeros_like(mean),
scale=np.ones_like(mean))
while (opt_count < self.max_iters) and np.max(var) > self.epsilon:

10
PythonLinearNonlinearControl/controllers/controller.py
View File

@ -2,9 +2,11 @@ import numpy as np
from ..envs.cost import calc_cost
class Controller():
""" Controller class
"""
def __init__(self, config, model):
"""
"""
@ -49,7 +51,7 @@ class Controller():
# get particle cost
costs = calc_cost(pred_xs, samples, g_xs,
self.state_cost_fn, self.input_cost_fn, \
self.state_cost_fn, self.input_cost_fn,
self.terminal_state_cost_fn)
return costs
@ -58,10 +60,12 @@ class Controller():
def gradient_hamiltonian_x(x, u, lam):
""" gradient of hamitonian with respect to the state,
"""
raise NotImplementedError("Implement gradient of hamitonian with respect to the state")
raise NotImplementedError(
"Implement gradient of hamitonian with respect to the state")
@staticmethod
def gradient_hamiltonian_u(x, u, lam):
""" gradient of hamitonian with respect to the input
"""
raise NotImplementedError("Implement gradient of hamitonian with respect to the input")
raise NotImplementedError(
"Implement gradient of hamitonian with respect to the input")

6
PythonLinearNonlinearControl/controllers/ddp.py
View File

@ -8,6 +8,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class DDP(Controller):
""" Differential Dynamic Programming
@ -18,6 +19,7 @@ class DDP(Controller):
https://github.com/studywolf/control, and
https://github.com/anassinator/ilqr
"""
def __init__(self, config, model):
"""
"""
@ -98,7 +100,7 @@ class DDP(Controller):
try:
# backward
k, K = self.backward(f_x, f_u, f_xx, f_ux, f_uu, \
k, K = self.backward(f_x, f_u, f_xx, f_ux, f_uu,
l_x, l_xx, l_u, l_uu, l_ux)
# line search
@ -139,7 +141,7 @@ class DDP(Controller):
# increase regularization term.
self.delta = max(1.0, self.delta) * self.init_delta
self.mu = max(self.mu_min, self.mu * self.delta)
logger.debug("Update regularization term to {}"\
logger.debug("Update regularization term to {}"
.format(self.mu))
if self.mu >= self.mu_max:
logger.debug("Reach Max regularization term")

4
PythonLinearNonlinearControl/controllers/ilqr.py
View File

@ -8,6 +8,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class iLQR(Controller):
""" iterative Liner Quadratique Regulator
@ -16,6 +17,7 @@ class iLQR(Controller):
Intelligent Robots and Systems (pp. 4906-4913). and Study Wolf,
https://github.com/studywolf/control
"""
def __init__(self, config, model):
"""
"""
@ -130,7 +132,7 @@ class iLQR(Controller):
# increase regularization term.
self.delta = max(1.0, self.delta) * self.init_delta
self.mu = max(self.mu_min, self.mu * self.delta)
logger.debug("Update regularization term to {}"\
logger.debug("Update regularization term to {}"
.format(self.mu))
if self.mu >= self.mu_max:
logger.debug("Reach Max regularization term")

1
PythonLinearNonlinearControl/controllers/make_controllers.py
View File

@ -6,6 +6,7 @@ from .mppi_williams import MPPIWilliams
from .ilqr import iLQR
from .ddp import DDP
def make_controller(args, config, model):
if args.controller_type == "MPC":

10
PythonLinearNonlinearControl/controllers/mpc.py
View File

@ -9,6 +9,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class LinearMPC(Controller):
""" Model Predictive Controller for linear model
@ -21,6 +22,7 @@ class LinearMPC(Controller):
Ref:
Maciejowski, J. M. (2002). Predictive control: with constraints.
"""
def __init__(self, config, model):
"""
Args:
@ -114,7 +116,7 @@ class LinearMPC(Controller):
for i in range(self.pred_len - 1):
for j in range(self.input_size):
temp_F[j * 2: (j + 1) * 2,\
temp_F[j * 2: (j + 1) * 2,
((i+1) * self.input_size) + j] = np.array([1., -1.])
self.F = np.vstack((self.F, temp_F))
@ -187,14 +189,14 @@ class LinearMPC(Controller):
# using cvxopt
def optimized_func(dt_us):
return (np.dot(dt_us, np.dot(H, dt_us.reshape(-1, 1))) \
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(),\
opt_sol = minimize(optimized_func, self.prev_sol.flatten(),
constraints=[cons])
opt_dt_us = opt_sol.x
@ -213,7 +215,7 @@ class LinearMPC(Controller):
"""
# to dt form
opt_dt_u_seq = np.cumsum(opt_dt_us.reshape(self.pred_len,\
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()

8
PythonLinearNonlinearControl/controllers/mppi.py
View File

@ -8,6 +8,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class MPPI(Controller):
""" Model Predictive Path Integral for linear and nonlinear method
@ -18,6 +19,7 @@ class MPPI(Controller):
Deep Dynamics Models for Learning Dexterous Manipulation.
arXiv preprint arXiv:1909.11652.
"""
def __init__(self, config, model):
super(MPPI, self).__init__(config, model)
@ -47,7 +49,7 @@ class MPPI(Controller):
self.input_cost_fn = config.input_cost_fn
# init mean
self.prev_sol = np.tile((config.INPUT_UPPER_BOUND \
self.prev_sol = np.tile((config.INPUT_UPPER_BOUND
+ config.INPUT_LOWER_BOUND) / 2.,
self.pred_len)
self.prev_sol = self.prev_sol.reshape(self.pred_len, self.input_size)
@ -81,13 +83,13 @@ class MPPI(Controller):
for t in range(self.pred_len):
if t > 0:
noised_inputs[:, t, :] = self.beta \
* (self.prev_sol[t, :] \
* (self.prev_sol[t, :]
+ noise[:, t, :]) \
+ (1 - self.beta) \
* noised_inputs[:, t-1, :]
else:
noised_inputs[:, t, :] = self.beta \
* (self.prev_sol[t, :] \
* (self.prev_sol[t, :]
+ noise[:, t, :]) \
+ (1 - self.beta) \
* self.history_u[-1]

6
PythonLinearNonlinearControl/controllers/mppi_williams.py
View File

@ -8,6 +8,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class MPPIWilliams(Controller):
""" Model Predictive Path Integral for linear and nonlinear method
@ -19,6 +20,7 @@ class MPPIWilliams(Controller):
2017 IEEE International Conference on Robotics and Automation (ICRA),
Singapore, 2017, pp. 1714-1721.
"""
def __init__(self, config, model):
super(MPPIWilliams, self).__init__(config, model)
@ -47,7 +49,7 @@ class MPPIWilliams(Controller):
self.input_cost_fn = config.input_cost_fn
# init mean
self.prev_sol = np.tile((config.INPUT_UPPER_BOUND \
self.prev_sol = np.tile((config.INPUT_UPPER_BOUND
+ config.INPUT_LOWER_BOUND) / 2.,
self.pred_len)
self.prev_sol = self.prev_sol.reshape(self.pred_len, self.input_size)
@ -85,7 +87,7 @@ class MPPIWilliams(Controller):
# get particle cost
costs = calc_cost(pred_xs, samples, g_xs,
self.state_cost_fn, None, \
self.state_cost_fn, None,
self.terminal_state_cost_fn)
return costs

2
PythonLinearNonlinearControl/controllers/random.py
View File

@ -8,6 +8,7 @@ from ..envs.cost import calc_cost
logger = getLogger(__name__)
class RandomShooting(Controller):
""" Random Shooting Method for linear and nonlinear method
@ -19,6 +20,7 @@ class RandomShooting(Controller):
using probabilistic dynamics models.
In Advances in Neural Information Processing Systems (pp. 4754-4765).
"""
def __init__(self, config, model):
super(RandomShooting, self).__init__(config, model)

28
PythonLinearNonlinearControl/envs/cartpole.py
View File

@ -4,6 +4,7 @@ from matplotlib.axes import Axes
from .env import Env
from ..plotters.plot_objs import square
class CartPoleEnv(Env):
""" Cartpole Environment
@ -13,6 +14,7 @@ class CartPoleEnv(Env):
6-832-underactuated-robotics-spring-2009/readings/
MIT6_832s09_read_ch03.pdf
"""
def __init__(self):
"""
"""
@ -76,21 +78,21 @@ class CartPoleEnv(Env):
# x
d_x0 = self.curr_x[1]
# v_x
d_x1 = (u[0] + self.config["mp"] * np.sin(self.curr_x[2]) \
* (self.config["l"] * (self.curr_x[3]**2) \
d_x1 = (u[0] + self.config["mp"] * np.sin(self.curr_x[2])
* (self.config["l"] * (self.curr_x[3]**2)
+ self.config["g"] * np.cos(self.curr_x[2]))) \
/ (self.config["mc"] + self.config["mp"] \
/ (self.config["mc"] + self.config["mp"]
* (np.sin(self.curr_x[2])**2))
# theta
d_x2 = self.curr_x[3]
# v_theta
d_x3 = (-u[0] * np.cos(self.curr_x[2]) \
- self.config["mp"] * self.config["l"] * (self.curr_x[3]**2) \
* np.cos(self.curr_x[2]) * np.sin(self.curr_x[2]) \
- (self.config["mc"] + self.config["mp"]) * self.config["g"] \
d_x3 = (-u[0] * np.cos(self.curr_x[2])
- self.config["mp"] * self.config["l"] * (self.curr_x[3]**2)
* np.cos(self.curr_x[2]) * np.sin(self.curr_x[2])
- (self.config["mc"] + self.config["mp"]) * self.config["g"]
* np.sin(self.curr_x[2])) \
/ (self.config["l"] * (self.config["mc"] + self.config["mp"] \
/ (self.config["l"] * (self.config["mc"] + self.config["mp"]
* (np.sin(self.curr_x[2])**2)))
next_x = self.curr_x +\
@ -134,10 +136,10 @@ class CartPoleEnv(Env):
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",\
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),\
to_plot.plot(np.linspace(-1., 1., num=50), np.zeros(50),
c="k", linestyle="dashed")
# set axis
@ -166,13 +168,13 @@ class CartPoleEnv(Env):
pole_y (numpy.ndarray): y data of pole
"""
# cart
cart_x, cart_y = square(curr_x[0], 0.,\
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"] \
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"] \
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

4
PythonLinearNonlinearControl/envs/cost.py
View File

@ -4,6 +4,7 @@ import numpy as np
logger = getLogger(__name__)
def calc_cost(pred_xs, input_sample, g_xs,
state_cost_fn, input_cost_fn, terminal_state_cost_fn):
""" calculate the cost
@ -24,7 +25,8 @@ def calc_cost(pred_xs, input_sample, g_xs,
# state cost
state_cost = 0.
if state_cost_fn is not None:
state_pred_par_cost = state_cost_fn(pred_xs[:, 1:-1, :], g_xs[:, 1:-1, :])
state_pred_par_cost = state_cost_fn(
pred_xs[:, 1:-1, :], g_xs[:, 1:-1, :])
state_cost = np.sum(np.sum(state_pred_par_cost, axis=-1), axis=-1)
# terminal cost

2
PythonLinearNonlinearControl/envs/env.py
View File

@ -1,5 +1,6 @@
import numpy as np
class Env():
""" Environments class
Attributes:
@ -8,6 +9,7 @@ class Env():
history_x (list[numpy.ndarray]): historty of state, shape(step_count*state_size)
step_count (int): step count
"""
def __init__(self, config):
"""
"""

12
PythonLinearNonlinearControl/envs/first_order_lag.py
View File

@ -3,17 +3,19 @@ import scipy
from scipy import integrate
from .env import Env
class FirstOrderLagEnv(Env):
""" First Order Lag System Env
"""
def __init__(self, tau=0.63):
"""
"""
self.config = {"state_size" : 4,\
"input_size" : 2,\
"dt" : 0.05,\
"max_step" : 500,\
"input_lower_bound": [-0.5, -0.5],\
self.config = {"state_size": 4,
"input_size": 2,
"dt": 0.05,
"max_step": 500,
"input_lower_bound": [-0.5, -0.5],
"input_upper_bound": [0.5, 0.5],
}

1
PythonLinearNonlinearControl/envs/make_envs.py
View File

@ -3,6 +3,7 @@ from .two_wheeled import TwoWheeledConstEnv
from .two_wheeled import TwoWheeledTrackEnv
from .cartpole import CartPoleEnv
def make_env(args):
if args.env == "FirstOrderLag":

12
PythonLinearNonlinearControl/envs/nonlinear_sample_system.py
View File

@ -4,17 +4,19 @@ from scipy import integrate
from .env import Env
from ..common.utils import update_state_with_Runge_Kutta
class NonlinearSampleEnv(Env):
""" Nonlinear Sample Env
"""
def __init__(self):
"""
"""
self.config = {"state_size" : 2,\
"input_size" : 1,\
"dt" : 0.01,\
"max_step" : 250,\
"input_lower_bound": [-0.5],\
self.config = {"state_size": 2,
"input_size": 1,
"dt": 0.01,
"max_step": 250,
"input_lower_bound": [-0.5],
"input_upper_bound": [0.5],
}

49
PythonLinearNonlinearControl/envs/two_wheeled.py
View File

@ -5,6 +5,7 @@ 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
@ -28,19 +29,21 @@ def step_two_wheeled_env(curr_x, u, dt, method="Oylar"):
return next_x
class TwoWheeledConstEnv(Env):
""" Two wheeled robot with constant goal Env
"""
def __init__(self):
"""
"""
self.config = {"state_size" : 3,\
"input_size" : 2,\
"dt" : 0.01,\
"max_step" : 500,\
"input_lower_bound": (-1.5, -3.14),\
"input_upper_bound": (1.5, 3.14),\
"car_size": 0.2,\
self.config = {"state_size": 3,
"input_size": 2,
"dt": 0.01,
"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)
}
@ -160,10 +163,10 @@ class TwoWheeledConstEnv(Env):
self.config["car_size"], curr_x[2])
# left tire
center_x = (self.config["car_size"] \
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"] \
center_y = (self.config["car_size"]
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]-np.pi/2.) + curr_x[1]
@ -172,10 +175,10 @@ class TwoWheeledConstEnv(Env):
self.config["wheel_size"], curr_x[2])
# right tire
center_x = (self.config["car_size"] \
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"] \
center_y = (self.config["car_size"]
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]+np.pi/2.) + curr_x[1]
@ -187,19 +190,21 @@ class TwoWheeledConstEnv(Env):
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,\
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)
}
@ -354,10 +359,10 @@ class TwoWheeledTrackEnv(Env):
self.config["car_size"], curr_x[2])
# left tire
center_x = (self.config["car_size"] \
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"] \
center_y = (self.config["car_size"]
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]-np.pi/2.) + curr_x[1]
@ -366,10 +371,10 @@ class TwoWheeledTrackEnv(Env):
self.config["wheel_size"], curr_x[2])
# right tire
center_x = (self.config["car_size"] \
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"] \
center_y = (self.config["car_size"]
+ self.config["wheel_size"][1]) \
* np.sin(curr_x[2]+np.pi/2.) + curr_x[1]

10
PythonLinearNonlinearControl/helper.py
View File

@ -7,6 +7,7 @@ import six
import pickle
from logging import DEBUG, basicConfig, getLogger, FileHandler, StreamHandler, Formatter, Logger
def make_logger(save_dir):
"""
Args:
@ -33,6 +34,7 @@ def make_logger(save_dir):
# sh_handler = StreamHandler()
# logger.addHandler(sh_handler)
def int_tuple(s):
""" transform str to tuple
Args:
@ -42,6 +44,7 @@ def int_tuple(s):
"""
return tuple(int(i) for i in s.split(','))
def bool_flag(s):
""" transform str to bool flg
Args:
@ -54,6 +57,7 @@ def bool_flag(s):
msg = 'Invalid value "%s" for bool flag (should be 0 or 1)'
raise ValueError(msg % s)
def file_exists(path):
""" Check file existence on given path
Args:
@ -63,6 +67,7 @@ def file_exists(path):
"""
return os.path.exists(path)
def create_dir_if_not_exist(outdir):
""" Check directory existence and creates new directory if not exist
Args:
@ -77,6 +82,7 @@ def create_dir_if_not_exist(outdir):
return
os.makedirs(outdir)
def write_text_to_file(file_path, data):
""" Write given text data to file
Args:
@ -86,6 +92,7 @@ def write_text_to_file(file_path, data):
with open(file_path, 'w') as f:
f.write(data)
def read_text_from_file(file_path):
""" Read given file as text
Args:
@ -96,6 +103,7 @@ def read_text_from_file(file_path):
with open(file_path, 'r') as f:
return f.read()
def save_pickle(file_path, data):
""" pickle given data to file
Args:
@ -105,6 +113,7 @@ def save_pickle(file_path, data):
with open(file_path, 'wb') as f:
pickle.dump(data, f)
def load_pickle(file_path):
""" load pickled data from file
Args:
@ -118,6 +127,7 @@ def load_pickle(file_path):
else:
return pickle.load(f, encoding='bytes')
def prepare_output_dir(base_dir, args, time_format='%Y-%m-%d-%H%M%S'):
""" prepare a directory with current datetime as name.
created directory contains the command and args when the script was called as text file.

52
PythonLinearNonlinearControl/models/cartpole.py
View File

@ -2,9 +2,11 @@ import numpy as np
from .model import Model
class CartPoleModel(Model):
""" cartpole model
"""
def __init__(self, config):
"""
"""
@ -31,16 +33,16 @@ class CartPoleModel(Model):
# x
d_x0 = curr_x[1]
# v_x
d_x1 = (u[0] + self.mp * np.sin(curr_x[2]) \
* (self.l * (curr_x[3]**2) \
d_x1 = (u[0] + self.mp * np.sin(curr_x[2])
* (self.l * (curr_x[3]**2)
+ self.g * np.cos(curr_x[2]))) \
/ (self.mc + self.mp * (np.sin(curr_x[2])**2))
# theta
d_x2 = curr_x[3]
# v_theta
d_x3 = (-u[0] * np.cos(curr_x[2]) \
- self.mp * self.l * (curr_x[3]**2) \
* np.cos(curr_x[2]) * np.sin(curr_x[2]) \
d_x3 = (-u[0] * np.cos(curr_x[2])
- self.mp * self.l * (curr_x[3]**2)
* np.cos(curr_x[2]) * np.sin(curr_x[2])
- (self.mc + self.mp) * self.g * np.sin(curr_x[2])) \
/ (self.l * (self.mc + self.mp * (np.sin(curr_x[2])**2)))
@ -53,16 +55,16 @@ class CartPoleModel(Model):
# x
d_x0 = curr_x[:, 1]
# v_x
d_x1 = (u[:, 0] + self.mp * np.sin(curr_x[:, 2]) \
* (self.l * (curr_x[:, 3]**2) \
d_x1 = (u[:, 0] + self.mp * np.sin(curr_x[:, 2])
* (self.l * (curr_x[:, 3]**2)
+ self.g * np.cos(curr_x[:, 2]))) \
/ (self.mc + self.mp * (np.sin(curr_x[:, 2])**2))
# theta
d_x2 = curr_x[:, 3]
# v_theta
d_x3 = (-u[:, 0] * np.cos(curr_x[:, 2]) \
- self.mp * self.l * (curr_x[:, 3]**2) \
* np.cos(curr_x[:, 2]) * np.sin(curr_x[:, 2]) \
d_x3 = (-u[:, 0] * np.cos(curr_x[:, 2])
- self.mp * self.l * (curr_x[:, 3]**2)
* np.cos(curr_x[:, 2]) * np.sin(curr_x[:, 2])
- (self.mc + self.mp) * self.g * np.sin(curr_x[:, 2])) \
/ (self.l * (self.mc + self.mp * (np.sin(curr_x[:, 2])**2)))
@ -99,31 +101,31 @@ class CartPoleModel(Model):
tmp2 = 1. / (self.mc + self.mp * (np.sin(xs[:, 2])**2))
f_x[:, 1, 2] = - us[:, 0] * tmp \
- tmp * (self.mp * np.sin(xs[:, 2]) \
* (self.l * xs[:, 3]**2 \
- tmp * (self.mp * np.sin(xs[:, 2])
* (self.l * xs[:, 3]**2
+ self.g * np.cos(xs[:, 2]))) \
+ tmp2 * (self.mp * np.cos(xs[:, 2]) * self.l \
* xs[:, 3]**2 \
+ self.mp * self.g * (np.cos(xs[:, 2])**2 \
+ tmp2 * (self.mp * np.cos(xs[:, 2]) * self.l
* xs[:, 3]**2
+ self.mp * self.g * (np.cos(xs[:, 2])**2
- np.sin(xs[:, 2])**2))
f_x[:, 3, 2] = - 1. / self.l * tmp \
* (-us[:, 0] * np.cos(xs[:, 2]) \
- self.mp * self.l * (xs[:, 3]**2) \
* np.cos(xs[:, 2]) * np.sin(xs[:, 2]) \
* (-us[:, 0] * np.cos(xs[:, 2])
- self.mp * self.l * (xs[:, 3]**2)
* np.cos(xs[:, 2]) * np.sin(xs[:, 2])
- (self.mc + self.mp) * self.g * np.sin(xs[:, 2])) \
+ 1. / self.l * tmp2 \
* (us[:, 0] * np.sin(xs[:, 2]) \
- self.mp * self.l * xs[:, 3]**2 \
* (np.cos(xs[:, 2])**2 - np.sin(xs[:, 2])**2) \
- (self.mc + self.mp) \
* (us[:, 0] * np.sin(xs[:, 2])
- self.mp * self.l * xs[:, 3]**2
* (np.cos(xs[:, 2])**2 - np.sin(xs[:, 2])**2)
- (self.mc + self.mp)
* self.g * np.cos(xs[:, 2]))
# f_theta_dot
f_x[:, 1, 3] = tmp2 * (self.mp * np.sin(xs[:, 2]) \
f_x[:, 1, 3] = tmp2 * (self.mp * np.sin(xs[:, 2])
* self.l * 2 * xs[:, 3])
f_x[:, 2, 3] = np.ones(pred_len)
f_x[:, 3, 3] = 1. / self.l * tmp2 \
* (-2. * self.mp * self.l * xs[:, 3] \
* (-2. * self.mp * self.l * xs[:, 3]
* np.cos(xs[:, 2]) * np.sin(xs[:, 2]))
return f_x * dt + np.eye(state_size) # to discrete form
@ -150,7 +152,7 @@ class CartPoleModel(Model):
f_u[:, 1, 0] = 1. / (self.mc + self.mp * (np.sin(xs[:, 2])**2))
f_u[:, 3, 0] = -np.cos(xs[:, 2]) \
/ (self.l * (self.mc \
/ (self.l * (self.mc
+ self.mp * (np.sin(xs[:, 2])**2)))
return f_u * dt # to discrete form

8
PythonLinearNonlinearControl/models/first_order_lag.py
View File

@ -3,6 +3,7 @@ import scipy.linalg
from scipy import integrate
from .model import LinearModel
class FirstOrderLagModel(LinearModel):
""" first order lag model
Attributes:
@ -10,13 +11,15 @@ class FirstOrderLagModel(LinearModel):
u (numpy.ndarray):
history_pred_xs (numpy.ndarray):
"""
def __init__(self, config, tau=0.63):
"""
Args:
tau (float): time constant
"""
# param
self.A, self.B = self._to_state_space(tau, dt=config.DT) # discrete system
self.A, self.B = self._to_state_space(
tau, dt=config.DT) # discrete system
super(FirstOrderLagModel, self).__init__(self.A, self.B)
@staticmethod
@ -44,7 +47,8 @@ class FirstOrderLagModel(LinearModel):
B = np.zeros_like(Bc)
for m in range(Bc.shape[0]):
for n in range(Bc.shape[1]):
integrate_fn = lambda tau: np.matmul(scipy.linalg.expm(Ac*tau), Bc)[m, n]
def integrate_fn(tau): return np.matmul(
scipy.linalg.expm(Ac*tau), Bc)[m, n]
sol = integrate.quad(integrate_fn, 0, dt)
B[m, n] = sol[0]

1
PythonLinearNonlinearControl/models/make_models.py
View File

@ -2,6 +2,7 @@ from .first_order_lag import FirstOrderLagModel
from .two_wheeled import TwoWheeledModel
from .cartpole import CartPoleModel
def make_model(args, config):
if args.env == "FirstOrderLag":

10
PythonLinearNonlinearControl/models/model.py
View File

@ -1,8 +1,10 @@
import numpy as np
class Model():
""" base class of model
"""
def __init__(self):
"""
"""
@ -75,7 +77,7 @@ class Model():
# next_x.shape = (pop_size, state_size)
next_x = self.predict_next_state(x, us[t])
# update
pred_xs = np.concatenate((pred_xs, next_x[np.newaxis, :, :]),\
pred_xs = np.concatenate((pred_xs, next_x[np.newaxis, :, :]),
axis=0)
x = next_x
@ -99,13 +101,13 @@ class Model():
# get size
(pred_len, input_size) = us.shape
# pred final adjoint state
lam = self.predict_terminal_adjoint_state(xs[-1],\
lam = self.predict_terminal_adjoint_state(xs[-1],
terminal_g_x=g_xs[-1])
lams = lam[np.newaxis, :]
for t in range(pred_len-1, 0, -1):
prev_lam = \
self.predict_adjoint_state(lam, xs[t], us[t],\
self.predict_adjoint_state(lam, xs[t], us[t],
goal=g_xs[t], t=t)
# update
lams = np.concatenate((prev_lam[np.newaxis, :], lams), axis=0)
@ -175,6 +177,7 @@ class Model():
raise NotImplementedError("Implement hessian of model \
with respect to the input")
class LinearModel(Model):
""" discrete linear model, x[k+1] = Ax[k] + Bu[k]
@ -182,6 +185,7 @@ class LinearModel(Model):
A (numpy.ndarray): shape(state_size, state_size)
B (numpy.ndarray): shape(state_size, input_size)
"""
def __init__(self, A, B):
"""
"""

2
PythonLinearNonlinearControl/models/two_wheeled.py
View File

@ -2,9 +2,11 @@ import numpy as np
from .model import Model
class TwoWheeledModel(Model):
""" two wheeled model
"""
def __init__(self, config):
"""
"""

2
PythonLinearNonlinearControl/planners/closest_point_planner.py
View File

@ -1,9 +1,11 @@
import numpy as np
from .planner import Planner
class ClosestPointPlanner(Planner):
""" This planner make goal state according to goal path
"""
def __init__(self, config):
"""
"""

2
PythonLinearNonlinearControl/planners/const_planner.py
View File

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

2
PythonLinearNonlinearControl/planners/planner.py
View File

@ -1,8 +1,10 @@
import numpy as np
class Planner():
"""
"""
def __init__(self):
"""
"""

4
PythonLinearNonlinearControl/plotters/animator.py
View File

@ -8,9 +8,11 @@ import matplotlib.animation as animation
logger = getLogger(__name__)
class Animator():
""" animation class
"""
def __init__(self, env, args=None):
"""
"""
@ -65,7 +67,7 @@ class Animator():
"""
# set up animation figures
self._setup()
_update_img = lambda i: self._update_img(i, history_x, history_g_x)
def _update_img(i): return self._update_img(i, history_x, history_g_x)
# Set up formatting for the movie files
Writer = animation.writers['ffmpeg']

9
PythonLinearNonlinearControl/plotters/plot_func.py
View File

@ -5,6 +5,7 @@ import matplotlib.pyplot as plt
from ..helper import save_pickle, load_pickle
def plot_result(history, history_g=None, ylabel="x",
save_dir="./result", name="state_history"):
"""
@ -28,7 +29,7 @@ def plot_result(history, history_g=None, ylabel="x",
def plot(axis, history, history_g=None):
axis.plot(range(iters), history, c="r", linewidth=3)
if history_g is not None:
axis.plot(range(iters), history_g,\
axis.plot(range(iters), history_g,
c="b", linewidth=3, label="goal")
if i < size:
@ -47,6 +48,7 @@ def plot_result(history, history_g=None, ylabel="x",
axis1.legend(ncol=1, bbox_to_anchor=(0., 1.02, 1., 0.102), loc=3)
figure.savefig(path, bbox_inches="tight", pad_inches=0.05)
def plot_results(history_x, history_u, history_g=None, args=None):
"""
@ -70,6 +72,7 @@ def plot_results(history_x, history_u, history_g=None, args=None):
name=env + "-input_history",
save_dir="./result/" + controller_type)
def save_plot_data(history_x, history_u, history_g=None, args=None):
""" save plot data
@ -98,6 +101,7 @@ def save_plot_data(history_x, history_u, history_g=None, args=None):
env + "-history_g.pkl")
save_pickle(path, history_g)
def load_plot_data(env, controller_type, result_dir="./result"):
"""
Args:
@ -123,6 +127,7 @@ def load_plot_data(env, controller_type, result_dir="./result"):
return history_x, history_u, history_g
def plot_multi_result(histories, histories_g=None, labels=None, ylabel="x",
save_dir="./result", name="state_history"):
"""
@ -146,7 +151,7 @@ def plot_multi_result(histories, histories_g=None, labels=None, ylabel="x",
axis.plot(range(iters), history,
linewidth=3, label=label, alpha=0.7, linestyle="dashed")
if history_g is not None:
axis.plot(range(iters), history_g,\
axis.plot(range(iters), history_g,
c="b", linewidth=3)
if i < size:

4
PythonLinearNonlinearControl/plotters/plot_objs.py
View File

@ -5,6 +5,7 @@ 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
@ -29,6 +30,7 @@ def circle(center_x, center_y, radius, start=0., end=2*np.pi, n_point=100):
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
@ -50,6 +52,7 @@ def circle_with_angle(center_x, center_y, radius, angle):
return circle_x, circle_y, angle_x, angle_y
def square(center_x, center_y, shape, angle):
""" Create square
@ -77,6 +80,7 @@ def square(center_x, center_y, shape, angle):
return trans_points[:, 0], trans_points[:, 1]
def square_with_angle(center_x, center_y, shape, angle):
""" Create square with angle line

1
PythonLinearNonlinearControl/runners/make_runners.py
View File

@ -1,4 +1,5 @@
from .runner import ExpRunner
def make_runner(args):
return ExpRunner()

4
PythonLinearNonlinearControl/runners/runner.py
View File

@ -4,9 +4,11 @@ import numpy as np
logger = getLogger(__name__)
class ExpRunner():
""" experiment runner
"""
def __init__(self):
"""
"""
@ -46,6 +48,6 @@ class ExpRunner():
score += cost
step_count += 1
logger.debug("Controller type = {}, Score = {}"\
logger.debug("Controller type = {}, Score = {}"
.format(controller, score))
return np.array(history_x), np.array(history_u), np.array(history_g)