add to goal cost

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -8,73 +8,12 @@ author: Atsushi Sakai (@Atsushi_twi)
 bstacleR=0.5;%衝突判定用の障害物の半径
 global dt; dt=0.1;%刻み時間[s]
 
-%ロボットの力学モデル
-%[最高速度[m/s],最高回頭速度[rad/s],最高加減速度[m/ss],最高加減回頭速度[rad/ss],
-% 速度解像度[m/s],回頭速度解像度[rad/s]]
-Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];
-
-%評価関数のパラメータ [heading,dist,velocity,predictDT]
-evalParam=[0.1,0.2,0.1,3.0];
-area=[-1 11 -1 11];%シミュレーションエリアの広さ [xmin xmax ymin ymax]
-
-%シミュレーション結果
-result.x=[];
-tic;
-%movcount=0;
-% Main loop
-for i=1:5000
-    %DWAによる入力値の計算
-    [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
-    x=f(x,u);%運動モデルによる移動
-
-    %シミュレーション結果の保存
-    result.x=[result.x; x'];
-
-    %ゴール判定
-    if norm(x(1:2)-goal')<0.5
-        disp('Arrive Goal!!');break;
-    end
-
-    %====Animation====
-    hold off;
-    ArrowLength=0.5;%矢印の長さ
-    %ロボット
-    quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;
-    plot(result.x(:,1),result.x(:,2),'-b');hold on;
-    plot(goal(1),goal(2),'*r');hold on;
-    plot(obstacle(:,1),obstacle(:,2),'*k');hold on;
-    %探索軌跡表示
-    if ~isempty(traj)
-        for it=1:length(traj(:,1))/5
-            ind=1+(it-1)*5;
-            plot(traj(ind,:),traj(ind+1,:),'-g');hold on;
-        end
-    end
-    axis(area);
-    grid on;
-    drawnow;
-    %movcount=movcount+1;
-    %mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
-end
-figure(2)
-plot(result.x(:,4));
-toc
-%movie2avi(mov,'movie.avi');
-
-
-function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)
-%DWAによる入力値の計算をする関数
 
 %Dynamic Window[vmin,vmax,ωmin,ωmax]の作成
 Vr=CalcDynamicWindow(x,model);
 %評価関数の計算
 [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);
 
-if isempty(evalDB)
-    disp('no path to goal!!');
-    u=[0;0];return;
-end
-
 %各評価関数の正規化
 evalDB=NormalizeEval(evalDB);
 
@@ -119,18 +58,6 @@ if sum(EvalDB(:,5))~=0
     EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5));
 end
 
-function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)
-%軌跡データを作成する関数
-global dt;
-time=0;
-u=[vt;ot];%入力値
-traj=x;%軌跡データ
-while time<=evaldt
-    time=time+dt;%シミュレーション時間の更新
-    x=f(x,u);%運動モデルによる推移
-    traj=[traj x];
-end
-
 function stopDist=CalcBreakingDist(vel,model)
 %現在の速度から力学モデルに従って制動距離を計算する関数
 global dt;
@@ -165,45 +92,6 @@ end
 
 heading=180-targetTheta;
 
