Merge pull request Shunichi09#6 from Shunichi09/develop

Add: TwoWheeledTrack Env

Author: Shunichi09

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)

PythonLinearNonlinearControl/configs/cartpole.py

@@ -3,16 +3,17 @@
 class CartPoleConfigModule():
  # parameters
  ENV_NAME = "CartPole-v0"
+ PLANNER_TYPE = "Const"
  TYPE = "Nonlinear"
  TASK_HORIZON = 500
  PRED_LEN = 50
  STATE_SIZE = 4
  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 @@ def __init__(self):
  @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 @@ def input_cost_fn(u):
  @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 @@ def state_cost_fn(x, g_x):
  @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 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
  + 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 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
  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 @@ def gradient_cost_fn_with_input(x, u):
  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 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
  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 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
  * -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 @@ def hessian_cost_fn_with_input(x, u):
  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 @@ def hessian_cost_fn_with_input_state(x, u):
  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)

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()

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 @@ def input_cost_fn(u):
  """
  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 @@ def state_cost_fn(x, g_x):
  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 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
  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 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
  l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size)
  or shape(1, state_size)
  """
+ diff = TwoWheeledConfigModule.fit_diff_in_range(x - g_x)
+ 
  if not terminal:
- return 2. * (x - g_x) * np.diag(TwoWheeledConfigModule.Q)
+ return 2. * (diff) * np.diag(TwoWheeledConfigModule.Q)
 
- return (2. * (x - g_x) \
+ return (2. * (diff) \
  * np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :]
 
  @staticmethod

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)
+ return DDP(config, model)
+ 
+ raise ValueError("No controller: {}".format(args.controller_type))

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 @@ def __init__(self, config, model):
  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,19 +185,38 @@ def obtain_sol(self, curr_x, g_xs):
 
  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)
  A = matrix(A)
  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]