Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Sign up
Appearance settings

Commit a36a8bc

Browse files
author
Shunichi09
committed
Update: fix ilqr and ddp, models
1 parent bdb8225 commit a36a8bc

File tree

6 files changed

+22
-14
lines changed

6 files changed

+22
-14
lines changed

‎PythonLinearNonlinearControl/configs/two_wheeled.py‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,9 @@ class TwoWheeledConfigModule():
1010
INPUT_SIZE = 2
1111
DT = 0.01
1212
# cost parameters
13-
R = np.eye(INPUT_SIZE) *0.1
14-
Q = np.eye(STATE_SIZE) * 0.5
15-
Sf = np.eye(STATE_SIZE)
13+
R = np.diag([0.1, 0.1])
14+
Q = np.diag([1., 1., 0.01])
15+
Sf = np.diag([5., 5., 1.])
1616
# bounds
1717
INPUT_LOWER_BOUND = np.array([-1.5, 3.14])
1818
INPUT_UPPER_BOUND = np.array([1.5, 3.14])
@@ -41,15 +41,15 @@ def __init__(self):
4141
},
4242
"iLQR":{
4343
"max_iter": 500,
44-
"mu": 1.,
44+
"init_mu": 1.,
4545
"mu_min": 1e-6,
4646
"mu_max": 1e10,
4747
"init_delta": 2.,
4848
"threshold": 1e-6,
4949
},
5050
"DDP":{
5151
"max_iter": 500,
52-
"mu": 1.,
52+
"init_mu": 1.,
5353
"mu_min": 1e-6,
5454
"mu_max": 1e10,
5555
"init_delta": 2.,

‎PythonLinearNonlinearControl/controllers/ddp.py‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,11 @@ class DDP(Controller):
1212
""" Differential Dynamic Programming
1313
1414
Ref:
15-
Tassa, Y., Erez, T., & Todorov, E. (2012). . In 2012 IEEE/RSJ International Conference on
15+
Tassa, Y., Erez, T., & Todorov, E. (2012).
16+
In 2012 IEEE/RSJ International Conference on
1617
Intelligent Robots and Systems (pp. 4906-4913). and Study Wolf,
17-
https://github.com/studywolf/control
18+
https://github.com/studywolf/control, and
19+
https://github.com/anassinator/ilqr
1820
"""
1921
def __init__(self, config, model):
2022
"""
@@ -41,7 +43,8 @@ def __init__(self, config, model):
4143

4244
# controller parameters
4345
self.max_iter = config.opt_config["DDP"]["max_iter"]
44-
self.mu = config.opt_config["DDP"]["mu"]
46+
self.init_mu = config.opt_config["DDP"]["init_mu"]
47+
self.mu = self.init_mu
4548
self.mu_min = config.opt_config["DDP"]["mu_min"]
4649
self.mu_max = config.opt_config["DDP"]["mu_max"]
4750
self.init_delta = config.opt_config["DDP"]["init_delta"]
@@ -81,6 +84,8 @@ def obtain_sol(self, curr_x, g_xs):
8184
sol = self.prev_sol.copy()
8285
converged_sol = False
8386
update_sol = True
87+
self.mu = self.init_mu
88+
self.delta = self.init_delta
8489

8590
# line search param
8691
alphas = 1.1**(-np.arange(10)**2)

‎PythonLinearNonlinearControl/controllers/ilqr.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,8 @@ def __init__(self, config, model):
4141

4242
# controller parameters
4343
self.max_iter = config.opt_config["iLQR"]["max_iter"]
44-
self.mu = config.opt_config["iLQR"]["mu"]
44+
self.init_mu = config.opt_config["iLQR"]["init_mu"]
45+
self.mu = self.init_mu
4546
self.mu_min = config.opt_config["iLQR"]["mu_min"]
4647
self.mu_max = config.opt_config["iLQR"]["mu_max"]
4748
self.init_delta = config.opt_config["iLQR"]["init_delta"]
@@ -81,6 +82,8 @@ def obtain_sol(self, curr_x, g_xs):
8182
sol = self.prev_sol.copy()
8283
converged_sol = False
8384
update_sol = True
85+
self.mu = self.init_mu
86+
self.delta = self.init_delta
8487

8588
# line search param
8689
alphas = 1.1**(-np.arange(10)**2)

‎PythonLinearNonlinearControl/controllers/make_controllers.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,4 +18,4 @@ def make_controller(args, config, model):
1818
elif args.controller_type == "iLQR":
1919
return iLQR(config, model)
2020
elif args.controller_type == "DDP":
21-
return iLQR(config, model)
21+
return DDP(config, model)

‎PythonLinearNonlinearControl/envs/two_wheeled.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ def reset(self, init_x=None):
5656
self.curr_x = init_x
5757

5858
# goal
59-
self.g_x = np.array([5., 5., 0.])
59+
self.g_x = np.array([2.5, 2.5, 0.])
6060

6161
# clear memory
6262
self.history_x = []

‎PythonLinearNonlinearControl/models/two_wheeled.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@ def calc_f_xx(xs, us, dt):
121121
f_xx[:, 0, 2, 2] = -np.cos(xs[:, 2]) * us[:, 0]
122122
f_xx[:, 1, 2, 2] = -np.sin(xs[:, 2]) * us[:, 0]
123123

124-
return f_xx
124+
return f_xx*dt
125125

126126
@staticmethod
127127
def calc_f_ux(xs, us, dt):
@@ -144,7 +144,7 @@ def calc_f_ux(xs, us, dt):
144144
f_ux[:, 0, 0, 2] = -np.sin(xs[:, 2])
145145
f_ux[:, 1, 0, 2] = np.cos(xs[:, 2])
146146

147-
return f_ux
147+
return f_ux*dt
148148

149149
@staticmethod
150150
def calc_f_uu(xs, us, dt):
@@ -164,4 +164,4 @@ def calc_f_uu(xs, us, dt):
164164

165165
f_uu = np.zeros((pred_len, state_size, input_size, input_size))
166166

167-
return f_uu
167+
return f_uu*dt

0 commit comments

Comments
(0)

AltStyle によって変換されたページ (->オリジナル) /