Some progress

mgmike1011 · May 26, 2023 · 0a23bd6 · 0a23bd6
1 parent 4edd7cc
commit 0a23bd6
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 85 deletions.
diff --git a/resource/path.csv b/resource/path.csv
@@ -1,44 +1,4 @@
 x_pos,y_pos,orientation_x,orientation_y,orientation_z,orientation_w
-7.111144077498466e-05,-6.045162308510044e-07,-2.7939677238464355e-09,8.949427865445614e-09,-9.942799806594849e-06,1.0
-7.148926670197397e-05,-6.0454539152488e-07,-2.7939677238464355e-09,8.949427865445614e-09,-9.942799806594849e-06,1.0
-7.186708535300568e-05,-6.042941436135152e-07,-0.0,8.978531695902348e-09,-9.94349829852581e-06,1.0
-7.2244911279995e-05,-6.043924258847255e-07,-0.0,8.993083611130714e-09,-9.943731129169464e-06,1.0
-7.262273720698431e-05,-6.045225973139168e-07,-0.0,8.978531695902348e-09,-9.94349829852581e-06,1.0
-7.300056313397363e-05,-6.0448002159319e-07,-0.0,8.978531695902348e-09,-9.94349829852581e-06,1.0
-7.338025170611218e-05,-6.045578970770293e-07,2.3283064365386963e-10,-5.922629497945309e-09,-9.943265467882156e-06,1.0
-7.375807763310149e-05,-6.045463578630006e-07,2.3283064365386963e-10,-5.922629497945309e-09,-9.943265467882156e-06,1.0
-7.413591083604842e-05,-6.046586236152507e-07,-2.561137080192566e-09,-5.951733328402042e-09,-9.942566975951195e-06,1.0
-7.451372948708013e-05,-6.045593750059197e-07,-1.6298145055770874e-09,-5.9371814131736755e-09,-9.942799806594849e-06,1.0
-7.489155541406944e-05,-6.04546585236676e-07,-1.6298145055770874e-09,-5.9371814131736755e-09,-9.942799806594849e-06,1.0
-7.526938134105876e-05,-6.045704594725976e-07,-1.6298145055770874e-09,-5.9371814131736755e-09,-9.942799806594849e-06,1.0
-7.564524275949225e-05,-6.074434963920794e-07,-3.725290298461914e-09,8.847564458847046e-09,-9.952345862984657e-06,1.0
-7.602290133945644e-05,-6.073057079447608e-07,-6.28642737865448e-09,8.818460628390312e-09,-9.950948879122734e-06,1.0
-0.00012486649211496115,-6.862972554699809e-07,-1.0565854609012604e-06,-3.90401401091367e-05,-9.531853720545769e-06,1.0
-0.0004761421005241573,-9.530868965157424e-07,-6.543705239892006e-06,-0.0002863997360691428,-7.451046258211136e-06,1.0
-0.0014266560319811106,-1.1742863534891512e-06,-1.5908852219581604e-05,-0.0008192420355044305,-3.423541784286499e-06,0.9999996423721313
-0.003257380099967122,-1.2214950402267277e-06,-2.6571331545710564e-05,-0.001553249778226018,2.7636997401714325e-06,0.9999988079071045
-0.0062397257424890995,-1.0178353022638476e-06,-3.546872176229954e-05,-0.0023095954675227404,1.1289259418845177e-05,0.9999973177909851
-0.01064597349613905,-6.314182883215835e-07,-4.101102240383625e-05,-0.002924559637904167,2.2090040147304535e-05,0.9999958276748657
-0.016757456585764885,-2.0891729946015403e-07,-4.5098597183823586e-05,-0.003331422805786133,3.4949975088238716e-05,0.9999943971633911
-0.024866284802556038,3.4996082831639796e-07,-5.009514279663563e-05,-0.0035820137709379196,5.0143804401159286e-05,0.9999936819076538
-0.03525810316205025,1.348494606645545e-06,-5.603558383882046e-05,-0.003790950635448098,6.716535426676273e-05,0.999992847442627
-0.04804303124547005,3.209474016330205e-06,-5.9519894421100616e-05,-0.003943555522710085,8.525955490767956e-05,0.9999921917915344
-0.06321649253368378,5.969167432340328e-06,-5.91999851167202e-05,-0.003969037439674139,0.00010431651026010513,0.9999921917915344
-0.08077890425920486,9.73987152974587e-06,-5.590519867837429e-05,-0.003872084431350231,0.0001233916264027357,0.9999925494194031
-0.10073644667863846,1.458587394154165e-05,-5.173985846340656e-05,-0.0037210199516266584,0.00014158803969621658,0.9999931454658508
-0.12309695780277252,2.0602557924576104e-05,-4.9797818064689636e-05,-0.003603140590712428,0.00015956186689436436,0.9999935030937195
-0.14789247512817383,2.8164049581391737e-05,-5.0486065447330475e-05,-0.0038733447436243296,0.0001775878481566906,0.9999924898147583
-0.17516128718852997,3.756783917197026e-05,-5.214940756559372e-05,-0.004956093616783619,0.0001950936857610941,0.9999876618385315
-0.20483462512493134,4.8968522605719045e-05,-5.329865962266922e-05,-0.0060890414752066135,0.0002122865989804268,0.9999814629554749
-0.236857071518898,6.243420648388565e-05,-5.326187238097191e-05,-0.006657916586846113,0.00022937683388590813,0.9999778270721436
-0.27120909094810486,7.802232721587643e-05,-5.2184099331498146e-05,-0.006445718463510275,0.0002465515863150358,0.9999791383743286
-0.30791184306144714,9.584909275872633e-05,-5.152984522283077e-05,-0.005687593948096037,0.000263759633526206,0.9999838471412659
-0.3470095694065094,0.00011619822180364281,-5.0609465688467026e-05,-0.004873539321124554,0.00028095394372940063,0.9999881386756897
-0.38854214549064636,0.00013921092613600194,-4.996731877326965e-05,-0.004445868078619242,0.0002979396376758814,0.9999901056289673
-0.43252456188201904,0.00016505496751051396,-4.9890950322151184e-05,-0.004571101162582636,0.0003145853988826275,0.9999895095825195
-0.4789184331893921,0.0001938939531100914,-5.019456148147583e-05,-0.004823291674256325,0.0003314486239105463,0.999988317489624
-0.5276547074317932,0.00022586455452255905,-5.0386181101202965e-05,-0.004436392802745104,0.0003477917052805424,0.9999900460243225
-0.5787618160247803,0.00026106019504368305,-5.0693750381469727e-05,-0.0037269750609993935,0.00036407518200576305,0.9999929666519165
 0.6322765350341797,0.00029966438887640834,-5.0948234274983406e-05,-0.0031016713473945856,0.0003803758881986141,0.9999951720237732
 0.6882277727127075,0.00034186471020802855,-5.081784911453724e-05,-0.0028811790980398655,0.0003966104704886675,0.999995768070221
 0.7466220855712891,0.00038781040348112583,-5.017244257032871e-05,-0.0031384325120598078,0.00041270372457802296,0.9999949932098389

