ilqgames
A new real-time solver for large-scale differential games.
three_player_overtaking_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 overtaking 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/orientation_cost.h>
49 #include <ilqgames/cost/proximity_cost.h>
50 #include <ilqgames/cost/quadratic_cost.h>
51 #include <ilqgames/cost/quadratic_polyline2_cost.h>
52 #include <ilqgames/cost/semiquadratic_cost.h>
53 #include <ilqgames/cost/semiquadratic_polyline2_cost.h>
54 #include <ilqgames/cost/weighted_convex_proximity_cost.h>
55 #include <ilqgames/dynamics/concatenated_dynamical_system.h>
56 #include <ilqgames/dynamics/single_player_car_6d.h>
57 #include <ilqgames/dynamics/single_player_unicycle_4d.h>
58 #include <ilqgames/examples/three_player_overtaking_example.h>
59 #include <ilqgames/geometry/polyline2.h>
60 #include <ilqgames/solver/ilq_solver.h>
61 #include <ilqgames/solver/lq_feedback_solver.h>
62 #include <ilqgames/solver/problem.h>
63 #include <ilqgames/solver/solver_params.h>
64 #include <ilqgames/utils/solver_log.h>
65 #include <ilqgames/utils/strategy.h>
66 #include <ilqgames/utils/types.h>
67 
68 #include <math.h>
69 #include <memory>
70 #include <vector>
71 
72 namespace ilqgames {
73 
74 namespace {
75 
76 // Car inter-axle distance.
77 static constexpr float kInterAxleLength = 4.0; // m
78 
79 // Cost weights.
80 static constexpr float kOmegaCostWeight = 500000.0;
81 static constexpr float kJerkCostWeight = 500.0;
82 
83 static constexpr float kACostWeight = 50.0;
84 static constexpr float kP1NominalVCostWeight = 10.0;
85 static constexpr float kP2NominalVCostWeight = 1.0;
86 static constexpr float kP3NominalVCostWeight = 1.0;
87 
88 static constexpr float kLaneCostWeight = 25.0;
89 static constexpr float kLaneBoundaryCostWeight = 100.0;
90 
91 static constexpr float kMinProximity = 5.0;
92 static constexpr float kP1ProximityCostWeight = 100.0;
93 static constexpr float kP2ProximityCostWeight = 100.0;
94 static constexpr float kP3ProximityCostWeight = 100.0;
95 using ProxCost = ProximityCost;
96 
97 // Heading weight
98 static constexpr float kNominalHeadingCostWeight = 150.0;
99 
100 static constexpr bool kOrientedRight = true;
101 
102 // Lane width.
103 static constexpr float kLaneHalfWidth = 2.5; // m
104 
105 // Nominal speed.
106 static constexpr float kP1NominalV = 15.0; // m/s
107 static constexpr float kP2NominalV = 10.0; // m/s
108 static constexpr float kP3NominalV = 10.0; // m/s
109 
110 // Nominal heading
111 static constexpr float kP1NominalHeading = M_PI_2; // rad
112 
113 // Initial state.
114 static constexpr float kP1InitialX = 2.5; // m
115 static constexpr float kP1InitialY = -10.0; // m
116 
117 static constexpr float kP2InitialX = -1.0; // m
118 static constexpr float kP2InitialY = -10.0; // m
119 
120 static constexpr float kP3InitialX = 2.5; // m
121 static constexpr float kP3InitialY = 10.0; // m
122 
123 static constexpr float kP1InitialHeading = M_PI_2; // rad
124 static constexpr float kP2InitialHeading = M_PI_2; // rad
125 static constexpr float kP3InitialHeading = M_PI_2; // rad
126 
127 static constexpr float kP1InitialSpeed = 10.0; // m/s
128 static constexpr float kP2InitialSpeed = 2.0; // m/s
129 static constexpr float kP3InitialSpeed = 2.0; // m/s
130 
131 // State dimensions.
132 using P1 = SinglePlayerCar6D;
133 using P2 = SinglePlayerCar6D;
134 using P3 = SinglePlayerCar6D;
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 static const Dimension kP1AIdx = P1::kAIdx;
143 
144 static const Dimension kP2XIdx = P1::kNumXDims + P2::kPxIdx;
145 static const Dimension kP2YIdx = P1::kNumXDims + P2::kPyIdx;
146 static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kThetaIdx;
147 static const Dimension kP2PhiIdx = P1::kNumXDims + P2::kPhiIdx;
148 static const Dimension kP2VIdx = P1::kNumXDims + P2::kVIdx;
149 static const Dimension kP2AIdx = P1::kNumXDims + P2::kAIdx;
150 
151 // static const Dimension kP2XIdx = P1::kNumXDims + P2::kNumXDims + P2::kPxIdx;
152 // static const Dimension kP2YIdx = P1::kNumXDims + P2::kNumXDims + P2::kPyIdx;
153 // static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kNumXDims +
154 // P2::kThetaIdx; static const Dimension kP2PhiIdx = P1::kNumXDims +
155 // P2::kNumXDims + P2::kPhiIdx; static const Dimension kP2VIdx = P1::kNumXDims +
156 // P2::kNumXDims + P2::kVIdx; static const Dimension kP2AIdx = P1::kNumXDims +
157 // P2::kNumXDims + P2::kAIdx;
158 
159 static const Dimension kP3XIdx = P1::kNumXDims + P2::kNumXDims + P3::kPxIdx;
160 static const Dimension kP3YIdx = P1::kNumXDims + P2::kNumXDims + P3::kPyIdx;
161 static const Dimension kP3HeadingIdx =
162  P1::kNumXDims + P2::kNumXDims + P3::kThetaIdx;
163 static const Dimension kP3VIdx = P1::kNumXDims + P2::kNumXDims + P3::kVIdx;
164 
165 // Control dimensions.
166 static const Dimension kP1OmegaIdx = 0;
167 static const Dimension kP1JerkIdx = 1;
168 static const Dimension kP2OmegaIdx = 0;
169 static const Dimension kP2JerkIdx = 1;
170 static const Dimension kP3OmegaIdx = 0;
171 static const Dimension kP3JerkIdx = 1;
172 } // anonymous namespace
173 
174 void ThreePlayerOvertakingExample::ConstructDynamics() {
175  dynamics_.reset(new ConcatenatedDynamicalSystem(
176  {std::make_shared<SinglePlayerCar6D>(kInterAxleLength),
177  std::make_shared<SinglePlayerCar6D>(kInterAxleLength),
178  std::make_shared<SinglePlayerCar6D>(kInterAxleLength)}));
179 }
180 
181 void ThreePlayerOvertakingExample::ConstructInitialState() {
182  x0_ = VectorXf::Zero(dynamics_->XDim());
183  x0_(kP1XIdx) = kP1InitialX;
184  x0_(kP1YIdx) = kP1InitialY;
185  x0_(kP1HeadingIdx) = kP1InitialHeading;
186  x0_(kP1VIdx) = kP1InitialSpeed;
187  x0_(kP2XIdx) = kP2InitialX;
188  x0_(kP2YIdx) = kP2InitialY;
189  x0_(kP2HeadingIdx) = kP2InitialHeading;
190  x0_(kP2VIdx) = kP2InitialSpeed;
191  x0_(kP3XIdx) = kP3InitialX;
192  x0_(kP3YIdx) = kP3InitialY;
193  x0_(kP3HeadingIdx) = kP3InitialHeading;
194  x0_(kP3VIdx) = kP3InitialSpeed;
195 }
196 
197 void ThreePlayerOvertakingExample::ConstructPlayerCosts() {
198  // Set up costs for all players.
199  player_costs_.emplace_back("P1");
200  player_costs_.emplace_back("P2");
201  player_costs_.emplace_back("P3");
202  auto& p1_cost = player_costs_[0];
203  auto& p2_cost = player_costs_[1];
204  auto& p3_cost = player_costs_[2];
205 
206  // Stay in lanes.
207  const Polyline2 lane1(
208  {Point2(kP2InitialX, -1000.0), Point2(kP2InitialX, 1000.0)});
209  const Polyline2 lane2(
210  {Point2(kP3InitialX, -1000.0), Point2(kP3InitialX, 1000.0)});
211 
212  const std::shared_ptr<QuadraticPolyline2Cost> p1_lane_cost(
213  new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP1XIdx, kP1YIdx},
214  "LaneCenter"));
215  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane_r_cost(
216  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
217  {kP1XIdx, kP1YIdx}, kLaneHalfWidth,
218  kOrientedRight, "LaneRightBoundary"));
219  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane_l_cost(
220  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
221  {kP1XIdx, kP1YIdx}, -kLaneHalfWidth,
222  !kOrientedRight, "LaneLeftBoundary"));
223  p1_cost.AddStateCost(p1_lane_cost);
224  p1_cost.AddStateCost(p1_lane_r_cost);
225  p1_cost.AddStateCost(p1_lane_l_cost);
226 
227  const std::shared_ptr<QuadraticPolyline2Cost> p2_lane_cost(
228  new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP2XIdx, kP2YIdx},
229  "LaneCenter"));
230  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_r_cost(
231  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
232  {kP2XIdx, kP2YIdx}, kLaneHalfWidth,
233  kOrientedRight, "LaneRightBoundary"));
234  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_l_cost(
235  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
236  {kP2XIdx, kP2YIdx}, -kLaneHalfWidth,
237  !kOrientedRight, "LaneLeftBoundary"));
238  p2_cost.AddStateCost(p2_lane_cost);
239  p2_cost.AddStateCost(p2_lane_r_cost);
240  p2_cost.AddStateCost(p2_lane_l_cost);
241 
242  const std::shared_ptr<QuadraticPolyline2Cost> p3_lane_cost(
243  new QuadraticPolyline2Cost(kLaneCostWeight, lane2, {kP3XIdx, kP3YIdx},
244  "LaneCenter"));
245  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_r_cost(
246  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
247  {kP3XIdx, kP3YIdx}, kLaneHalfWidth,
248  kOrientedRight, "LaneRightBoundary"));
249  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_l_cost(
250  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
251  {kP3XIdx, kP3YIdx}, -kLaneHalfWidth,
252  !kOrientedRight, "LaneLeftBoundary"));
253  p3_cost.AddStateCost(p3_lane_cost);
254  p3_cost.AddStateCost(p3_lane_r_cost);
255  p3_cost.AddStateCost(p3_lane_l_cost);
256 
257  // Max/min/nominal speed costs.
258  // const auto p1_min_v_cost = std::make_shared<SemiquadraticCost>(
259  // kMaxVCostWeight, kP1VIdx, kMinV, !kOrientedRight, "MinV");
260  // const auto p1_max_v_cost = std::make_shared<SemiquadraticCost>(
261  // kMaxVCostWeight, kP1VIdx, kP1MaxV, kOrientedRight, "MaxV");
262  const auto p1_nominal_v_cost = std::make_shared<QuadraticCost>(
263  kP1NominalVCostWeight, kP1VIdx, kP1NominalV, "NominalV");
264  // p1_cost.AddStateCost(p1_min_v_cost);
265  // p1_cost.AddStateCost(p1_max_v_cost);
266  p1_cost.AddStateCost(p1_nominal_v_cost);
267 
268  // const auto p2_min_v_cost = std::make_shared<SemiquadraticCost>(
269  // kMaxVCostWeight, kP2VIdx, kMinV, !kOrientedRight, "MinV");
270  // const auto p2_max_v_cost = std::make_shared<SemiquadraticCost>(
271  // kMaxVCostWeight, kP2VIdx, kP2MaxV, kOrientedRight, "MaxV");
272  const auto p2_nominal_v_cost = std::make_shared<QuadraticCost>(
273  kP2NominalVCostWeight, kP2VIdx, kP2NominalV, "NominalV");
274  // p2_cost.AddStateCost(p2_min_v_cost);
275  // p2_cost.AddStateCost(p2_max_v_cost);
276  p2_cost.AddStateCost(p2_nominal_v_cost);
277 
278  // const auto p3_min_v_cost = std::make_shared<SemiquadraticCost>(
279  // kMaxVCostWeight, kP3VIdx, kMinV, !kOrientedRight, "MinV");
280  // const auto p3_max_v_cost = std::make_shared<SemiquadraticCost>(
281  // kMaxVCostWeight, kP3VIdx, kP3MaxV, kOrientedRight, "MaxV");
282  const auto p3_nominal_v_cost = std::make_shared<QuadraticCost>(
283  kP3NominalVCostWeight, kP3VIdx, kP3NominalV, "NominalV");
284  // p3_cost.AddStateCost(p3_min_v_cost);
285  // p3_cost.AddStateCost(p3_max_v_cost);
286  p3_cost.AddStateCost(p3_nominal_v_cost);
287 
288  // Penalize control effort.
289  const auto p1_omega_cost = std::make_shared<QuadraticCost>(
290  kOmegaCostWeight, kP1OmegaIdx, 0.0, "Steering");
291  const auto p1_jerk_cost =
292  std::make_shared<QuadraticCost>(kJerkCostWeight, kP1JerkIdx, 0.0, "Jerk");
293  p1_cost.AddControlCost(0, p1_omega_cost);
294  p1_cost.AddControlCost(0, p1_jerk_cost);
295 
296  const auto p2_omega_cost = std::make_shared<QuadraticCost>(
297  kOmegaCostWeight, kP2OmegaIdx, 0.0, "Steering");
298  const auto p2_jerk_cost =
299  std::make_shared<QuadraticCost>(kJerkCostWeight, kP2JerkIdx, 0.0, "Jerk");
300  p2_cost.AddControlCost(1, p2_omega_cost);
301  p2_cost.AddControlCost(1, p2_jerk_cost);
302 
303  const auto p3_omega_cost = std::make_shared<QuadraticCost>(
304  kOmegaCostWeight, kP3OmegaIdx, 0.0, "Steering");
305  const auto p3_a_cost =
306  std::make_shared<QuadraticCost>(kJerkCostWeight, kP3JerkIdx, 0.0, "Jerk");
307  p3_cost.AddControlCost(2, p3_omega_cost);
308  p3_cost.AddControlCost(2, p3_a_cost);
309 
310  // Pairwise proximity costs.
311  const std::shared_ptr<ProxCost> p1p2_proximity_cost(
312  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
313  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
314  const std::shared_ptr<ProxCost> p1p3_proximity_cost(
315  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
316  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
317  p1_cost.AddStateCost(p1p2_proximity_cost);
318  p1_cost.AddStateCost(p1p3_proximity_cost);
319 
320  const std::shared_ptr<ProxCost> p2p1_proximity_cost(
321  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
322  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
323  const std::shared_ptr<ProxCost> p2p3_proximity_cost(
324  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
325  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
326  p2_cost.AddStateCost(p2p1_proximity_cost);
327  p2_cost.AddStateCost(p2p3_proximity_cost);
328 
329  const std::shared_ptr<ProxCost> p3p1_proximity_cost(
330  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
331  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
332  const std::shared_ptr<ProxCost> p3p2_proximity_cost(
333  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
334  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
335  // p3_cost.AddStateCost(p3p1_proximity_cost);
336  // p3_cost.AddStateCost(p3p2_proximity_cost);
337 }
338 
339 inline std::vector<float> ThreePlayerOvertakingExample::Xs(
340  const VectorXf& x) const {
341  return {x(kP1XIdx), x(kP2XIdx), x(kP3XIdx)};
342 }
343 
344 inline std::vector<float> ThreePlayerOvertakingExample::Ys(
345  const VectorXf& x) const {
346  return {x(kP1YIdx), x(kP2YIdx), x(kP3YIdx)};
347 }
348 
349 inline std::vector<float> ThreePlayerOvertakingExample::Thetas(
350  const VectorXf& x) const {
351  return {x(kP1HeadingIdx), x(kP2HeadingIdx), x(kP3HeadingIdx)};
352 }
353 
354 } // namespace ilqgames
Generated by   doxygen 1.8.11