-function Vr=CalcDynamicWindow(x,model)
-%モデルと現在の状態からDyamicWindowを計算
-global dt;
-%車両モデルによるWindow
-Vs=[0 model(1) -model(2) model(2)];
-
-%運動モデルによるWindow
-Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];
-
-%最終的なDynamic Windowの計算
-Vtmp=[Vs;Vd];
-Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
-%[vmin,vmax,ωmin,ωmax]
-
-function x = f(x, u)
-% Motion Model
-global dt;
-
-F = [1 0 0 0 0
-     0 1 0 0 0
-     0 0 1 0 0
-     0 0 0 0 0
-     0 0 0 0 0];
-
-B = [dt*cos(x(3)) 0
-    dt*sin(x(3)) 0
-    0 dt
-    1 0
-    0 1];
-
-x= F*x+B*u;
-
-function radian = toRadian(degree)
-% degree to radian
-radian = degree/180*pi;
-
-function degree = toDegree(radian)
-% radian to degree
-degree = radian/pi*180;
 """
 
 import math
@@ -220,11 +108,12 @@ class Config():
         self.max_yawrate = 40.0 * math.pi / 180.0
         self.max_accel = 0.2
         self.max_dyawrate = 40.0 * math.pi / 180.0
-        self.v_reso = 0.1
+        self.v_reso = 0.01
         self.yawrate_reso = 0.1 * math.pi / 180.0
 
         self.dt = 0.1  # [s]
         self.predict_time = 5.0  # [s]
+        self.to_goal_cost_gain = 1.0  # [s]
 
 
 def plot_arrow(x, y, yaw, length=0.5, width=0.1):
@@ -244,23 +133,23 @@ def motion(x, u, dt):
     return x
 
 
-def calc_dynamic_window(x, rspec, dt):
+def calc_dynamic_window(x, config):
 
     # Dynamic window from robot specification
-    Vs = [rspec.min_speed, rspec.max_speed,
-          -rspec.max_yawrate, rspec.max_yawrate]
+    Vs = [config.min_speed, config.max_speed,
+          -config.max_yawrate, config.max_yawrate]
 
     # Dynamic window from motion model
-    Vd = [x[3] - rspec.max_accel * dt,
-          x[3] + rspec.max_accel * dt,
-          x[4] - rspec.max_dyawrate * dt,
-          x[4] + rspec.max_dyawrate * dt]
-    print(Vs, Vd)
+    Vd = [x[3] - config.max_accel * config.dt,
+          x[3] + config.max_accel * config.dt,
+          x[4] - config.max_dyawrate * config.dt,
+          x[4] + config.max_dyawrate * config.dt]
+    #  print(Vs, Vd)
 
     #  [vmin,vmax, yawrate min, yawrate max]
     dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
           max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
-    print(dw)
+    #  print(dw)
 
     return dw
 
@@ -279,23 +168,59 @@ def calc_trajectory(xinit, v, y, config):
     return traj
 
 
-def calc_cost(x, dw, rspec, dt):
+def evaluate_path(x, dw, config, goal):
 
     xinit = x[:]
 
-    for v in np.arange(dw[0], dw[1], rspec.v_reso):
-        for y in np.arange(dw[2], dw[3], rspec.yawrate_reso):
-            traj = calc_trajectory(xinit, v, y, rspec)
-            plt.plot(traj[:, 0], traj[:, 1])
+    min_cost = float("Inf")
+    min_u = [0.0, 0.0]
+
+    #  print(dw)
+
+    for v in np.arange(dw[0], dw[1], config.v_reso):
+        for y in np.arange(dw[2], dw[3], config.yawrate_reso):
+            traj = calc_trajectory(xinit, v, y, config)
+            #  plt.plot(traj[:, 0], traj[:, 1])
+            #  print(v, ",", y)
+
+            angle_cost = calc_to_goal_cost(traj, goal, config)
+
+            final_cost = angle_cost
+
+            if min_cost >= final_cost:
+                min_cost = final_cost
+                min_u = [v, y]
+
+    #  print(min_u)
+    #  input()
+
+    return min_u
 
 
-def dwa_control(x, u, rspec, dt):
+def calc_to_goal_cost(traj, goal, config):
+    cost = 0
 
-    dw = calc_dynamic_window(x, rspec, dt)
+    #  traj_end_yaw = traj[-1, 2]
+    dy = goal[0] - traj[-1, 0]
+    dx = goal[1] - traj[-1, 1]
 
-    calc_cost(x, dw, rspec, dt)
+    #  goal_yaw = math.atan2(dy, dx)
 
-    u = np.array([0.5, 0.0])
+    goal_dis = math.sqrt(dx**2 + dy**2)
+    #  print(goal_dis)
+
+    #  cost = config.angle_cost_gain * abs(goal_yaw - traj_end_yaw) + goal_dis
+    cost = config.to_goal_cost_gain * goal_dis
+    #  print(cost)
+
+    return cost
+
+
+def dwa_control(x, u, config, dt, goal):
+
+    dw = calc_dynamic_window(x, config)
+
+    u = evaluate_path(x, dw, config, goal)
 
     return u
 
@@ -304,7 +229,7 @@ def main():
     print(__file__ + " start!!")
 
     # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
-    x = np.array([0.0, 0.0, math.pi / 4.0, 0.0, 0.0])
+    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
     # goal position [x(m), y(m)]
     goal = np.array([10, 10])
     # obstacles [x(m) y(m), ....]
@@ -323,11 +248,11 @@ def main():
 
     dt = 0.1
     u = np.array([0.0, 0.0])
-    rspec = Config()
+    config = Config()
 
-    for i in range(100):
+    for i in range(1000):
         plt.cla()
-        u = dwa_control(x, u, rspec, dt)
+        u = dwa_control(x, u, config, dt, goal)
 
         x = motion(x, u, dt)