diff --git a/tswr_awsim/iLQR_controller.py b/tswr_awsim/iLQR_controller.py
@@ -120,6 +120,10 @@ def __init__(self):
         # Position
         self.ref_path = None
 
+        # Control variables
+        self.theta = None
+        self.acceleration = None
+
         # Control publisher
         qos_policy = QoSProfile(reliability=ReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
                                 depth=10)
@@ -132,26 +136,21 @@ def __init__(self):
         self.control_timer = self.create_timer(control_timer, self.control_timer_callback)
 
         # iLQR Variables
-        self.n_x = 5
+        self.n_x = 4
         self.n_u = 2
 
-        self.N = 10
-        self.x0 = np.array([1, 0, 0, 1, 0])
+        self.N = 50
+        self.x0 = np.array([0, 0, 0, 0])
         self.u_trj = np.zeros((self.N - 1, self.n_u))
         self.x_trj = self.rollout(self.x0, self.u_trj)
 
-        self.r = 2.0
-        self.v_target = -1.0
         self.eps = 1e-6
 
         self.derivs = derivatives(self.discrete_dynamics, self.cost_stage, self.cost_final, self.n_x, self.n_u)
 
+        self.wheelbase = 0.33
         self.last_control_update_time = self.get_clock().now()
 
-        # Control output
-        self.theta = None
-        self.acceleration = None
-
     def current_pose_listener_callback(self, msg:PoseStamped):
         # Position
         self.curr_x = msg.pose.position.x
@@ -183,10 +182,9 @@ def publish_control(self, theta, accel):
         self.control_publisher.publish(acc)
 
     def control_publisher_timer_callback(self):
-        self.get_logger().info(f'Controller output: theta: {self.theta}, acceleration: {self.acceleration}')
         if (self.theta is not None) and (self.acceleration is not None):
             self.publish_control(self.theta, self.acceleration)
-            self.get_logger().info(f'Controller output: theta: {self.theta}, acceleration: {self.acceleration}')
+            self.get_logger().info(f'Controller output: theta: {self.theta}, acceleration: {self.acceleration}, curr_yaw: {self.curr_yaw}')
 
     def control_timer_callback(self):
         # Calculate control
@@ -214,44 +212,47 @@ def control_timer_callback(self):
         self.last_control_update_time = self.get_clock().now()
 
         # Setup problem and call iLQR
