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

Add twowheeled of nmpccgmres

This commit is contained in:
Shunichi09 2021-02-14 23:58:25 +09:00
parent 73833a9173
commit 4e2555d8a2
6 changed files with 103 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
PythonLinearNonlinearControl/configs/make_configs.py
View File

@ -1,5 +1,5 @@
from .first_order_lag import FirstOrderLagConfigModule
from .two_wheeled import TwoWheeledConfigModule
from .two_wheeled import TwoWheeledConfigModule, TwoWheeledExtendConfigModule
from .cartpole import CartPoleConfigModule
from .nonlinear_sample_system import NonlinearSampleSystemConfigModule, NonlinearSampleSystemExtendConfigModule
@ -12,6 +12,8 @@ def make_config(args):
if args.env == "FirstOrderLag":
return FirstOrderLagConfigModule()
elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack":
if args.controller_type == "NMPCCGMRES":
return TwoWheeledExtendConfigModule()
return TwoWheeledConfigModule()
elif args.env == "CartPole":
return CartPoleConfigModule()

2
PythonLinearNonlinearControl/configs/nonlinear_sample_system.py
View File

@ -290,8 +290,6 @@ class NonlinearSampleSystemConfigModule():
class NonlinearSampleSystemExtendConfigModule(NonlinearSampleSystemConfigModule):
INPUT_SIZE = 1 # include dummy input
def __init__(self):
super().__init__()
self.opt_config = {

89
PythonLinearNonlinearControl/configs/two_wheeled.py
View File

@ -22,17 +22,16 @@ class TwoWheeledConfigModule():
Q = np.diag([1., 1., 0.01])
Sf = np.diag([5., 5., 1.])
"""
# for track goal
"""
# 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])
"""
# for track goal to NMPC
R = np.diag([0.1, 0.1])
Q = np.diag([0.1, 0.1, 0.1])
Sf = np.diag([0.25, 0.25, 0.25])
R = np.diag([1., 1.])
Q = np.diag([0.001, 0.001, 0.001])
Sf = np.diag([1., 1., 0.001])
# bounds
INPUT_LOWER_BOUND = np.array([-1.5, -3.14])
INPUT_UPPER_BOUND = np.array([1.5, 3.14])
@ -326,3 +325,83 @@ class TwoWheeledConfigModule():
return lam_dot
else:
raise NotImplementedError
class TwoWheeledExtendConfigModule(TwoWheeledConfigModule):
PRED_LEN = 20
def __init__(self):
super().__init__()
self.opt_config = {
"NMPCCGMRES": {
"threshold": 1e-3,
"zeta": 5.,
"delta": 0.01,
"alpha": 0.5,
"tf": 1.,
"constraint": True
},
"NMPCNewton": {
"threshold": 1e-3,
"max_iteration": 500,
"learning_rate": 1e-3
}
}
@staticmethod
def gradient_hamiltonian_input_with_constraint(x, lam, u, g_x, dummy_u, raw):
"""
Args:
x (numpy.ndarray): shape(pred_len+1, state_size)
lam (numpy.ndarray): shape(pred_len, state_size)
u (numpy.ndarray): shape(pred_len, input_size)
g_xs (numpy.ndarray): shape(pred_len, state_size)
dummy_u (numpy.ndarray): shape(pred_len, input_size)
raw (numpy.ndarray): shape(pred_len, input_size), Lagrangian for constraints
Returns:
F (numpy.ndarray), shape(pred_len, 3)
"""
if len(x.shape) == 1:
vanilla_F = np.zeros(2)
extend_F = np.zeros(2) # 1 is the same as input size
extend_C = np.zeros(2)
vanilla_F[0] = u[0] + lam[0] * \
np.cos(x[2]) + lam[1] * np.sin(x[2]) + 2. * raw[0] * u[0]
vanilla_F[1] = u[1] + lam[2] + 2 * raw[1] * u[1]
extend_F[0] = -0.01 + 2. * raw[0] * dummy_u[0]
extend_F[1] = -0.01 + 2. * raw[1] * dummy_u[1]
extend_C[0] = u[0]**2 + dummy_u[0]**2 - \
TwoWheeledConfigModule.INPUT_LOWER_BOUND[0]**2
extend_C[1] = u[1]**2 + dummy_u[1]**2 - \
TwoWheeledConfigModule.INPUT_LOWER_BOUND[1]**2
F = np.concatenate([vanilla_F, extend_F, extend_C])
elif len(x.shape) == 2:
pred_len, _ = u.shape
vanilla_F = np.zeros((pred_len, 2))
extend_F = np.zeros((pred_len, 2)) # 1 is the same as input size
extend_C = np.zeros((pred_len, 2))
for i in range(pred_len):
vanilla_F[i, 0] = u[i, 0] + lam[i, 0] * \
np.cos(x[i, 2]) + lam[i, 1] * \
np.sin(x[i, 2]) + 2. * raw[i, 0] * u[i, 0]
vanilla_F[i, 1] = u[i, 1] + lam[i, 2] + 2 * raw[i, 1] * u[i, 1]
extend_F[i, 0] = -0.01 + 2. * raw[i, 0] * dummy_u[i, 0]
extend_F[i, 1] = -0.01 + 2. * raw[i, 1] * dummy_u[i, 1]
extend_C[i, 0] = u[i, 0]**2 + dummy_u[i, 0]**2 - \
TwoWheeledConfigModule.INPUT_LOWER_BOUND[0]**2
extend_C[i, 1] = u[i, 1]**2 + dummy_u[i, 1]**2 - \
TwoWheeledConfigModule.INPUT_LOWER_BOUND[1]**2
F = np.concatenate([vanilla_F, extend_F, extend_C], axis=1)
return F

2
PythonLinearNonlinearControl/envs/two_wheeled.py
View File

@ -59,7 +59,7 @@ class TwoWheeledConstEnv(Env):
self.step_count = 0
noise = np.clip(np.random.randn(3), -0.1, 0.1)
noise *= 0.01
noise *= 0.1
self.curr_x = np.zeros(self.config["state_size"]) + noise
if init_x is not None:

14
PythonLinearNonlinearControl/models/two_wheeled.py
View File

@ -56,6 +56,20 @@ class TwoWheeledModel(Model):
return next_x
def x_dot(self, curr_x, u):
""" compute x dot
Args:
curr_x (numpy.ndarray): current state, shape(state_size, )
u (numpy.ndarray): input, shape(input_size, )
Returns:
x_dot (numpy.ndarray): next state, shape(state_size, )
"""
B = np.array([[np.cos(curr_x[-1]), 0.],
[np.sin(curr_x[-1]), 0.],
[0., 1.]])
x_dot = np.matmul(B, u[:, np.newaxis])
return x_dot.flatten()
def predict_adjoint_state(self, lam, x, u, g_x=None, t=None):
""" predict adjoint states

2
scripts/simple_run.py
View File

@ -41,7 +41,7 @@ def main():
parser = argparse.ArgumentParser()
parser.add_argument("--controller_type", type=str, default="NMPCCGMRES")
parser.add_argument("--env", type=str, default="NonlinearSample")
parser.add_argument("--env", type=str, default="TwoWheeledConst")
parser.add_argument("--save_anim", type=bool_flag, default=0)
parser.add_argument("--result_dir", type=str, default="./result")