ilqgames
A new real-time solver for large-scale differential games.
three_player_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 intersection example. Ordering is given by the following:
40 // (P1, P2, P3) = (Car 1, Car 2, Pedestrian).
41 //
42 ///////////////////////////////////////////////////////////////////////////////
43 
44 #include <ilqgames/constraint/polyline2_signed_distance_constraint.h>
45 #include <ilqgames/constraint/proximity_constraint.h>
46 #include <ilqgames/constraint/single_dimension_constraint.h>
47 #include <ilqgames/cost/curvature_cost.h>
48 #include <ilqgames/cost/final_time_cost.h>
49 #include <ilqgames/cost/locally_convex_proximity_cost.h>
50 #include <ilqgames/cost/nominal_path_length_cost.h>
51 #include <ilqgames/cost/proximity_cost.h>
52 #include <ilqgames/cost/quadratic_cost.h>
53 #include <ilqgames/cost/quadratic_polyline2_cost.h>
54 #include <ilqgames/cost/semiquadratic_cost.h>
55 #include <ilqgames/cost/semiquadratic_polyline2_cost.h>
56 #include <ilqgames/cost/weighted_convex_proximity_cost.h>
57 #include <ilqgames/dynamics/concatenated_dynamical_system.h>
58 #include <ilqgames/dynamics/single_player_car_5d.h>
59 #include <ilqgames/dynamics/single_player_car_6d.h>
60 #include <ilqgames/dynamics/single_player_unicycle_4d.h>
61 #include <ilqgames/examples/three_player_intersection_example.h>
62 #include <ilqgames/geometry/polyline2.h>
63 #include <ilqgames/solver/problem.h>
64 #include <ilqgames/solver/solver_params.h>
65 #include <ilqgames/utils/solver_log.h>
66 #include <ilqgames/utils/strategy.h>
67 #include <ilqgames/utils/types.h>
68 
69 #include <math.h>
70 #include <memory>
71 #include <vector>
72 
73 namespace ilqgames {
74 
75 namespace {
76 
77 // Car inter-axle distance.
78 static constexpr float kInterAxleLength = 4.0; // m
79 
80 // Cost weights.
81 static constexpr float kStateRegularization = 1.0;
82 static constexpr float kControlRegularization = 5.0;
83 
84 static constexpr float kOmegaCostWeight = 0.1;
85 static constexpr float kJerkCostWeight = 0.1;
86 static constexpr float kMaxOmega = 1.0;
87 
88 static constexpr float kACostWeight = 0.1;
89 static constexpr float kNominalVCostWeight = 100.0;
90 
91 static constexpr float kLaneCostWeight = 25.0;
92 
93 static constexpr float kMinProximity = 6.0;
94 using ProxCost = ProximityCost;
95 static constexpr float kP1ProximityCostWeight = 10.0;
96 static constexpr float kP2ProximityCostWeight = 10.0;
97 static constexpr float kP3ProximityCostWeight = 10.0;
98 
99 static constexpr bool kOrientedRight = true;
100 static constexpr bool kBarrierOrientedInside = false;
101 
102 // Lane width.
103 static constexpr float kLaneHalfWidth = 2.5; // m
104 
105 // Nominal and max speed.
106 static constexpr float kP1MaxV = 12.0; // m/s
107 static constexpr float kP2MaxV = 12.0; // m/s
108 static constexpr float kP3MaxV = 2.0; // m/s
109 static constexpr float kMinV = 1.0; // m/s
110 
111 static constexpr float kP1NominalV = 8.0; // m/s
112 static constexpr float kP2NominalV = 5.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 = SinglePlayerCar6D;
134 using P2 = 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 kP3XIdx = P1::kNumXDims + P2::kNumXDims + P3::kPxIdx;
152 static const Dimension kP3YIdx = P1::kNumXDims + P2::kNumXDims + P3::kPyIdx;
153 static const Dimension kP3HeadingIdx =
154  P1::kNumXDims + P2::kNumXDims + P3::kThetaIdx;
155 static const Dimension kP3VIdx = P1::kNumXDims + P2::kNumXDims + P3::kVIdx;
156 
157 // Control dimensions.
158 static const Dimension kP1OmegaIdx = 0;
159 static const Dimension kP1JerkIdx = 1;
160 static const Dimension kP2OmegaIdx = 0;
161 static const Dimension kP2JerkIdx = 1;
162 static const Dimension kP3OmegaIdx = 0;
163 static const Dimension kP3AIdx = 1;
164 } // anonymous namespace
165 
166 void ThreePlayerIntersectionExample::ConstructDynamics() {
167  dynamics_.reset(new ConcatenatedDynamicalSystem(
168  {std::make_shared<P1>(kInterAxleLength),
169  std::make_shared<P2>(kInterAxleLength), std::make_shared<P3>()}));
170 }
171 
172 void ThreePlayerIntersectionExample::ConstructInitialState() {
173  x0_ = VectorXf::Zero(dynamics_->XDim());
174  x0_(kP1XIdx) = kP1InitialX;
175  x0_(kP1YIdx) = kP1InitialY;
176  x0_(kP1HeadingIdx) = kP1InitialHeading;
177  x0_(kP1VIdx) = kP1InitialSpeed;
178  x0_(kP2XIdx) = kP2InitialX;
179  x0_(kP2YIdx) = kP2InitialY;
180  x0_(kP2HeadingIdx) = kP2InitialHeading;
181  x0_(kP2VIdx) = kP2InitialSpeed;
182  x0_(kP3XIdx) = kP3InitialX;
183  x0_(kP3YIdx) = kP3InitialY;
184  x0_(kP3HeadingIdx) = kP3InitialHeading;
185  x0_(kP3VIdx) = kP3InitialSpeed;
186 }
187 
188 void ThreePlayerIntersectionExample::ConstructPlayerCosts() {
189  // Set up costs for all players.
190  player_costs_.emplace_back("P1", kStateRegularization,
191  kControlRegularization);
192  player_costs_.emplace_back("P2", kStateRegularization,
193  kControlRegularization);
194  player_costs_.emplace_back("P3", kStateRegularization,
195  kControlRegularization);
196  auto& p1_cost = player_costs_[0];
197  auto& p2_cost = player_costs_[1];
198  auto& p3_cost = player_costs_[2];
199 
200  // Stay in lanes.
201  const Polyline2 lane1(
202  {Point2(kP1InitialX, -1000.0), Point2(kP1InitialX, 1000.0)});
203  const Polyline2 lane2(
204  {Point2(kP2InitialX, 1000.0), Point2(kP2InitialX, 18.0),
205  Point2(kP2InitialX + 0.5, 15.0), Point2(kP2InitialX + 1.0, 14.0),
206  Point2(kP2InitialX + 3.0, 12.5), Point2(kP2InitialX + 6.0, 12.0),
207  Point2(1000.0, 12.0)});
208  const Polyline2 lane3(
209  {Point2(-1000.0, kP3InitialY), Point2(1000.0, kP3InitialY)});
210 
211  const std::shared_ptr<QuadraticPolyline2Cost> p1_lane_cost(
212  new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP1XIdx, kP1YIdx},
213  "LaneCenter"));
214  const std::shared_ptr<Polyline2SignedDistanceConstraint> p1_lane_r_constraint(
215  new Polyline2SignedDistanceConstraint(lane1, {kP1XIdx, kP1YIdx},
216  kLaneHalfWidth, !kOrientedRight,
217  "LaneRightBoundary"));
218  const std::shared_ptr<Polyline2SignedDistanceConstraint> p1_lane_l_constraint(
219  new Polyline2SignedDistanceConstraint(lane1, {kP1XIdx, kP1YIdx},
220  -kLaneHalfWidth, kOrientedRight,
221  "LaneLeftBoundary"));
222  p1_cost.AddStateCost(p1_lane_cost);
223  // p1_cost.AddStateConstraint(p1_lane_r_constraint);
224  // p1_cost.AddStateConstraint(p1_lane_l_constraint);
225 
226  const std::shared_ptr<QuadraticPolyline2Cost> p2_lane_cost(
227  new QuadraticPolyline2Cost(kLaneCostWeight, lane2, {kP2XIdx, kP2YIdx},
228  "LaneCenter"));
229  const std::shared_ptr<Polyline2SignedDistanceConstraint> p2_lane_r_constraint(
230  new Polyline2SignedDistanceConstraint(lane2, {kP2XIdx, kP2YIdx},
231  kLaneHalfWidth, !kOrientedRight,
232  "LaneRightBoundary"));
233  const std::shared_ptr<Polyline2SignedDistanceConstraint> p2_lane_l_constraint(
234  new Polyline2SignedDistanceConstraint(lane2, {kP2XIdx, kP2YIdx},
235  -kLaneHalfWidth, kOrientedRight,
236  "LaneLeftBoundary"));
237  p2_cost.AddStateCost(p2_lane_cost);
238  // p2_cost.AddStateConstraint(p2_lane_r_constraint);
239  // p2_cost.AddStateConstraint(p2_lane_l_constraint);
240 
241  const std::shared_ptr<QuadraticPolyline2Cost> p3_lane_cost(
242  new QuadraticPolyline2Cost(kLaneCostWeight, lane3, {kP3XIdx, kP3YIdx},
243  "LaneCenter"));
244  const std::shared_ptr<Polyline2SignedDistanceConstraint> p3_lane_r_constraint(
245  new Polyline2SignedDistanceConstraint(lane3, {kP3XIdx, kP3YIdx},
246  kLaneHalfWidth, !kOrientedRight,
247  "LaneRightBoundary"));
248  const std::shared_ptr<Polyline2SignedDistanceConstraint> p3_lane_l_constraint(
249  new Polyline2SignedDistanceConstraint(lane3, {kP3XIdx, kP3YIdx},
250  -kLaneHalfWidth, kOrientedRight,
251  "LaneLeftBoundary"));
252  p3_cost.AddStateCost(p3_lane_cost);
253  // p3_cost.AddStateConstraint(p3_lane_r_constraint);
254  // p3_cost.AddStateConstraint(p3_lane_l_constraint);
255 
256  // Max/min/nominal speed costs.
257  const auto p1_min_v_constraint = std::make_shared<SingleDimensionConstraint>(
258  kP1VIdx, kMinV, !kOrientedRight, "MinV");
259  const auto p1_max_v_constraint = std::make_shared<SingleDimensionConstraint>(
260  kP1VIdx, kP1MaxV, kOrientedRight, "MaxV");
261  const auto p1_nominal_v_cost = std::make_shared<QuadraticCost>(
262  kNominalVCostWeight, kP1VIdx, kP1NominalV, "NominalV");
263  // p1_cost.AddStateConstraint(p1_min_v_constraint);
264  // p1_cost.AddStateConstraint(p1_max_v_constraint);
265  p1_cost.AddStateCost(p1_nominal_v_cost);
266 
267  const auto p2_min_v_constraint = std::make_shared<SingleDimensionConstraint>(
268  kP2VIdx, kMinV, !kOrientedRight, "MinV");
269  const auto p2_max_v_constraint = std::make_shared<SingleDimensionConstraint>(
270  kP2VIdx, kP2MaxV, kOrientedRight, "MaxV");
271  const auto p2_nominal_v_cost = std::make_shared<QuadraticCost>(
272  kNominalVCostWeight, kP2VIdx, kP2NominalV, "NominalV");
273  // p2_cost.AddStateConstraint(p2_min_v_constraint);
274  // p2_cost.AddStateConstraint(p2_max_v_constraint);
275  p2_cost.AddStateCost(p2_nominal_v_cost);
276 
277  const auto p3_min_v_constraint = std::make_shared<SingleDimensionConstraint>(
278  kP3VIdx, kMinV, !kOrientedRight, "MinV");
279  const auto p3_max_v_constraint = std::make_shared<SingleDimensionConstraint>(
280  kP3VIdx, kP3MaxV, kOrientedRight, "MaxV");
281  const auto p3_nominal_v_cost = std::make_shared<QuadraticCost>(
282  kNominalVCostWeight, kP3VIdx, kP3NominalV, "NominalV");
283  // p3_cost.AddStateConstraint(p3_min_v_constraint);
284  // p3_cost.AddStateConstraint(p3_max_v_constraint);
285  p3_cost.AddStateCost(p3_nominal_v_cost);
286 
287  // Penalize control effort.
288  const auto p1_omega_max_constraint =
289  std::make_shared<SingleDimensionConstraint>(
290  kP1OmegaIdx, kMaxOmega, kOrientedRight, "SteeringMax");
291  const auto p1_omega_min_constraint =
292  std::make_shared<SingleDimensionConstraint>(
293  kP1OmegaIdx, -kMaxOmega, !kOrientedRight, "SteeringMin");
294  const auto p1_omega_cost = std::make_shared<QuadraticCost>(
295  kOmegaCostWeight, kP1OmegaIdx, 0.0, "Steering");
296  const auto p1_jerk_cost =
297  std::make_shared<QuadraticCost>(kJerkCostWeight, kP1JerkIdx, 0.0, "Jerk");
298  // p1_cost.AddControlConstraint(0, p1_omega_max_constraint);
299  // p1_cost.AddControlConstraint(0, p1_omega_min_constraint);
300  p1_cost.AddControlCost(0, p1_omega_cost);
301  p1_cost.AddControlCost(0, p1_jerk_cost);
302 
303  const auto p2_omega_max_constraint =
304  std::make_shared<SingleDimensionConstraint>(
305  kP2OmegaIdx, kMaxOmega, !kOrientedRight, "SteeringMax");
306  const auto p2_omega_min_constraint =
307  std::make_shared<SingleDimensionConstraint>(
308  kP2OmegaIdx, -kMaxOmega, kOrientedRight, "SteeringMin");
309  const auto p2_omega_cost = std::make_shared<QuadraticCost>(
310  kOmegaCostWeight, kP2OmegaIdx, 0.0, "Steering");
311  const auto p2_jerk_cost =
312  std::make_shared<QuadraticCost>(kJerkCostWeight, kP2JerkIdx, 0.0, "Jerk");
313  // p2_cost.AddControlConstraint(1, p2_omega_max_constraint);
314  // p2_cost.AddControlConstraint(1, p2_omega_min_constraint);
315  p2_cost.AddControlCost(1, p2_omega_cost);
316  p2_cost.AddControlCost(1, p2_jerk_cost);
317 
318  const auto p3_omega_max_constraint =
319  std::make_shared<SingleDimensionConstraint>(
320  kP3OmegaIdx, kMaxOmega, kOrientedRight, "SteeringMax");
321  const auto p3_omega_min_constraint =
322  std::make_shared<SingleDimensionConstraint>(
323  kP3OmegaIdx, -kMaxOmega, !kOrientedRight, "SteeringMin");
324  const auto p3_omega_cost = std::make_shared<QuadraticCost>(
325  kOmegaCostWeight, kP3OmegaIdx, 0.0, "Steering");
326  const auto p3_a_cost = std::make_shared<QuadraticCost>(kACostWeight, kP3AIdx,
327  0.0, "Acceleration");
328  // p3_cost.AddControlConstraint(2, p3_omega_max_constraint);
329  // p3_cost.AddControlConstraint(2, p3_omega_min_constraint);
330  p3_cost.AddControlCost(2, p3_omega_cost);
331  p3_cost.AddControlCost(2, p3_a_cost);
332 
333  // Pairwise proximity costs.
334  // const std::shared_ptr<ProxCost> p1p2_proximity_cost(
335  // new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
336  // {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
337  // const std::shared_ptr<ProxCost> p1p3_proximity_cost(
338  // new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
339  // {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
340  // p1_cost.AddStateCost(p1p2_proximity_cost);
341  // p1_cost.AddStateCost(p1p3_proximity_cost);
342 
343  // const std::shared_ptr<ProxCost> p2p1_proximity_cost(
344  // new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
345  // {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
346  // const std::shared_ptr<ProxCost> p2p3_proximity_cost(
347  // new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
348  // {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
349  // p2_cost.AddStateCost(p2p1_proximity_cost);
350  // p2_cost.AddStateCost(p2p3_proximity_cost);
351 
352  // const std::shared_ptr<ProxCost> p3p1_proximity_cost(
353  // new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
354  // {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
355  // const std::shared_ptr<ProxCost> p3p2_proximity_cost(
356  // new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
357  // {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
358  // p3_cost.AddStateCost(p3p1_proximity_cost);
359  // p3_cost.AddStateCost(p3p2_proximity_cost);
360 
361  // Collision-avoidance constraints.
362  constexpr bool kKeepClose = true;
363  const std::shared_ptr<ProximityConstraint> p1p2_proximity_constraint(
364  new ProximityConstraint({kP1XIdx, kP1YIdx}, {kP2XIdx, kP2YIdx},
365  kMinProximity, !kKeepClose,
366  "ProximityConstraintP2"));
367  const std::shared_ptr<ProximityConstraint> p1p3_proximity_constraint(
368  new ProximityConstraint({kP1XIdx, kP1YIdx}, {kP3XIdx, kP3YIdx},
369  kMinProximity, !kKeepClose,
370  "ProximityConstraintP3"));
371  p1_cost.AddStateConstraint(p1p2_proximity_constraint);
372  p1_cost.AddStateConstraint(p1p3_proximity_constraint);
373 
374  const std::shared_ptr<ProximityConstraint> p2p1_proximity_constraint(
375  new ProximityConstraint({kP2XIdx, kP2YIdx}, {kP1XIdx, kP1YIdx},
376  kMinProximity, !kKeepClose,
377  "ProximityConstraintP1"));
378  const std::shared_ptr<ProximityConstraint> p2p3_proximity_constraint(
379  new ProximityConstraint({kP2XIdx, kP2YIdx}, {kP3XIdx, kP3YIdx},
380  kMinProximity, !kKeepClose,
381  "ProximityConstraintP3"));
382  p2_cost.AddStateConstraint(p2p1_proximity_constraint);
383  p2_cost.AddStateConstraint(p2p3_proximity_constraint);
384 
385  const std::shared_ptr<ProximityConstraint> p3p1_proximity_constraint(
386  new ProximityConstraint({kP3XIdx, kP3YIdx}, {kP1XIdx, kP1YIdx},
387  kMinProximity, !kKeepClose,
388  "ProximityConstraintP1"));
389  const std::shared_ptr<ProximityConstraint> p3p2_proximity_constraint(
390  new ProximityConstraint({kP3XIdx, kP3YIdx}, {kP2XIdx, kP2YIdx},
391  kMinProximity, !kKeepClose,
392  "ProximityConstraintP2"));
393  p3_cost.AddStateConstraint(p3p1_proximity_constraint);
394  p3_cost.AddStateConstraint(p3p2_proximity_constraint);
395 
396  // Collision-avoidance constraints.
397  // const std::shared_ptr<ProximityConstraint> p1p2_proximity_constraint(
398  // new ProximityConstraint({kP1XIdx, kP1YIdx}, {kP2XIdx, kP2YIdx},
399  // kMinProximity, kConstraintOrientedInside,
400  // "ProximityConstraintP2"));
401  // const std::shared_ptr<ProximityConstraint> p1p3_proximity_constraint(
402  // new ProximityConstraint({kP1XIdx, kP1YIdx}, {kP3XIdx, kP3YIdx},
403  // kMinProximity, kConstraintOrientedInside,
404  // "ProximityConstraintP3"));
405  // p1_cost.AddStateConstraint(p1p2_proximity_constraint);
406  // p1_cost.AddStateConstraint(p1p3_proximity_constraint);
407 
408  // const std::shared_ptr<ProximityConstraint> p2p1_proximity_constraint(
409  // new ProximityConstraint({kP2XIdx, kP2YIdx}, {kP1XIdx, kP1YIdx},
410  // kMinProximity, kConstraintOrientedInside,
411  // "ProximityConstraintP1"));
412  // const std::shared_ptr<ProximityConstraint> p2p3_proximity_constraint(
413  // new ProximityConstraint({kP2XIdx, kP2YIdx}, {kP3XIdx, kP3YIdx},
414  // kMinProximity, kConstraintOrientedInside,
415  // "ProximityConstraintP3"));
416  // p2_cost.AddStateConstraint(p2p1_proximity_constraint);
417  // p2_cost.AddStateConstraint(p2p3_proximity_constraint);
418 
419  // const std::shared_ptr<ProximityConstraint> p3p1_proximity_constraint(
420  // new ProximityConstraint({kP3XIdx, kP3YIdx}, {kP1XIdx, kP1YIdx},
421  // kMinProximity, kConstraintOrientedInside,
422  // "ProximityConstraintP1"));
423  // const std::shared_ptr<ProximityConstraint> p3p2_proximity_constraint(
424  // new ProximityConstraint({kP3XIdx, kP3YIdx}, {kP2XIdx, kP2YIdx},
425  // kMinProximity, kConstraintOrientedInside,
426  // "ProximityConstraintP2"));
427  // p3_cost.AddStateConstraint(p3p1_proximity_constraint);
428  // p3_cost.AddStateConstraint(p3p2_proximity_constraint);
429 }
430 
431 inline std::vector<float> ThreePlayerIntersectionExample::Xs(
432  const VectorXf& x) const {
433  return {x(kP1XIdx), x(kP2XIdx), x(kP3XIdx)};
434 }
435 
436 inline std::vector<float> ThreePlayerIntersectionExample::Ys(
437  const VectorXf& x) const {
438  return {x(kP1YIdx), x(kP2YIdx), x(kP3YIdx)};
439 }
440 
441 inline std::vector<float> ThreePlayerIntersectionExample::Thetas(
442  const VectorXf& x) const {
443  return {x(kP1HeadingIdx), x(kP2HeadingIdx), x(kP3HeadingIdx)};
444 }
445 
446 } // namespace ilqgames
Generated by   doxygen 1.8.11