-        x0 = np.array([-3.0, 1.0, -0.2, 0.0, 0.0])
-        # x0 = np.array([self.curr_x, self.curr_y, self.curr_yaw, self.current_speed + self.eps, self.theta])
-        N = 20
-        max_iter = 20
+        x0 = np.array([self.curr_x, self.curr_y, self.current_speed, self.curr_yaw])
+        N = 50
+        max_iter = 50
         regu_init = 100
         x_trj, u_trj, cost_trace, regu_trace, redu_ratio_trace, redu_trace = self.run_ilqr(
             x0, N, max_iter, regu_init
         )
 
         self.get_logger().info(f'u_optimal: : {u_trj[0]}')
+        self.get_logger().info(f'Curr: pos: {[self.curr_x, self.curr_y]}, pred: {x_trj[0]}')
+        self.get_logger().info(f'Next: pos: {[self.ref_path[0][0], self.ref_path[0][1]]}, pred: {x_trj[-1]}')
 
-        if self.theta == None:
-            self.theta = time_elapsed * np.clip(u_trj[0][1], -1.0, 1.0)
-        else:
-            self.theta += time_elapsed * np.clip(u_trj[0][1], -1.0, 1.0)
+        self.theta = np.clip(u_trj[0][1], -1.0, 1.0)
 
-        self.acceleration = np.clip(u_trj[0][0], -1.0, 0.2)
-
-    def car_continuous_dynamics(self, x, u):
-        # x = [x position, y position, heading, speed, steering angle]
-        # u = [acceleration, steering velocity]
-        m = sym if x.dtype == object else np  # Check type for autodiff
-        heading = x[2]
-        v = x[3]
-        steer = x[4]
-        x_d = np.array(
-            [v * m.cos(heading), v * m.sin(heading), v * m.tan(steer), u[0], u[1]]
-        )
-        return x_d
+        self.acceleration = np.clip(u_trj[0][0], -1.0, 1.0)
 
     def discrete_dynamics(self, x, u):
         dt = 0.1
-        # DONE TODO: Fill in the Euler integrator below and return the next state
-        # print(car_continuous_dynamics(x, u))
-        x_next = x + dt * self.car_continuous_dynamics(x, u)
-        # print(x)
-        # print()
-        # print(x_next)
+
+        current_speed = x[2]
+        curr_yaw = x[3]
+
+        if self.acceleration is not None and self.theta is not None:
+            A = np.array([[1, 0, dt * np.cos(curr_yaw), -dt * current_speed * np.sin(curr_yaw)],
+                          [0, 1, dt * np.sin(curr_yaw), dt * current_speed * np.cos(curr_yaw)],
+                          [0, 0, 1, 0],
+                          [0, 0, (dt * np.tan(self.theta)) / self.wheelbase, 1]])
+        else:
+            A = np.zeros((4, 4))
+
+        if self.theta is not None:
+            B = np.array([[0, 0],
+                          [0, 0],
+                          [dt, 0],
+                          [0, dt * current_speed / (self.wheelbase * np.cos(self.theta) ** 2)]])
+        else:
+            B = np.zeros((4, 2))
+
+        x_dot = A @ x + B @ u
+        x_next = x + (dt * x_dot)
+
         return x_next
 
     def rollout(self, x0, u_trj):
@@ -270,16 +271,28 @@ def rollout(self, x0, u_trj):
 
     def cost_stage(self, x, u):
         m = sym if x.dtype == object else np  # Check type for autodiff
-        c_circle = (m.sqrt(x[0] ** 2 + x[1] ** 2 + self.eps) - self.r) ** 2
-        c_speed = (x[3] - self.v_target) ** 2
-        c_control = (u[0] ** 2 + u[1] ** 2) * 0.1
-        return c_circle + c_speed + c_control
+
+        if self.ref_path != None:
+            c_traj_x = (x[0] - self.ref_path[0][0]) ** 2
+            c_traj_y = (x[1] - self.ref_path[0][1]) ** 2
+            c_vel = (x[2] - 27.0) ** 2
+            c_traj_yaw = (x[3] - self.ref_path[0][2]) ** 2
+            c_control = (u[0] ** 2 + u[1] ** 2) * 0.1
+            return c_traj_x + c_traj_y + c_vel + c_control
+        else:
+            return 0.0
 
     def cost_final(self, x):
         m = sym if x.dtype == object else np  # Check type for autodiff
-        c_circle = (m.sqrt(x[0] ** 2 + x[1] ** 2 + self.eps) - self.r) ** 2
-        c_speed = (x[3] - self.v_target) ** 2
-        return c_circle + c_speed
+
+        if self.ref_path != None:
+            c_traj_x = (x[0] - self.ref_path[0][0]) ** 2
+            c_traj_y = (x[1] - self.ref_path[0][1]) ** 2
+            c_vel = (x[2] - 27.0) ** 2
+            c_traj_yaw = (x[3] - self.ref_path[0][2]) ** 2
+            return c_traj_x + c_traj_y + c_vel
+        else:
+            return 0.0
 
     def cost_trj(self, x_trj, u_trj):
         total = 0.0