diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl index b987dceb..9e5c1245 100644 --- a/example/PassingScenario/CreateMergingScenarioGame.jl +++ b/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, diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl index 609b4289..f694cd80 100644 --- a/example/PassingScenario/GroundTruthUtils.jl +++ b/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. diff --git a/example/PassingScenario/MergingScenarioConfig.jl b/example/PassingScenario/MergingScenarioConfig.jl index a081218d..470f2f35 100644 --- a/example/PassingScenario/MergingScenarioConfig.jl +++ b/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 diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl index 42e0a584..5cdd1717 100644 --- a/example/PassingScenario/RunMergingScenario.jl +++ b/example/PassingScenario/RunMergingScenario.jl @@ -42,14 +42,15 @@ lw_m = cfg.lane_width_m # x₁ = [-lw_m/2; 15.; pi/2; v_goal; lw_m/2; 0.; pi/2; v_goal] # Generate a ground truth trajectory on which to run the leadership filter for a merging trajectory. -us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg) -us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg) +# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg) +# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg) us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg) +# us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1) p1_on_left = (x₁[1] < 0 && x₁[5] > 0) @assert xor(x₁[1] < 0 && x₁[5] > 0, x₁[1] > 0 && x₁[5] < 0) -println(p1_on_left) +println("P1 on left: ", p1_on_left) # us_refs, x₁ = get_merging_trajectory_p1_same_start_101(cfg) # us_refs = [zeros(2, T) for ii in 1:2] @@ -70,6 +71,8 @@ check_valid = get_validator(si, cfg) @assert check_valid(x_refs, us_refs, times[1:T]; p1_on_left) plot_silqgames_gt(dyn, cfg, times[1:T], x_refs, us_refs) +plot(x_refs[4, :]) + # gt_threshold = 1e-2 # gt_max_iters = 1000 # gt_step_size = 1e-2 @@ -110,6 +113,8 @@ Q = 1e-2 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2]) rng = MersenneTwister(0) R = zeros(xdim(dyn), xdim(dyn)) + 1e-2 * I +lf_R = 1.1 * R +# lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1]) zs = zeros(xdim(dyn), T) Ts = 20 num_games = 1 @@ -159,7 +164,7 @@ x̂s, P̂s, probs, pf, sg_objs = leadership_filter(dyn, costs, t₀, times, P₁, # initial covariance at the beginning of simulation us_refs, # the control inputs that the actor takes zs, # the measurements - 1.1 * R, + lf_R, process_noise_distribution, s_init_distrib, discrete_state_transition; diff --git a/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl b/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl index e3c142a8..3e209ee5 100644 --- a/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl +++ b/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl @@ -48,7 +48,7 @@ R = 0.02 * Matrix(I, xdim(dyn), xdim(dyn)) zs = zeros(xdim(dyn), T) Ts = 30 num_games = 1 -num_particles = 200 +num_particles = 100 p_transition = 0.98 p_init = 0.5 @@ -57,16 +57,16 @@ p_init = 0.5 # max_iters = 50 # step_size = 2e-2 -lf_threshold = 1e-3 #1e-3 -lf_max_iters = 50 -lf_step_size = 2e-2 +lf_threshold = 1e-2 #1e-3 +lf_max_iters = 200 +lf_step_size = 1e-2 gt_silq_num_runs=1 # config variables gt_silq_threshold=1e-3 -gt_silq_max_iters=2000 +gt_silq_max_iters=3000 gt_silq_step_size=1e-2 gt_silq_verbose=true diff --git a/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl b/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl index 0a201a00..dc48c855 100644 --- a/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl +++ b/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl @@ -21,9 +21,9 @@ include("SILQGamesMCUtils.jl") data_folder = "mc_data" -num_sims = 20 +num_sims = 40 # x₁ = [2.0, 1.0, -2.677945044588987, 0.0, -0.9192214382877095, 2.03838954750859, -1.1471487177940904, 0.0] -angle_diff = 0.2 +angle_diff = 0.1 # angle_diff = 0. topfolder_name = joinpath(data_folder, "nonlq_mc$(num_sims)_L$(leader_idx)_$(get_date_str())") diff --git a/src/LeadershipFilter.jl b/src/LeadershipFilter.jl index 4a4ee291..44a03591 100644 --- a/src/LeadershipFilter.jl +++ b/src/LeadershipFilter.jl @@ -138,7 +138,7 @@ function make_stackelberg_meas_model(tt::Int, sg_obj::SILQGamesObject, leader_id meas = extract_measurements_from_stack_trajectory(xs, tt-1, tt) # println(meas) if !is_converged - println("$(sg_obj.current_idx)/$(sg_obj.num_runs) @ $tt - $num_iters => $(convergence_metrics[1, num_iters]) > $(sg_obj.threshold)") +# println("$(sg_obj.current_idx)/$(sg_obj.num_runs) @ $tt - $num_iters => $(convergence_metrics[1, num_iters]) > $(sg_obj.threshold)") end return meas end diff --git a/src/TwoStateParticleFilter.jl b/src/TwoStateParticleFilter.jl index b9fff013..a6a7267e 100644 --- a/src/TwoStateParticleFilter.jl +++ b/src/TwoStateParticleFilter.jl @@ -186,13 +186,20 @@ function step_pf(pf::ParticleFilter, time_range, f_dynamics, h_measures, discret pf.particles[:,i,k] = f_dynamics[s_idx](time_range, 𝒳_prev[:,i], u_input_k, pf.rng) pf.z_models[:,i,k] = h_measures[s_idx](pf.particles[:,i,k]) + rz = round.(z, sigdigits=6) + rzhat = round.(pf.z_models[:,i,k]) + # println("L$(pf.s[1,i,k]) particle $i - x meas $(rz[1]) $(rz[5]) p1 ($(rzhat[1])) p2 ($(rzhat[5]))") + # println("L$(pf.s[1,i,k]) particle $i - y meas $(rz[2]) $(rz[6]) p1 ($(rzhat[2])) p2 ($(rzhat[6]))") + # println("L$(pf.s[1,i,k]) particle $i - θ meas $(rz[3]) $(rz[7]) p1 ($(rzhat[3])) p2 ($(rzhat[7]))") + # println("L$(pf.s[1,i,k]) particle $i - v meas $(rz[4]) $(rz[8]) p1 ($(rzhat[4])) p2 ($(rzhat[8]))") + z_hat = pf.z_models[:,i,k] # println("||z - ẑ|| = $(norm(z - z_hat))") # println("true z: $z, z hat: $(z_hat)") distrib = MvNormal(pf.z_models[:,i,k], R) p[i] = compute_measurement_lkhd(distrib, z) - println("meas. lkhd = $(p[i])") +# println("meas. lkhd = $(p[i])") end c_inv = weights_prev' * p