ilqgames
A new real-time solver for large-scale differential games.
three_player_flat_intersection_example.cpp
1 /*
2  * Copyright (c) 2019, 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 *flat* intersection example. Ordering is given by the following:
40 // (P1, P2, P3) = (Car 1, Car 2, Pedestrian).
41 //
42 ///////////////////////////////////////////////////////////////////////////////
43 
44 #include <ilqgames/cost/curvature_cost.h>
45 #include <ilqgames/cost/final_time_cost.h>
46 #include <ilqgames/cost/locally_convex_proximity_cost.h>
47 #include <ilqgames/cost/nominal_path_length_cost.h>
48 #include <ilqgames/cost/proximity_cost.h>
49 #include <ilqgames/cost/quadratic_cost.h>
50 #include <ilqgames/cost/quadratic_norm_cost.h>
51 #include <ilqgames/cost/quadratic_polyline2_cost.h>
52 #include <ilqgames/cost/semiquadratic_cost.h>
53 #include <ilqgames/cost/semiquadratic_norm_cost.h>
54 #include <ilqgames/cost/semiquadratic_polyline2_cost.h>
55 #include <ilqgames/dynamics/concatenated_flat_system.h>
56 #include <ilqgames/dynamics/single_player_flat_car_6d.h>
57 #include <ilqgames/dynamics/single_player_flat_unicycle_4d.h>
58 #include <ilqgames/examples/three_player_flat_intersection_example.h>
59 #include <ilqgames/geometry/polyline2.h>
60 #include <ilqgames/solver/problem.h>
61 #include <ilqgames/utils/initialize_along_route.h>
62 #include <ilqgames/utils/solver_log.h>
63 #include <ilqgames/utils/strategy.h>
64 #include <ilqgames/utils/types.h>
65 
66 #include <math.h>
67 #include <memory>
68 #include <vector>
69 
70 namespace ilqgames {
71 
72 namespace {
73 
74 // Car inter-axle distance.
75 static constexpr float kInterAxleLength = 4.0; // m
76 
77 // Cost weights.
78 static constexpr float kUnicycleAuxCostWeight = 500.0;
79 static constexpr float kCarAuxCostWeight = 500.0;
80 static constexpr float kOmegaCostWeight = 50.0;
81 static constexpr float kJerkCostWeight = 50.0;
82 
83 static constexpr float kACostWeight = 5.0;
84 static constexpr float kCurvatureCostWeight = 1.0;
85 static constexpr float kMaxVCostWeight = 10.0;
86 static constexpr float kNominalVCostWeight = 10.0;
87 
88 static constexpr float kGoalCostWeight = 0.1;
89 static constexpr float kLaneCostWeight = 25.0;
90 static constexpr float kLaneBoundaryCostWeight = 100.0;
91 
92 static constexpr float kMinProximity = 6.0;
93 static constexpr float kP1ProximityCostWeight = 100.0;
94 static constexpr float kP2ProximityCostWeight = 100.0;
95 static constexpr float kP3ProximityCostWeight = 10.0;
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 // Goal points.
104 static constexpr float kP1GoalX = -6.0; // m
105 static constexpr float kP1GoalY = 600.0; // m
106 
107 static constexpr float kP2GoalX = 500.0; // m
108 static constexpr float kP2GoalY = 12.0; // m
109 
110 static constexpr float kP3GoalX = 100.0; // m
111 static constexpr float kP3GoalY = 16.0; // m
112 
113 // Nominal and max speed.
114 static constexpr float kP1MaxV = 12.0; // m/s
115 static constexpr float kP2MaxV = 12.0; // m/s
116 static constexpr float kP3MaxV = 2.0; // m/s
117 static constexpr float kMinV = 1.0; // m/s
118 
119 static constexpr float kP1NominalV = 8.0; // m/s
120 static constexpr float kP2NominalV = 5.0; // m/s
121 static constexpr float kP3NominalV = 1.5; // m/s
122 
123 // Initial state.
124 static constexpr float kP1InitialX = -2.0; // m
125 static constexpr float kP2InitialX = -10.0; // m
126 static constexpr float kP3InitialX = -11.0; // m
127 
128 static constexpr float kP1InitialY = -30.0; // m
129 static constexpr float kP2InitialY = 45.0; // m
130 static constexpr float kP3InitialY = 16.0; // m
131 
132 static constexpr float kP1InitialHeading = M_PI_2; // rad
133 static constexpr float kP2InitialHeading = -M_PI_2; // rad
134 static constexpr float kP3InitialHeading = 0.0; // rad
135 
136 static constexpr float kP1InitialSpeed = 5.0; // m/s
137 static constexpr float kP2InitialSpeed = 5.0; // m/s
138 static constexpr float kP3InitialSpeed = 1.25; // m/s
139 
140 // State dimensions.
141 using P1 = SinglePlayerFlatCar6D;
142 using P2 = SinglePlayerFlatCar6D;
143 using P3 = SinglePlayerFlatUnicycle4D;
144 
145 static const Dimension kP1XIdx = P1::kPxIdx;
146 static const Dimension kP1YIdx = P1::kPyIdx;
147 static const Dimension kP1HeadingIdx = P1::kThetaIdx;
148 static const Dimension kP1PhiIdx = P1::kPhiIdx;
149 static const Dimension kP1VIdx = P1::kVIdx;
150 static const Dimension kP1AIdx = P1::kAIdx;
151 static const Dimension kP1VxIdx = P1::kVxIdx;
152 static const Dimension kP1VyIdx = P1::kVyIdx;
153 static const Dimension kP1AxIdx = P1::kAxIdx;
154 static const Dimension kP1AyIdx = P1::kAyIdx;
155 
156 static const Dimension kP2XIdx = P1::kNumXDims + P2::kPxIdx;
157 static const Dimension kP2YIdx = P1::kNumXDims + P2::kPyIdx;
158 static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kThetaIdx;
159 static const Dimension kP2PhiIdx = P1::kNumXDims + P2::kPhiIdx;
160 static const Dimension kP2VIdx = P1::kNumXDims + P2::kVIdx;
161 static const Dimension kP2AIdx = P1::kNumXDims + P2::kAIdx;
162 static const Dimension kP2VxIdx = P1::kNumXDims + P2::kVxIdx;
163 static const Dimension kP2VyIdx = P1::kNumXDims + P2::kVyIdx;
164 static const Dimension kP2AxIdx = P1::kNumXDims + P2::kAxIdx;
165 static const Dimension kP2AyIdx = P1::kNumXDims + P2::kAyIdx;
166 
167 static const Dimension kP3XIdx = P1::kNumXDims + P2::kNumXDims + P3::kPxIdx;
168 static const Dimension kP3YIdx = P1::kNumXDims + P2::kNumXDims + P3::kPyIdx;
169 static const Dimension kP3HeadingIdx =
170  P1::kNumXDims + P2::kNumXDims + P3::kThetaIdx;
171 static const Dimension kP3VIdx = P1::kNumXDims + P2::kNumXDims + P3::kVIdx;
172 static const Dimension kP3VxIdx = P1::kNumXDims + P2::kNumXDims + P3::kVxIdx;
173 static const Dimension kP3VyIdx = P1::kNumXDims + P2::kNumXDims + P3::kVyIdx;
174 
175 // Control dimensions.
176 static const Dimension kP1OmegaIdx = 0;
177 static const Dimension kP1JerkIdx = 1;
178 static const Dimension kP2OmegaIdx = 0;
179 static const Dimension kP2JerkIdx = 1;
180 static const Dimension kP3OmegaIdx = 0;
181 static const Dimension kP3AIdx = 1;
182 
183 } // anonymous namespace
184 
185 void ThreePlayerFlatIntersectionExample::ConstructDynamics() {
186  dynamics_.reset(new ConcatenatedFlatSystem(
187  {std::make_shared<P1>(kInterAxleLength),
188  std::make_shared<P2>(kInterAxleLength), std::make_shared<P3>()}));
189 }
190 
191 void ThreePlayerFlatIntersectionExample::ConstructInitialState() {
192  VectorXf x0 = VectorXf::Zero(dynamics_->XDim());
193  x0(kP1XIdx) = kP1InitialX;
194  x0(kP1YIdx) = kP1InitialY;
195  x0(kP1HeadingIdx) = kP1InitialHeading;
196  x0(kP1VIdx) = kP1InitialSpeed;
197  x0(kP2XIdx) = kP2InitialX;
198  x0(kP2YIdx) = kP2InitialY;
199  x0(kP2HeadingIdx) = kP2InitialHeading;
200  x0(kP2VIdx) = kP2InitialSpeed;
201  x0(kP3XIdx) = kP3InitialX;
202  x0(kP3YIdx) = kP3InitialY;
203  x0(kP3HeadingIdx) = kP3InitialHeading;
204  x0(kP3VIdx) = kP3InitialSpeed;
205 
206  x0_ = dynamics_->ToLinearSystemState(x0);
207 }
208 
209 void ThreePlayerFlatIntersectionExample::ConstructPlayerCosts() {
210  // Set up costs for all players.
211  player_costs_.emplace_back("P1");
212  player_costs_.emplace_back("P2");
213  player_costs_.emplace_back("P3");
214  auto& p1_cost = player_costs_[0];
215  auto& p2_cost = player_costs_[1];
216  auto& p3_cost = player_costs_[2];
217 
218  // Stay in lanes.
219  const Polyline2 lane1(
220  {Point2(kP1InitialX, -1000.0), Point2(kP1InitialX, 1000.0)});
221  const Polyline2 lane2(
222  {Point2(kP2InitialX, 1000.0), Point2(kP2InitialX, 18.0),
223  Point2(kP2InitialX + 0.5, 15.0), Point2(kP2InitialX + 1.0, 14.0),
224  Point2(kP2InitialX + 3.0, 12.5), Point2(kP2InitialX + 6.0, 12.0),
225  Point2(1000.0, 12.0)});
226  const Polyline2 lane3(
227  {Point2(-1000.0, kP3InitialY), Point2(1000.0, kP3InitialY)});
228 
229  const std::shared_ptr<QuadraticPolyline2Cost> p1_lane_cost(
230  new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP1XIdx, kP1YIdx},
231  "LaneCenter"));
232  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane_r_cost(
233  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
234  {kP1XIdx, kP1YIdx}, kLaneHalfWidth,
235  kOrientedRight, "LaneRightBoundary"));
236  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane_l_cost(
237  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
238  {kP1XIdx, kP1YIdx}, -kLaneHalfWidth,
239  !kOrientedRight, "LaneLeftBoundary"));
240  p1_cost.AddStateCost(p1_lane_cost);
241  p1_cost.AddStateCost(p1_lane_r_cost);
242  p1_cost.AddStateCost(p1_lane_l_cost);
243 
244  const std::shared_ptr<QuadraticPolyline2Cost> p2_lane_cost(
245  new QuadraticPolyline2Cost(kLaneCostWeight, lane2, {kP2XIdx, kP2YIdx},
246  "LaneCenter"));
247  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_r_cost(
248  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
249  {kP2XIdx, kP2YIdx}, kLaneHalfWidth,
250  kOrientedRight, "LaneRightBoundary"));
251  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_l_cost(
252  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
253  {kP2XIdx, kP2YIdx}, -kLaneHalfWidth,
254  !kOrientedRight, "LaneLeftBoundary"));
255  p2_cost.AddStateCost(p2_lane_cost);
256  p2_cost.AddStateCost(p2_lane_r_cost);
257  p2_cost.AddStateCost(p2_lane_l_cost);
258 
259  const std::shared_ptr<QuadraticPolyline2Cost> p3_lane_cost(
260  new QuadraticPolyline2Cost(kLaneCostWeight, lane3, {kP3XIdx, kP3YIdx},
261  "LaneCenter"));
262  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_r_cost(
263  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3,
264  {kP3XIdx, kP3YIdx}, kLaneHalfWidth,
265  kOrientedRight, "LaneRightBoundary"));
266  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_l_cost(
267  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3,
268  {kP3XIdx, kP3YIdx}, -kLaneHalfWidth,
269  !kOrientedRight, "LaneLeftBoundary"));
270  p3_cost.AddStateCost(p3_lane_cost);
271  p3_cost.AddStateCost(p3_lane_r_cost);
272  p3_cost.AddStateCost(p3_lane_l_cost);
273 
274  // Max/min/nominal speed costs.
275  const std::shared_ptr<SemiquadraticNormCost> p1_min_v_cost(
276  new SemiquadraticNormCost(kMaxVCostWeight, {kP1VxIdx, kP1VyIdx}, kMinV,
277  !kOrientedRight, "MinV"));
278  const std::shared_ptr<SemiquadraticNormCost> p1_max_v_cost(
279  new SemiquadraticNormCost(kMaxVCostWeight, {kP1VxIdx, kP1VyIdx}, kP1MaxV,
280  kOrientedRight, "MaxV"));
281  const std::shared_ptr<QuadraticNormCost> p1_nominal_v_cost(
282  new QuadraticNormCost(kNominalVCostWeight, {kP1VxIdx, kP1VyIdx},
283  kP1NominalV, "NominalV"));
284  p1_cost.AddStateCost(p1_min_v_cost);
285  p1_cost.AddStateCost(p1_max_v_cost);
286  p1_cost.AddStateCost(p1_nominal_v_cost);
287 
288  const std::shared_ptr<SemiquadraticNormCost> p2_min_v_cost(
289  new SemiquadraticNormCost(kMaxVCostWeight, {kP2VxIdx, kP2VyIdx}, kMinV,
290  !kOrientedRight, "MinV"));
291  const std::shared_ptr<SemiquadraticNormCost> p2_max_v_cost(
292  new SemiquadraticNormCost(kMaxVCostWeight, {kP2VxIdx, kP2VyIdx}, kP2MaxV,
293  kOrientedRight, "MaxV"));
294  const std::shared_ptr<QuadraticNormCost> p2_nominal_v_cost(
295  new QuadraticNormCost(kNominalVCostWeight, {kP2VxIdx, kP2VyIdx},
296  kP2NominalV, "NominalV"));
297  p2_cost.AddStateCost(p2_min_v_cost);
298  p2_cost.AddStateCost(p2_max_v_cost);
299  p2_cost.AddStateCost(p2_nominal_v_cost);
300 
301  const std::shared_ptr<SemiquadraticNormCost> p3_min_v_cost(
302  new SemiquadraticNormCost(kMaxVCostWeight, {kP3VxIdx, kP3VyIdx}, kMinV,
303  !kOrientedRight, "MinV"));
304  const std::shared_ptr<SemiquadraticNormCost> p3_max_v_cost(
305  new SemiquadraticNormCost(kMaxVCostWeight, {kP3VxIdx, kP3VyIdx}, kP3MaxV,
306  kOrientedRight, "MaxV"));
307  const std::shared_ptr<QuadraticNormCost> p3_nominal_v_cost(
308  new QuadraticNormCost(kNominalVCostWeight, {kP3VxIdx, kP3VyIdx},
309  kP3NominalV, "NominalV"));
310  p3_cost.AddStateCost(p3_min_v_cost);
311  p3_cost.AddStateCost(p3_max_v_cost);
312  p3_cost.AddStateCost(p3_nominal_v_cost);
313 
314  // Penalize control effort.
315  constexpr Dimension kApplyInAllDimensions = -1;
316  const auto unicycle_aux_cost = std::make_shared<QuadraticCost>(
317  kUnicycleAuxCostWeight, kApplyInAllDimensions, 0.0, "Auxiliary Input");
318  const auto car_aux_cost = std::make_shared<QuadraticCost>(
319  kCarAuxCostWeight, kApplyInAllDimensions, 0.0, "Auxiliary Input");
320  p1_cost.AddControlCost(0, car_aux_cost);
321  p2_cost.AddControlCost(1, car_aux_cost);
322  p3_cost.AddControlCost(2, unicycle_aux_cost);
323 
324  // Pairwise proximity costs.
325  const std::shared_ptr<ProxCost> p1p2_proximity_cost(
326  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
327  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
328  const std::shared_ptr<ProxCost> p1p3_proximity_cost(
329  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
330  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
331  p1_cost.AddStateCost(p1p2_proximity_cost);
332  p1_cost.AddStateCost(p1p3_proximity_cost);
333 
334  const std::shared_ptr<ProxCost> p2p1_proximity_cost(
335  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
336  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
337  const std::shared_ptr<ProxCost> p2p3_proximity_cost(
338  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
339  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
340  p2_cost.AddStateCost(p2p1_proximity_cost);
341  p2_cost.AddStateCost(p2p3_proximity_cost);
342 
343  const std::shared_ptr<ProxCost> p3p1_proximity_cost(
344  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
345  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
346  const std::shared_ptr<ProxCost> p3p2_proximity_cost(
347  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
348  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
349  p3_cost.AddStateCost(p3p1_proximity_cost);
350  p3_cost.AddStateCost(p3p2_proximity_cost);
351 }
352 
353 inline std::vector<float> ThreePlayerFlatIntersectionExample::Xs(
354  const VectorXf& xi) const {
355  return {xi(kP1XIdx), xi(kP2XIdx), xi(kP3XIdx)};
356 }
357 
358 inline std::vector<float> ThreePlayerFlatIntersectionExample::Ys(
359  const VectorXf& xi) const {
360  return {xi(kP1YIdx), xi(kP2YIdx), xi(kP3YIdx)};
361 }
362 
363 inline std::vector<float> ThreePlayerFlatIntersectionExample::Thetas(
364  const VectorXf& xi) const {
365  const VectorXf x = dynamics_->FromLinearSystemState(xi);
366  return {x(kP1HeadingIdx), x(kP2HeadingIdx), x(kP3HeadingIdx)};
367 }
368 
369 } // namespace ilqgames
Generated by   doxygen 1.8.11