[WIP] Implement merging scenario.

example/PassingScenario/CreateMergingScenarioGame.jl

@@ -22,6 +22,82 @@ function combine_cost_funcs(funcs, weights)
     return g
 end
 
+function make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, player_idx)
+
+    base_idx = 4 * (player_idx - 1)
+    player_xidx = base_idx + 1
+    player_yidx = base_idx + 2
+
+    L₁ = cfg.region1_length_m
+    L₂ = cfg.region2_length_m
+    w = cfg.lane_width_m
+
+    merging_trajectory_position_cost(si, x, us, t) = begin
+        dist_along_lane = x[player_yidx]
+
+        if dist_along_lane ≤ L₁ # separate lanes
+            goal_pos = (p1_on_left) ? -w/2 : w/2
+        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+        #     mult = (p1_on_left) ? -1 : 1
+        #     goal_pos = mult * (w/4 + lin_width_at_dist_proportion * w/4)
+        else # in second or final region
+            goal_pos = 0.
+        end
+
+        return 1//2 * (x[player_xidx] - goal_pos)^2
+    end
+
+    return merging_trajectory_position_cost
+end
+
+function make_collision_cost(cfg; large_number=LARGE_NUMBER)
+    avoid_collisions_cost_fn(si, x, us, t) = begin
+        # This log barrier avoids agents getting within some configured radius of one another.
+        dist_to_boundary = norm([1 1 0 0 -1 -1 0 0] * x, cfg.dist_norm_order) - cfg.collision_radius_m
+        # TODO(hamzah) - add term to drag trajectory back to valid zone
+        return (dist_to_boundary ≤ 0) ? large_number : -log(dist_to_boundary)
+    end
+
+    return avoid_collisions_cost_fn
+end
+
+function make_piecewise_lane_boundary_cost(cfg, p1_on_left, player_idx; large_number=LARGE_NUMBER)
+    base_idx = 4 * (player_idx - 1)
+    player_xidx = base_idx + 1
+    player_yidx = base_idx + 2
+
+    L₁ = cfg.region1_length_m
+    L₂ = cfg.region2_length_m
+    w = cfg.lane_width_m
+
+    # This player is on the left if it's P1 on left or P2 where P1 not on left.
+    player_on_left = (p1_on_left && player_idx == 1) || !p1_on_left && player_idx == 2
+
+    stay_within_lanes(si, x, us, t) = begin
+        dist_along_lane = x[player_yidx]
+
+        if dist_along_lane ≤ L₁ # separate lanes
+            upper_bound = (player_on_left) ? 0. : w
+            lower_bound = (player_on_left) ? -w : 0.
+        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+            upper_bound = w/2 + lin_width_at_dist_proportion * w/2
+            lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
+        else # in final region
+            upper_bound = w/2
+            lower_bound = -w/2
+        end
+
+        # println(x[1], " ", lower_bound," ", upper_bound)
+        violates_bound = x[player_xidx] ≥ upper_bound || x[player_xidx] ≤ lower_bound
+        return violates_bound ? large_number : -log(upper_bound-x[player_xidx]) -log(x[player_xidx]-lower_bound)
+    end
+
+    return stay_within_lanes
+end
+
+
 function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p2, goal_p1, goal_p2; large_number=1e6, p1_on_left=true)
     @assert length(w_p1) == NUM_MERGING_SCENARIO_SUBCOSTS
     @assert length(w_p2) == NUM_MERGING_SCENARIO_SUBCOSTS
@@ -33,7 +109,7 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
     const_1 = 1.
     Q1 = zeros(8, 8)
     Q1[1, 1] = const_1 * 0.
-    Q1[2, 2] = const_1 * 0.5#2.
+    Q1[2, 2] = const_1 * 0.#0.5#2.
     Q1[3, 3] = const_1 * 1.
     Q1[4, 4] = const_1 * 1.
     q_cost1 = QuadraticCost(Q1)
@@ -43,7 +119,7 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
 
     Q2 = zeros(8, 8)
     Q2[5, 5] = const_1 * 0.
-    Q2[6, 6] = const_1 * 0.5
+    Q2[6, 6] = const_1 * 0#.5
     Q2[7, 7] = const_1 * 1.
     Q2[8, 8] = const_1 * 1.
     q_cost2 = QuadraticCost(Q2)
@@ -52,56 +128,49 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
     c2a = QuadraticCostWithOffset(q_cost2, goal_p2)
 
 
