diff --git a/iLQR/README.md b/ilqr/README.md similarity index 100% rename from iLQR/README.md rename to ilqr/README.md diff --git a/iLQR/animation.py b/ilqr/animation.py similarity index 100% rename from iLQR/animation.py rename to ilqr/animation.py diff --git a/iLQR/goal_maker.py b/ilqr/goal_maker.py similarity index 100% rename from iLQR/goal_maker.py rename to ilqr/goal_maker.py diff --git a/iLQR/ilqr.py b/ilqr/ilqr.py similarity index 89% rename from iLQR/ilqr.py rename to ilqr/ilqr.py index ff9ece2..8466069 100644 --- a/iLQR/ilqr.py +++ b/ilqr/ilqr.py @@ -164,9 +164,9 @@ class iLQRController(): l_xx : numpy.ndarray, shape(STATE_SIZE, STATE_SIZE) second order differential of stage cost by x """ - Q_11 = 1. # terminal x cost weight - Q_22 = 1. # terminal y cost weight - Q_33 = 0.01 # terminal theta cost weight + Q_11 = 10. # terminal x cost weight + Q_22 = 10. # terminal y cost weight + Q_33 = 0.1 # terminal theta cost weight error = self.simulator.xs - self.target @@ -290,19 +290,36 @@ class iLQRController(): for t in range(tN-1): # x(t+1) = f(x(t), u(t)) = x(t) + dx(t) * dt # linearized dx(t) = np.dot(A(t), x(t)) + np.dot(B(t), u(t)) - # f_x = np.eye + A(t) - # f_u = B(t) + # f_x = (np.eye + A(t)) * dt + # f_u = (B(t)) * dt + # continuous --> discrete A, B = self.finite_differences(X[t], U[t]) f_x[t] = np.eye(self.STATE_SIZE) + A * self.dt f_u[t] = B * self.dt - + + """ NOTE: why multiply dt in original program ?? + So the dt multiplication and + I is because we’re actually taking the derivative of the state with respect to the previous state. Which is not + dx = Ax + Bu, + but rather + x(t) = x(t-1) + (Ax(t-1) + Bu(t-1))*dt + And that’s where the identity matrix and dt come from! + So that part’s in the comments of the code, but the *dt on all the cost function stuff is not commented on at all! + So here the dt lets you normalize behaviour for different time steps (assuming you also scale the number of steps in the sequence). + So if you have a time step of .01 or .001 you’re not racking up 10 times as much cost function in the latter case. + And if you run the code with 50 steps in the sequence and dt=.01 and 500 steps in the sequence + and dt=.001 you’ll see that you get the same results, which is not the case at all when you don’t take dt into account in the cost function! + """ + (l[t], l_x[t], l_xx[t], l_u[t], l_uu[t], l_ux[t]) = self.cost(X[t], U[t]) + + """ # we consider this part in cost function. l[t] *= self.dt l_x[t] *= self.dt l_xx[t] *= self.dt l_u[t] *= self.dt l_uu[t] *= self.dt l_ux[t] *= self.dt + """ # and for final state l[-1], l_x[-1], l_xx[-1] = self.cost_final(X[-1]) @@ -329,8 +346,8 @@ class iLQRController(): Q_uu_evals += lamb Q_uu_inv = np.dot(Q_uu_evecs, np.dot(np.diag(1.0/Q_uu_evals), Q_uu_evecs.T)) - k[t] = -np.dot(Q_uu_inv, Q_u) - K[t] = -np.dot(Q_uu_inv, Q_ux) + k[t] = -1. * np.dot(Q_uu_inv, Q_u) + K[t] = -1. * np.dot(Q_uu_inv, Q_ux) V_x = Q_x - np.dot(K[t].T, np.dot(Q_uu, k[t])) V_xx = Q_xx - np.dot(K[t].T, np.dot(Q_uu, K[t])) diff --git a/iLQR/main_dynamic.py b/ilqr/main_dynamic.py similarity index 100% rename from iLQR/main_dynamic.py rename to ilqr/main_dynamic.py diff --git a/iLQR/main_static.py b/ilqr/main_static.py similarity index 100% rename from iLQR/main_static.py rename to ilqr/main_static.py diff --git a/iLQR/model.py b/ilqr/model.py similarity index 100% rename from iLQR/model.py rename to ilqr/model.py