ilqgames
A new real-time solver for large-scale differential games.
two_player_collision_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: Tanmay Gautam ( tgautam23@berkeley.edu )
35  * David Fridovich-Keil ( dfk@eecs.berkeley.edu )
36  */
37 
38 ///////////////////////////////////////////////////////////////////////////////
39 //
40 // Three player collision example.
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/two_player_collision_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 = 5000.0;
81 static constexpr float kJerkCostWeight = 3250.0;
82 
83 static constexpr float kACostWeight = 50.0;
84 static constexpr float kP1NominalVCostWeight = 10.0;
85 static constexpr float kP2NominalVCostWeight = 1.0;
86 
87 static constexpr float kLaneCostWeight = 250.0;
88 static constexpr float kLaneBoundaryCostWeight = 50000.0;
89 
90 static constexpr float kMinProximity = 7.5;
91 static constexpr float kP1ProximityCostWeight = 5000.0;
92 static constexpr float kP2ProximityCostWeight = 5000.0;
93 using ProxCost = ProximityCost;
94 
95 // Heading weight
96 static constexpr float kNominalHeadingCostWeight = 150.0;
97 
98 static constexpr bool kOrientedRight = true;
99 
100 // Lane width.
101 static constexpr float kLaneHalfWidth = 2.5; // m
102 
103 // Nominal speed.
104 static constexpr float kP1NominalV = 5.0; // m/s
105 static constexpr float kP2NominalV = 5.0; // m/s
106 
107 // Nominal heading
108 static constexpr float kP1NominalHeading = M_PI_2; // rad
109 
110 // Initial state.
111 static constexpr float kP1InitialX = 2.5; // m
112 static constexpr float kP1InitialY = -50.0; // m
113 
114 static constexpr float kP2InitialX = 2.5; // m
115 static constexpr float kP2InitialY = 50.0; // m
116 
117 static constexpr float kP1InitialHeading = M_PI_2; // rad
118 static constexpr float kP2InitialHeading = -M_PI_2; // rad
119 
120 static constexpr float kP1InitialSpeed = 10.0; // m/s
121 static constexpr float kP2InitialSpeed = 2.0; // m/s
122 
123 // Goal cost weights
124 static constexpr float kGoalCostWeight = 1000.0;
125 static constexpr float kP1GoalX = 2.5;
126 static constexpr float kP1GoalY = 50.0;
127 static constexpr float kP2GoalX = 2.5;
128 static constexpr float kP2GoalY = -50.0;
129 
130 // State dimensions.
131 using P1 = SinglePlayerCar6D;
132 using P2 = SinglePlayerCar6D;
133 
134 static const Dimension kP1XIdx = P1::kPxIdx;
135 static const Dimension kP1YIdx = P1::kPyIdx;
136 static const Dimension kP1HeadingIdx = P1::kThetaIdx;
137 static const Dimension kP1PhiIdx = P1::kPhiIdx;
138 static const Dimension kP1VIdx = P1::kVIdx;
139 static const Dimension kP1AIdx = P1::kAIdx;
140 
141 static const Dimension kP2XIdx = P1::kNumXDims + P2::kPxIdx;
142 static const Dimension kP2YIdx = P1::kNumXDims + P2::kPyIdx;
143 static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kThetaIdx;
144 static const Dimension kP2PhiIdx = P1::kNumXDims + P2::kPhiIdx;
145 static const Dimension kP2VIdx = P1::kNumXDims + P2::kVIdx;
146 static const Dimension kP2AIdx = P1::kNumXDims + P2::kAIdx;
147 
148 // Control dimensions.
149 static const Dimension kP1OmegaIdx = 0;
150 static const Dimension kP1JerkIdx = 1;
151 static const Dimension kP2OmegaIdx = 0;
152 static const Dimension kP2JerkIdx = 1;
153 
154 } // anonymous namespace
155 
156 void TwoPlayerCollisionExample::ConstructDynamics() {
157  dynamics_.reset(new ConcatenatedDynamicalSystem(
158  {std::make_shared<SinglePlayerCar6D>(kInterAxleLength),
159  std::make_shared<SinglePlayerCar6D>(kInterAxleLength)}));
160 }
161 
162 void TwoPlayerCollisionExample::ConstructInitialState() {
163  x0_ = VectorXf::Zero(dynamics_->XDim());
164  x0_(kP1XIdx) = kP1InitialX;
165  x0_(kP1YIdx) = kP1InitialY;
166  x0_(kP1HeadingIdx) = kP1InitialHeading;
167  x0_(kP1VIdx) = kP1InitialSpeed;
168  x0_(kP2XIdx) = kP2InitialX;
169  x0_(kP2YIdx) = kP2InitialY;
170  x0_(kP2HeadingIdx) = kP2InitialHeading;
171  x0_(kP2VIdx) = kP2InitialSpeed;
172 }
173 
174 void TwoPlayerCollisionExample::ConstructPlayerCosts() {
175  // Set up costs for all players.
176  player_costs_.emplace_back("P1", 1.0, 0.0);
177  player_costs_.emplace_back("P2", 1.0, 0.0);
178  auto& p1_cost = player_costs_[0];
179  auto& p2_cost = player_costs_[1];
180 
181  // cost for deviating from the center of the lane (for both p1 and p2)
182  const Polyline2 lane1_p1p2({Point2(2.5, -50.0), Point2(2.5, 50.0)});
183  const std::shared_ptr<QuadraticPolyline2Cost> p1_lane1_cost(
184  new QuadraticPolyline2Cost(kLaneCostWeight, lane1_p1p2,
185  {kP1XIdx, kP1YIdx}, "LaneCenter"));
186  const std::shared_ptr<QuadraticPolyline2Cost> p2_lane1_cost(
187  new QuadraticPolyline2Cost(kLaneCostWeight * 10, lane1_p1p2,
188  {kP2XIdx, kP2YIdx}, "LaneCenter"));
189  p1_cost.AddStateCost(p1_lane1_cost);
190  p2_cost.AddStateCost(p2_lane1_cost);
191 
192  // cost for leaving the left boundary of the lane (for both p1 and p2)
193  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane1_l_cost(
194  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight * 1000, lane1_p1p2,
195  {kP1XIdx, kP1YIdx}, -kLaneHalfWidth,
196  !kOrientedRight, "LaneLeftBoundary"));
197  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane1_l_cost(
198  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight * 10, lane1_p1p2,
199  {kP2XIdx, kP2YIdx}, -kLaneHalfWidth,
200  !kOrientedRight, "LaneLeftBoundary"));
201  p1_cost.AddStateCost(p1_lane1_l_cost);
202  p2_cost.AddStateCost(p2_lane1_l_cost);
203 
204  // p2 cost of leaving right lane boundary
205  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane1_r_cost(
206  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1_p1p2,
207  {kP2XIdx, kP2YIdx}, kLaneHalfWidth,
208  kOrientedRight, "LaneRightBoundary"));
209  p2_cost.AddStateCost(p2_lane1_r_cost);
210 
211  // p1 right lane boundary cost
212  const Polyline2 lane1_p1({Point2(2.5 + kLaneHalfWidth, -50.0),
213  Point2(2.5 + kLaneHalfWidth, -5.0)});
214  const Polyline2 lane2_p1(
215  {Point2(2.5 + kLaneHalfWidth, 5.0), Point2(2.5 + kLaneHalfWidth, 50.0)});
216  const Polyline2 lane3_p1({Point2(10.0, -5.0), Point2(10.0, 5.0)});
217  const Polyline2 lane4_p1(
218  {Point2(2.5 + kLaneHalfWidth, 5.0), Point2(25.0, 5.0)});
219  const Polyline2 lane5_p1(
220  {Point2(2.5 + kLaneHalfWidth, -5.0), Point2(25, -5.0)});
221  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane2_r_cost(
222  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1_p1,
223  {kP1XIdx, kP1YIdx}, 0.0, kOrientedRight,
224  "LaneRightBoundary_lane1_p1"));
225  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane3_r_cost(
226  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2_p1,
227  {kP1XIdx, kP1YIdx}, 0.0, kOrientedRight,
228  "LaneRightBoundary_lane2_p1"));
229  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane4_r_cost(
230  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3_p1,
231  {kP1XIdx, kP1YIdx}, 0.0, kOrientedRight,
232  "LaneRightBoundary_lane3_p1"));
233  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane5_l_cost(
234  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane4_p1,
235  {kP1XIdx, kP1YIdx}, 0.0, !kOrientedRight,
236  "LaneLeftBoundary_lane4_p1"));
237  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane6_r_cost(
238  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane5_p1,
239  {kP1XIdx, kP1YIdx}, 0.0, kOrientedRight,
240  "LaneRightBoundary_lane5_p1"));
241  p1_cost.AddStateCost(p1_lane2_r_cost);
242  p1_cost.AddStateCost(p1_lane3_r_cost);
243  p1_cost.AddStateCost(p1_lane4_r_cost);
244  p1_cost.AddStateCost(p1_lane5_l_cost);
245  p1_cost.AddStateCost(p1_lane6_r_cost);
246 
247  // Max/min/nominal speed costs.
248  // const auto p1_min_v_cost = std::make_shared<SemiquadraticCost>(
249  // kMaxVCostWeight, kP1VIdx, kMinV, !kOrientedRight, "MinV");
250  // const auto p1_max_v_cost = std::make_shared<SemiquadraticCost>(
251  // kMaxVCostWeight, kP1VIdx, kP1MaxV, kOrientedRight, "MaxV");
252  const auto p1_nominal_v_cost = std::make_shared<QuadraticCost>(
253  kP1NominalVCostWeight, kP1VIdx, kP1NominalV, "NominalV");
254  // p1_cost.AddStateCost(p1_min_v_cost);
255  // p1_cost.AddStateCost(p1_max_v_cost);
256  p1_cost.AddStateCost(p1_nominal_v_cost);
257 
258  // const auto p2_min_v_cost = std::make_shared<SemiquadraticCost>(
259  // kMaxVCostWeight, kP2VIdx, kMinV, !kOrientedRight, "MinV");
260  // const auto p2_max_v_cost = std::make_shared<SemiquadraticCost>(
261  // kMaxVCostWeight, kP2VIdx, kP2MaxV, kOrientedRight, "MaxV");
262  const auto p2_nominal_v_cost = std::make_shared<QuadraticCost>(
263  kP2NominalVCostWeight, kP2VIdx, kP2NominalV, "NominalV");
264  // p2_cost.AddStateCost(p2_min_v_cost);
265  // p2_cost.AddStateCost(p2_max_v_cost);
266  p2_cost.AddStateCost(p2_nominal_v_cost);
267 
268  // Penalize control effort.
269  const auto p1_omega_cost = std::make_shared<QuadraticCost>(
270  kOmegaCostWeight, kP1OmegaIdx, 0.0, "Steering");
271  const auto p1_jerk_cost =
272  std::make_shared<QuadraticCost>(kJerkCostWeight, kP1JerkIdx, 0.0, "Jerk");
273  p1_cost.AddControlCost(0, p1_omega_cost);
274  p1_cost.AddControlCost(0, p1_jerk_cost);
275 
276  const auto p2_omega_cost = std::make_shared<QuadraticCost>(
277  kOmegaCostWeight, kP2OmegaIdx, 0.0, "Steering");
278  const auto p2_jerk_cost =
279  std::make_shared<QuadraticCost>(kJerkCostWeight, kP2JerkIdx, 0.0, "Jerk");
280  p2_cost.AddControlCost(1, p2_omega_cost);
281  p2_cost.AddControlCost(1, p2_jerk_cost);
282 
283  // Goal costs.
284  constexpr float kFinalTimeWindow = 0.5; // s
285  const auto p1_goalx_cost = std::make_shared<FinalTimeCost>(
286  std::make_shared<QuadraticCost>(kGoalCostWeight, kP1XIdx, kP1GoalX),
287  time::kTimeHorizon - kFinalTimeWindow, "GoalX");
288  const auto p1_goaly_cost = std::make_shared<FinalTimeCost>(
289  std::make_shared<QuadraticCost>(kGoalCostWeight, kP1YIdx, kP1GoalY),
290  time::kTimeHorizon - kFinalTimeWindow, "GoalY");
291  p1_cost.AddStateCost(p1_goalx_cost);
292  p1_cost.AddStateCost(p1_goaly_cost);
293 
294  const auto p2_goalx_cost = std::make_shared<FinalTimeCost>(
295  std::make_shared<QuadraticCost>(kGoalCostWeight, kP2XIdx, kP2GoalX),
296  time::kTimeHorizon - kFinalTimeWindow, "GoalX");
297  const auto p2_goaly_cost = std::make_shared<FinalTimeCost>(
298  std::make_shared<QuadraticCost>(kGoalCostWeight, kP2YIdx, kP2GoalY),
299  time::kTimeHorizon - kFinalTimeWindow, "GoalY");
300  p2_cost.AddStateCost(p2_goalx_cost);
301  p2_cost.AddStateCost(p2_goaly_cost);
302 
303  // Pairwise proximity costs.
304  const std::shared_ptr<ProxCost> p1p2_proximity_cost(
305  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
306  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
307  p1_cost.AddStateCost(p1p2_proximity_cost);
308 
309  const std::shared_ptr<ProxCost> p2p1_proximity_cost(
310  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
311  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
312  p2_cost.AddStateCost(p2p1_proximity_cost);
313 }
314 
315 inline std::vector<float> TwoPlayerCollisionExample::Xs(
316  const VectorXf& x) const {
317  return {x(kP1XIdx), x(kP2XIdx)};
318 }
319 
320 inline std::vector<float> TwoPlayerCollisionExample::Ys(
321  const VectorXf& x) const {
322  return {x(kP1YIdx), x(kP2YIdx)};
323 }
324 
325 inline std::vector<float> TwoPlayerCollisionExample::Thetas(
326  const VectorXf& x) const {
327  return {x(kP1HeadingIdx), x(kP2HeadingIdx)};
328 }
329 
330 } // namespace ilqgames
Generated by   doxygen 1.8.11