-    merging_trajectory_position_cost_p1(si, x, us, t) = begin
-        dist_along_lane = x[2]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            goal_pos = (p1_on_left) ? -w/2 : w/2
-        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-        #     goal_pos = -w/4 - lin_width_at_dist_proportion * w/4
-        else # in final region 
-            goal_pos = 0.
-        end
-
-        return 1//2 * (x[1] - goal_pos)^2
-    end
-
-    merging_trajectory_position_cost_p2(si, x, us, t) = begin
-        dist_along_lane = x[6]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            goal_pos = (p1_on_left) ? w/2 : -w/2
-            # goal_pos = w/2
-        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-        #     goal_pos = w/4 + lin_width_at_dist_proportion * w/4
-        else # in final region 
-            goal_pos = 0.
-        end
-
-        return 1//2 * (x[5] - goal_pos)^2
-    end
-
-    c1a_i = PlayerCost(merging_trajectory_position_cost_p1, si)
-    c2a_i = PlayerCost(merging_trajectory_position_cost_p2, si)
+    # merging_trajectory_position_cost_p1(si, x, us, t) = begin
+    #     dist_along_lane = x[2]
+    #     L₁ = cfg.region1_length_m
+    #     L₂ = cfg.region2_length_m
+    #     w = cfg.lane_width_m
+
+    #     if dist_along_lane ≤ L₁ # separate lanes
+    #         goal_pos = (p1_on_left) ? -w/2 : w/2
+    #     # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+    #     #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+    #     #     goal_pos = -w/4 - lin_width_at_dist_proportion * w/4
+    #     else # in second or final region 
+    #         goal_pos = 0.
+    #     end
+
+    #     return 1//2 * (x[1] - goal_pos)^2
+    # end
+
+    # merging_trajectory_position_cost_p2(si, x, us, t) = begin
+    #     dist_along_lane = x[6]
+    #     L₁ = cfg.region1_length_m
+    #     L₂ = cfg.region2_length_m
+    #     w = cfg.lane_width_m
+
+    #     if dist_along_lane ≤ L₁ # separate lanes
+    #         goal_pos = (p1_on_left) ? w/2 : -w/2
+    #         # goal_pos = w/2
+    #     # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+    #     #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+    #     #     goal_pos = w/4 + lin_width_at_dist_proportion * w/4
+    #     else # in second or final region 
+    #         goal_pos = 0.
+    #     end
+
+    #     return 1//2 * (x[5] - goal_pos)^2
+    # end
+
+    c1a_i = PlayerCost(make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, 1), si)
+    c2a_i = PlayerCost(make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, 2), si)
 
     # 2. avoid collisions
-    avoid_collisions_cost_fn(si, x, us, t) = begin
-        # This log barrier avoids agents getting within some configured radius of one another.
-        # TODO(hamzah) - this may accidentally be using 1-norm.
-        dist_to_boundary = norm([1 1 0 0 -1 -1 0 0] * x, cfg.dist_norm_order) - cfg.collision_radius_m
-        return (dist_to_boundary ≤ 0) ? large_number : -log(dist_to_boundary)
-    end
-
-    c1b = PlayerCost(avoid_collisions_cost_fn, si)
-    c2b = PlayerCost(avoid_collisions_cost_fn, si)
+    c1b = PlayerCost(make_collision_cost(cfg), si)
+    c2b = PlayerCost(make_collision_cost(cfg), si)
 
     # 3. enforce speed limit and turning limit
     c1c_i = AbsoluteLogBarrierCost(4, cfg.speed_limit_mps, false)
@@ -142,53 +211,8 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
 
     # 6. log barriers on the x dimension ensure that the vehicles don't exit the road
     # TODO(hamzah) - remove assumption of straight road
-    stay_within_lanes_p1(si, x, us, t) = begin
-        dist_along_lane = x[2]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            upper_bound = (p1_on_left) ? 0. : w
-            lower_bound = (p1_on_left) ? -w : 0.
-        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-            upper_bound = w/2 + lin_width_at_dist_proportion * w/2
-            lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
-        else # in final region 
-            upper_bound = w/2
-            lower_bound = -w/2
-        end
-
-        # println(x[1], " ", lower_bound," ", upper_bound)
-        violates_bound = x[1] ≥ upper_bound || x[1] ≤ lower_bound
-        return violates_bound ? large_number : -log(upper_bound-x[1]) -log(x[1]-lower_bound)
-    end
-
-    stay_within_lanes_p2(si, x, us, t) = begin
-        dist_along_lane = x[6]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            upper_bound = (p1_on_left) ? w : 0.
-            lower_bound = (p1_on_left) ? 0. : -w
-        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-            upper_bound = w/2 + lin_width_at_dist_proportion * w/2
-            lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
-        else # in final region 
-            upper_bound = w/2
-            lower_bound = -w/2
-        end
-
-        violates_bound = x[5] ≥ upper_bound || x[5] ≤ lower_bound
-        return violates_bound ? large_number : -log(upper_bound-x[5]) -log(x[5]-lower_bound)
-    end
-
-    c1f = PlayerCost(stay_within_lanes_p1, si)
-    c2f = PlayerCost(stay_within_lanes_p2, si)
+    c1f = PlayerCost(make_piecewise_lane_boundary_cost(cfg, p1_on_left, 1; large_number), si)
+    c2f = PlayerCost(make_piecewise_lane_boundary_cost(cfg, p1_on_left, 2; large_number), si)
 
     # player 1 stays ahead of player 2
     stay_ahead_cost(si, x, us, t) = begin
