ilqgames
A new real-time solver for large-scale differential games.
three_player_intersection_reachability_example.cpp
1 /*
2  * Copyright (c) 2020, The Regents of the University of California (Regents).
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are
7  * met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  *
12  * 2. Redistributions in binary form must reproduce the above
13  * copyright notice, this list of conditions and the following
14  * disclaimer in the documentation and/or other materials provided
15  * with the distribution.
16  *
17  * 3. Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * Please contact the author(s) of this library if you have any questions.
34  * Authors: David Fridovich-Keil ( dfk@eecs.berkeley.edu )
35  */
36 
37 ///////////////////////////////////////////////////////////////////////////////
38 //
39 // Three player intersection reachability example. Ordering is given by:
40 // (P1, P2, P3) = (Car 1, Car 2, Pedestrian).
41 //
42 ///////////////////////////////////////////////////////////////////////////////
43 
44 #include <ilqgames/cost/curvature_cost.h>
45 #include <ilqgames/cost/extreme_value_cost.h>
46 #include <ilqgames/cost/final_time_cost.h>
47 #include <ilqgames/cost/locally_convex_proximity_cost.h>
48 #include <ilqgames/cost/nominal_path_length_cost.h>
49 #include <ilqgames/cost/polyline2_signed_distance_cost.h>
50 #include <ilqgames/cost/proximity_cost.h>
51 #include <ilqgames/cost/quadratic_cost.h>
52 #include <ilqgames/cost/quadratic_polyline2_cost.h>
53 #include <ilqgames/cost/relative_distance_cost.h>
54 #include <ilqgames/cost/semiquadratic_cost.h>
55 #include <ilqgames/cost/semiquadratic_polyline2_cost.h>
56 #include <ilqgames/cost/signed_distance_cost.h>
57 #include <ilqgames/cost/weighted_convex_proximity_cost.h>
58 #include <ilqgames/dynamics/concatenated_dynamical_system.h>
59 #include <ilqgames/dynamics/single_player_car_5d.h>
60 #include <ilqgames/dynamics/single_player_car_6d.h>
61 #include <ilqgames/dynamics/single_player_unicycle_4d.h>
62 #include <ilqgames/examples/three_player_intersection_reachability_example.h>
63 #include <ilqgames/geometry/polyline2.h>
64 #include <ilqgames/solver/ilq_solver.h>
65 #include <ilqgames/solver/lq_feedback_solver.h>
66 #include <ilqgames/solver/problem.h>
67 #include <ilqgames/solver/solver_params.h>
68 #include <ilqgames/utils/solver_log.h>
69 #include <ilqgames/utils/strategy.h>
70 #include <ilqgames/utils/types.h>
71 
72 #include <math.h>
73 #include <memory>
74 #include <vector>
75 
76 namespace ilqgames {
77 
78 namespace {
79 
80 // Car inter-axle distance.
81 static constexpr float kInterAxleLength = 4.0; // m
82 
83 // Cost weights.
84 static constexpr float kStateRegularization = 10.0;
85 static constexpr float kControlRegularization = 10.0;
86 
87 static constexpr float kOmegaCostWeight = 0.1;
88 static constexpr float kACostWeight = 0.1;
89 static constexpr float kP1ControlCostWeight = 0.1;
90 
91 static constexpr float kLaneCostWeight = 25.0;
92 static constexpr float kLaneBoundaryCostWeight = 100.0;
93 
94 static constexpr float kProximityCostWeight = 0.0;
95 static constexpr float kMinProximity = 6.0; // m
96 using ProxCost = ProximityCost;
97 
98 static constexpr bool kOrientedRight = true;
99 
100 // Lane width.
101 static constexpr float kLaneHalfWidth = 2.5; // m
102 
103 // Nominal and max speed.
104 static constexpr float kMaxVCostWeight = 100.0;
105 static constexpr float kP1MaxV = 12.0; // m/s
106 static constexpr float kP2MaxV = 12.0; // m/s
107 static constexpr float kP3MaxV = 2.0; // m/s
108 static constexpr float kMinV = 1.0; // m/s
109 
110 static constexpr float kNominalVCostWeight = 10.0;
111 static constexpr float kP1NominalV = 8.0; // m/s
112 static constexpr float kP2NominalV = 6.0; // m/s
113 static constexpr float kP3NominalV = 1.5; // m/s
114 
115 // Initial state.
116 static constexpr float kP1InitialX = -2.0; // m
117 static constexpr float kP2InitialX = -10.0; // m
118 static constexpr float kP3InitialX = -11.0; // m
119 
120 static constexpr float kP1InitialY = -30.0; // m
121 static constexpr float kP2InitialY = 45.0; // m
122 static constexpr float kP3InitialY = 16.0; // m
123 
124 static constexpr float kP1InitialHeading = M_PI_2; // rad
125 static constexpr float kP2InitialHeading = -M_PI_2; // rad
126 static constexpr float kP3InitialHeading = 0.0; // rad
127 
128 static constexpr float kP1InitialSpeed = 4.0; // m/s
129 static constexpr float kP2InitialSpeed = 3.0; // m/s
130 static constexpr float kP3InitialSpeed = 1.25; // m/s
131 
132 // State dimensions.
133 using P1 = SinglePlayerCar5D;
134 using P2 = SinglePlayerCar5D;
135 using P3 = SinglePlayerUnicycle4D;
136 
137 static const Dimension kP1XIdx = P1::kPxIdx;
138 static const Dimension kP1YIdx = P1::kPyIdx;
139 static const Dimension kP1HeadingIdx = P1::kThetaIdx;
140 static const Dimension kP1PhiIdx = P1::kPhiIdx;
141 static const Dimension kP1VIdx = P1::kVIdx;
142 
143 static const Dimension kP2XIdx = P1::kNumXDims + P2::kPxIdx;
144 static const Dimension kP2YIdx = P1::kNumXDims + P2::kPyIdx;
145 static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kThetaIdx;
146 static const Dimension kP2PhiIdx = P1::kNumXDims + P2::kPhiIdx;
147 static const Dimension kP2VIdx = P1::kNumXDims + P2::kVIdx;
148 
149 static const Dimension kP3XIdx = P1::kNumXDims + P2::kNumXDims + P3::kPxIdx;
150 static const Dimension kP3YIdx = P1::kNumXDims + P2::kNumXDims + P3::kPyIdx;
151 static const Dimension kP3HeadingIdx =
152  P1::kNumXDims + P2::kNumXDims + P3::kThetaIdx;
153 static const Dimension kP3VIdx = P1::kNumXDims + P2::kNumXDims + P3::kVIdx;
154 
155 // Control dimensions.
156 static const Dimension kP1OmegaIdx = 0;
157 static const Dimension kP1AIdx = 1;
158 static const Dimension kP2OmegaIdx = 0;
159 static const Dimension kP2AIdx = 1;
160 static const Dimension kP3OmegaIdx = 0;
161 static const Dimension kP3AIdx = 1;
162 
163 } // anonymous namespace
164 
165 void ThreePlayerIntersectionReachabilityExample::ConstructDynamics() {
166  dynamics_.reset(new ConcatenatedDynamicalSystem(
167  {std::make_shared<P1>(kInterAxleLength),
168  std::make_shared<P2>(kInterAxleLength), std::make_shared<P3>()}));
169 }
170 
171 void ThreePlayerIntersectionReachabilityExample::ConstructInitialState() {
172  x0_ = VectorXf::Zero(dynamics_->XDim());
173  x0_(kP1XIdx) = kP1InitialX;
174  x0_(kP1YIdx) = kP1InitialY;
175  x0_(kP1HeadingIdx) = kP1InitialHeading;
176  x0_(kP1VIdx) = kP1InitialSpeed;
177  x0_(kP2XIdx) = kP2InitialX;
178  x0_(kP2YIdx) = kP2InitialY;
179  x0_(kP2HeadingIdx) = kP2InitialHeading;
180  x0_(kP2VIdx) = kP2InitialSpeed;
181  x0_(kP3XIdx) = kP3InitialX;
182  x0_(kP3YIdx) = kP3InitialY;
183  x0_(kP3HeadingIdx) = kP3InitialHeading;
184  x0_(kP3VIdx) = kP3InitialSpeed;
185 }
186 
187 void ThreePlayerIntersectionReachabilityExample::ConstructPlayerCosts() {
188  // Set up costs for all players.
189  player_costs_.emplace_back("P1", kStateRegularization,
190  kControlRegularization);
191  player_costs_.emplace_back("P2", kStateRegularization,
192  kControlRegularization);
193  player_costs_.emplace_back("P3", kStateRegularization,
194  kControlRegularization);
195  auto& p1_cost = player_costs_[0];
196  auto& p2_cost = player_costs_[1];
197  auto& p3_cost = player_costs_[2];
198 
199  // Stay in lanes.
200  const Polyline2 lane1(
201  {Point2(kP1InitialX, -1000.0), Point2(kP1InitialX, 1000.0)});
202  const Polyline2 lane2(
203  {Point2(kP2InitialX, 1000.0), Point2(kP2InitialX, 28.0),
204  Point2(kP2InitialX + 0.5, 25.0), Point2(kP2InitialX + 1.0, 24.0),
205  Point2(kP2InitialX + 3.0, 22.5), Point2(kP2InitialX + 6.0, 22.0),
206  Point2(1000.0, 22.0)});
207  const Polyline2 lane3(
208  {Point2(-1000.0, kP3InitialY), Point2(1000.0, kP3InitialY)});
209 
210  const std::shared_ptr<QuadraticPolyline2Cost> p2_lane_cost(
211  new QuadraticPolyline2Cost(kLaneCostWeight, lane2, {kP2XIdx, kP2YIdx},
212  "LaneCenter"));
213  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_r_cost(
214  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
215  {kP2XIdx, kP2YIdx}, kLaneHalfWidth,
216  kOrientedRight, "LaneRightBoundary"));
217  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_l_cost(
218  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
219  {kP2XIdx, kP2YIdx}, -kLaneHalfWidth,
220  !kOrientedRight, "LaneLeftBoundary"));
221  p2_cost.AddStateCost(p2_lane_cost);
222  p2_cost.AddStateCost(p2_lane_r_cost);
223  p2_cost.AddStateCost(p2_lane_l_cost);
224 
225  const std::shared_ptr<QuadraticPolyline2Cost> p3_lane_cost(
226  new QuadraticPolyline2Cost(kLaneCostWeight, lane3, {kP3XIdx, kP3YIdx},
227  "LaneCenter"));
228  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_r_cost(
229  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3,
230  {kP3XIdx, kP3YIdx}, kLaneHalfWidth,
231  kOrientedRight, "LaneRightBoundary"));
232  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_l_cost(
233  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3,
234  {kP3XIdx, kP3YIdx}, -kLaneHalfWidth,
235  !kOrientedRight, "LaneLeftBoundary"));
236  p3_cost.AddStateCost(p3_lane_cost);
237  p3_cost.AddStateCost(p3_lane_r_cost);
238  p3_cost.AddStateCost(p3_lane_l_cost);
239 
240  // Max/min/nominal speed costs.
241  const auto p2_min_v_cost = std::make_shared<SemiquadraticCost>(
242  kMaxVCostWeight, kP2VIdx, kMinV, !kOrientedRight, "MinV");
243  const auto p2_max_v_cost = std::make_shared<SemiquadraticCost>(
244  kMaxVCostWeight, kP2VIdx, kP2MaxV, kOrientedRight, "MaxV");
245  const auto p2_nominal_v_cost = std::make_shared<QuadraticCost>(
246  kNominalVCostWeight, kP2VIdx, kP2NominalV, "NominalV");
247  p2_cost.AddStateCost(p2_min_v_cost);
248  p2_cost.AddStateCost(p2_max_v_cost);
249  p2_cost.AddStateCost(p2_nominal_v_cost);
250 
251  const auto p3_min_v_cost = std::make_shared<SemiquadraticCost>(
252  kMaxVCostWeight, kP3VIdx, kMinV, !kOrientedRight, "MinV");
253  const auto p3_max_v_cost = std::make_shared<SemiquadraticCost>(
254  kMaxVCostWeight, kP3VIdx, kP3MaxV, kOrientedRight, "MaxV");
255  const auto p3_nominal_v_cost = std::make_shared<QuadraticCost>(
256  kNominalVCostWeight, kP3VIdx, kP3NominalV, "NominalV");
257  p3_cost.AddStateCost(p3_min_v_cost);
258  p3_cost.AddStateCost(p3_max_v_cost);
259  p3_cost.AddStateCost(p3_nominal_v_cost);
260 
261  // Penalize control effort.
262  const auto p1_omega_cost = std::make_shared<QuadraticCost>(
263  kP1ControlCostWeight, kP1OmegaIdx, 0.0, "Steering");
264  const auto p1_jerk_cost = std::make_shared<QuadraticCost>(
265  kP1ControlCostWeight, kP1AIdx, 0.0, "Acceleration");
266  p1_cost.AddControlCost(0, p1_omega_cost);
267  p1_cost.AddControlCost(0, p1_jerk_cost);
268 
269  const auto p2_omega_cost = std::make_shared<QuadraticCost>(
270  kOmegaCostWeight, kP2OmegaIdx, 0.0, "Steering");
271  const auto p2_jerk_cost = std::make_shared<QuadraticCost>(
272  kACostWeight, kP2AIdx, 0.0, "Acceleration");
273  p2_cost.AddControlCost(1, p2_omega_cost);
274  p2_cost.AddControlCost(1, p2_jerk_cost);
275 
276  const auto p3_omega_cost = std::make_shared<QuadraticCost>(
277  kOmegaCostWeight, kP3OmegaIdx, 0.0, "Steering");
278  const auto p3_a_cost = std::make_shared<QuadraticCost>(kACostWeight, kP3AIdx,
279  0.0, "Acceleration");
280  p3_cost.AddControlCost(2, p3_omega_cost);
281  p3_cost.AddControlCost(2, p3_a_cost);
282 
283  // Collision-avoidance costs.
284  const std::shared_ptr<SignedDistanceCost> p1p2_proximity_cost(
285  new SignedDistanceCost({kP1XIdx, kP1YIdx}, {kP2XIdx, kP2YIdx},
286  kMinProximity, true, "ProxCostP2"));
287  const std::shared_ptr<SignedDistanceCost> p1p3_proximity_cost(
288  new SignedDistanceCost({kP1XIdx, kP1YIdx}, {kP3XIdx, kP3YIdx},
289  kMinProximity, true, "ProxCostP3"));
290 
291  const std::shared_ptr<ProxCost> p2p1_proximity_cost(
292  new ProxCost(kProximityCostWeight, {kP2XIdx, kP2YIdx}, {kP1XIdx, kP1YIdx},
293  kMinProximity, "ProxCostP1"));
294  const std::shared_ptr<ProxCost> p2p3_proximity_cost(
295  new ProxCost(kProximityCostWeight, {kP2XIdx, kP2YIdx}, {kP3XIdx, kP3YIdx},
296  kMinProximity, "ProxCostP3"));
297  p2_cost.AddStateCost(p2p1_proximity_cost);
298  p2_cost.AddStateCost(p2p3_proximity_cost);
299 
300  const std::shared_ptr<ProxCost> p3p1_proximity_cost(
301  new ProxCost(kProximityCostWeight, {kP3XIdx, kP3YIdx}, {kP1XIdx, kP1YIdx},
302  kMinProximity, "ProxCostP1"));
303  const std::shared_ptr<ProxCost> p3p2_proximity_cost(
304  new ProxCost(kProximityCostWeight, {kP3XIdx, kP3YIdx}, {kP2XIdx, kP2YIdx},
305  kMinProximity, "ProxCostP2"));
306  p3_cost.AddStateCost(p3p1_proximity_cost);
307  p3_cost.AddStateCost(p3p2_proximity_cost);
308 
309  // Ego should care about the max of his signed distance costs.
310  constexpr bool kTakeMin = true;
311  const std::shared_ptr<ExtremeValueCost> p1_relative_distance_cost(
312  new ExtremeValueCost({p1p2_proximity_cost, p1p3_proximity_cost},
313  !kTakeMin, "RelativeDistance"));
314  p1_cost.AddStateCost(p1_relative_distance_cost);
315 
316  // Ego objective should be max over time.
317  p1_cost.SetMaxOverTime();
318 }
319 
320 inline std::vector<float> ThreePlayerIntersectionReachabilityExample::Xs(
321  const VectorXf& x) const {
322  return {x(kP1XIdx), x(kP2XIdx), x(kP3XIdx)};
323 }
324 
325 inline std::vector<float> ThreePlayerIntersectionReachabilityExample::Ys(
326  const VectorXf& x) const {
327  return {x(kP1YIdx), x(kP2YIdx), x(kP3YIdx)};
328 }
329 
330 inline std::vector<float> ThreePlayerIntersectionReachabilityExample::Thetas(
331  const VectorXf& x) const {
332  return {x(kP1HeadingIdx), x(kP2HeadingIdx), x(kP3HeadingIdx)};
333 }
334 
335 } // namespace ilqgames
Generated by   doxygen 1.8.11