@@ -199,7 +223,6 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
     end
     c1g = PlayerCost(stay_ahead_cost, si)
 
-
     costs_p1 = [c1a,
                 c1a_i,
                 c1b,

example/PassingScenario/GroundTruthUtils.jl

@@ -302,8 +302,6 @@ function get_merging_trajectory_p1_same_start_101(cfg::MergingScenarioConfig)
                     7.7*ones(1, 15),
                        zeros(1, 49),
                        zeros(1, 27)
-                   # -8.7*ones(1, 17),
-                   # -5  *ones(1, 10)
                    )
               )
 
@@ -314,11 +312,15 @@ end
 # p1 on right and p2 on left
 function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
     v_init = 10.
+    v_goal = v_init
     lw_m = cfg.lane_width_m
 
     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
     x₁ = [lw_m/2; 0.; pi/2; v_init; -lw_m/2; 15.; pi/2; v_init]
 
+    p1_goal = vcat([0.; 80; pi/2; v_goal], zeros(4))
+    p2_goal = vcat(zeros(4),               [0.; 95.; pi/2; v_goal])
+
     # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
     w1 = vcat(hcat(zeros(1, 40),
                   -0.2*ones(1, 8),
@@ -332,13 +334,6 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
                     7.7*ones(1, 15),
                        zeros(1, 49),
                        zeros(1, 27)
-                    # -7.7*ones(1, 15),
-                    # 7.7*ones(1, 15),
-                    #     zeros(1, 46)
-                       # zeros(1, 49),
-                       # zeros(1, 27)
-                   # -8.7*ones(1, 17),
-                   # -5  *ones(1, 10)
                    )
               )
 
@@ -356,33 +351,34 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
                     -7.7*ones(1, 15),
                     7.7*ones(1, 15),
                         zeros(1, 46)
-                       # zeros(1, 49),
-                       # zeros(1, 27)
-                   # -8.7*ones(1, 17),
-                   # -5  *ones(1, 10)
-                   )
-              )
-
-    # vcat(hcat(
-    #                zeros(1, 50),
-    #                0.4*ones(1, 8),
-    #               -0.4*ones(1, 8),
-    #                zeros(1, 9),
-    #                zeros(1, 16),
-    #                zeros(1, 10)), 
-    #           hcat( 5  *ones(1, 10),
-    #                 8.7*ones(1, 15),
-    #                    zeros(1, 49),
-    #                    zeros(1, 27)
-    #                # -8.7*ones(1, 17),
-    #                # -5  *ones(1, 10)
-    #                )
-    #           )
+                )
+            )
 
     ws = [w2, w1]
     return ws, x₁, p1_goal, p2_goal
 end
 
+# p1 on right and p2 on left
+function get_merging_debug(cfg::MergingScenarioConfig, player_in_front=2)
+    v_init = 10.
+    v_goal = 10.
+    lw_m = cfg.lane_width_m
+
+    x₁ = [-0.01; 50.; pi/2; v_init; 0.01; 40.; pi/2; v_init]
+
+
+    p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
+    p2_goal = vcat(zeros(4),               [0.; 130.; pi/2; v_goal])
+
+    if player_in_front == 2
+        x₁[2], x₁[6] = x₁[6], x₁[2]
+        p1_goal[2], p2_goal[6] = p2_goal[6], p1_goal[2]
+    end
+
+    ws = [zeros(2, 101), zeros(2, 101)]
+    return ws, x₁, p1_goal, p2_goal
+end
+
 
 
 # For saving the trajectory - in case we want to use it later.

example/PassingScenario/MergingScenarioConfig.jl

@@ -2,6 +2,8 @@ using Parameters
 
 # This file contains configuration variables related to the road and constraints.
 
+const LARGE_NUMBER = 1e6
+
 # The configuration defines a straight two-way lane that defines a right-hand coordinate system where y points in the
 # direction of the lane, and x points to the right side of the road.
 DEFAULT_LANE_WIDTH = 2.3