ilqgames
A new real-time solver for large-scale differential games.
roundabout_merging_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 // Roundabout merging example.
40 //
41 ///////////////////////////////////////////////////////////////////////////////
42 
43 #include <ilqgames/cost/curvature_cost.h>
44 #include <ilqgames/cost/final_time_cost.h>
45 #include <ilqgames/cost/locally_convex_proximity_cost.h>
46 #include <ilqgames/cost/nominal_path_length_cost.h>
47 #include <ilqgames/cost/proximity_cost.h>
48 #include <ilqgames/cost/quadratic_cost.h>
49 #include <ilqgames/cost/quadratic_polyline2_cost.h>
50 #include <ilqgames/cost/semiquadratic_cost.h>
51 #include <ilqgames/cost/semiquadratic_polyline2_cost.h>
52 #include <ilqgames/cost/weighted_convex_proximity_cost.h>
53 #include <ilqgames/dynamics/concatenated_dynamical_system.h>
54 #include <ilqgames/dynamics/single_player_car_6d.h>
55 #include <ilqgames/dynamics/single_player_unicycle_4d.h>
56 #include <ilqgames/examples/roundabout_lane_center.h>
57 #include <ilqgames/examples/roundabout_merging_example.h>
58 #include <ilqgames/geometry/polyline2.h>
59 #include <ilqgames/solver/ilq_solver.h>
60 #include <ilqgames/solver/lq_feedback_solver.h>
61 #include <ilqgames/solver/problem.h>
62 #include <ilqgames/solver/solver_params.h>
63 #include <ilqgames/utils/initialize_along_route.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 // Cost weights.
77 static constexpr float kOmegaCostWeight = 500.0;
78 static constexpr float kACostWeight = 50.0;
79 static constexpr float kJerkCostWeight = 5.0;
80 
81 static constexpr float kMaxVCostWeight = 1000.0;
82 static constexpr float kNominalVCostWeight = 10.0;
83 
84 static constexpr float kLaneCostWeight = 25.0;
85 static constexpr float kLaneBoundaryCostWeight = 100.0;
86 
87 static constexpr float kMinProximity = 6.0;
88 static constexpr float kP1ProximityCostWeight = 100.0;
89 static constexpr float kP2ProximityCostWeight = 100.0;
90 static constexpr float kP3ProximityCostWeight = 100.0;
91 static constexpr float kP4ProximityCostWeight = 100.0;
92 using ProxCost = ProximityCost;
93 
94 static constexpr bool kOrientedRight = true;
95 
96 // Lane width.
97 static constexpr float kLaneHalfWidth = 2.5; // m
98 
99 // Nominal and max speed.
100 static constexpr float kP1MaxV = 12.0; // m/s
101 static constexpr float kP2MaxV = 12.0; // m/s
102 static constexpr float kP3MaxV = 12.0; // m/s
103 static constexpr float kP4MaxV = 12.0; // m/s
104 static constexpr float kMinV = 1.0; // m/s
105 
106 static constexpr float kP1NominalV = 10.0; // m/s
107 static constexpr float kP2NominalV = 10.0; // m/s
108 static constexpr float kP3NominalV = 10.0; // m/s
109 static constexpr float kP4NominalV = 10.0; // m/s
110 
111 // Initial distance from roundabout.
112 static constexpr float kP1InitialDistanceToRoundabout = 25.0; // m
113 static constexpr float kP2InitialDistanceToRoundabout = 10.0; // m
114 static constexpr float kP3InitialDistanceToRoundabout = 25.0; // m
115 static constexpr float kP4InitialDistanceToRoundabout = 10.0; // m
116 
117 static constexpr float kP1InitialSpeed = 3.0; // m/s
118 static constexpr float kP2InitialSpeed = 2.0; // m/s
119 static constexpr float kP3InitialSpeed = 3.0; // m/s
120 static constexpr float kP4InitialSpeed = 2.0; // m/s
121 
122 // State dimensions.
123 static constexpr float kInterAxleDistance = 4.0; // m
124 using P1 = SinglePlayerCar6D;
125 using P2 = SinglePlayerCar6D;
126 using P3 = SinglePlayerCar6D;
127 using P4 = SinglePlayerCar6D;
128 
129 static const Dimension kP1XIdx = P1::kPxIdx;
130 static const Dimension kP1YIdx = P1::kPyIdx;
131 static const Dimension kP1HeadingIdx = P1::kThetaIdx;
132 static const Dimension kP1VIdx = P1::kVIdx;
133 static const Dimension kP1AIdx = P1::kAIdx;
134 
135 static const Dimension kP2XIdx = P1::kNumXDims + P2::kPxIdx;
136 static const Dimension kP2YIdx = P1::kNumXDims + P2::kPyIdx;
137 static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kThetaIdx;
138 static const Dimension kP2VIdx = P1::kNumXDims + P2::kVIdx;
139 static const Dimension kP2AIdx = P1::kNumXDims + P2::kAIdx;
140 
141 static const Dimension kP3XIdx = P1::kNumXDims + P2::kNumXDims + P3::kPxIdx;
142 static const Dimension kP3YIdx = P1::kNumXDims + P2::kNumXDims + P3::kPyIdx;
143 static const Dimension kP3HeadingIdx =
144  P1::kNumXDims + P2::kNumXDims + P3::kThetaIdx;
145 static const Dimension kP3VIdx = P1::kNumXDims + P2::kNumXDims + P3::kVIdx;
146 static const Dimension kP3AIdx = P1::kNumXDims + P2::kNumXDims + P3::kAIdx;
147 
148 static const Dimension kP4XIdx =
149  P1::kNumXDims + P2::kNumXDims + P3::kNumXDims + P4::kPxIdx;
150 static const Dimension kP4YIdx =
151  P1::kNumXDims + P2::kNumXDims + P3::kNumXDims + P4::kPyIdx;
152 static const Dimension kP4HeadingIdx =
153  P1::kNumXDims + P2::kNumXDims + P3::kNumXDims + P4::kThetaIdx;
154 static const Dimension kP4VIdx =
155  P1::kNumXDims + P2::kNumXDims + P3::kNumXDims + P4::kVIdx;
156 static const Dimension kP4AIdx =
157  P1::kNumXDims + P2::kNumXDims + P3::kNumXDims + P4::kAIdx;
158 
159 // Control dimensions.
160 static const Dimension kP1OmegaIdx = 0;
161 static const Dimension kP1JerkIdx = 1;
162 static const Dimension kP2OmegaIdx = 0;
163 static const Dimension kP2JerkIdx = 1;
164 static const Dimension kP3OmegaIdx = 0;
165 static const Dimension kP3JerkIdx = 1;
166 static const Dimension kP4OmegaIdx = 0;
167 static const Dimension kP4JerkIdx = 1;
168 
169 // Set up lanes for each player.
170 static constexpr float kAngleOffset = M_PI_2 * 0.5;
171 static constexpr float kWedgeSize = M_PI;
172 const std::vector<float> angles = {kAngleOffset,
173  kAngleOffset + 2.0 * M_PI / 4.0,
174  kAngleOffset + 2.0 * 2.0 * M_PI / 4.0,
175  kAngleOffset + 3.0 * 2.0 * M_PI / 4.0};
176 const Polyline2 lane1(RoundaboutLaneCenter(angles[0], angles[0] + kWedgeSize,
177  kP1InitialDistanceToRoundabout));
178 const Polyline2 lane2(RoundaboutLaneCenter(angles[1], angles[1] + kWedgeSize,
179  kP2InitialDistanceToRoundabout));
180 const Polyline2 lane3(RoundaboutLaneCenter(angles[2], angles[2] + kWedgeSize,
181  kP3InitialDistanceToRoundabout));
182 const Polyline2 lane4(RoundaboutLaneCenter(angles[3], angles[3] + kWedgeSize,
183  kP4InitialDistanceToRoundabout));
184 
185 } // anonymous namespace
186 
187 void RoundaboutMergingExample::ConstructDynamics() {
188  dynamics_.reset(new ConcatenatedDynamicalSystem(
189  {std::make_shared<P1>(kInterAxleDistance),
190  std::make_shared<P2>(kInterAxleDistance),
191  std::make_shared<P3>(kInterAxleDistance),
192  std::make_shared<P4>(kInterAxleDistance)}));
193 }
194 
195 void RoundaboutMergingExample::ConstructInitialState() {
196  x0_ = VectorXf::Zero(dynamics_->XDim());
197  x0_(kP1XIdx) = lane1.Segments()[0].FirstPoint().x();
198  x0_(kP1YIdx) = lane1.Segments()[0].FirstPoint().y();
199  x0_(kP1HeadingIdx) = lane1.Segments()[0].Heading();
200  x0_(kP1VIdx) = kP1InitialSpeed;
201  x0_(kP2XIdx) = lane2.Segments()[0].FirstPoint().x();
202  x0_(kP2YIdx) = lane2.Segments()[0].FirstPoint().y();
203  x0_(kP2HeadingIdx) = lane2.Segments()[0].Heading();
204  x0_(kP2VIdx) = kP2InitialSpeed;
205  x0_(kP3XIdx) = lane3.Segments()[0].FirstPoint().x();
206  x0_(kP3YIdx) = lane3.Segments()[0].FirstPoint().y();
207  x0_(kP3HeadingIdx) = lane3.Segments()[0].Heading();
208  x0_(kP3VIdx) = kP3InitialSpeed;
209  x0_(kP4XIdx) = lane4.Segments()[0].FirstPoint().x();
210  x0_(kP4YIdx) = lane4.Segments()[0].FirstPoint().y();
211  x0_(kP4HeadingIdx) = lane4.Segments()[0].Heading();
212  x0_(kP4VIdx) = kP4InitialSpeed;
213 }
214 
215 void RoundaboutMergingExample::ConstructInitialOperatingPoint() {
216  // Initialize operating points to follow these lanes at the nominal speed.
217  // InitializeAlongRoute(lane1, 0.0, kP1InitialSpeed, {kP1XIdx, kP1YIdx},
218  // operating_point_.get());
219  // InitializeAlongRoute(lane2, 0.0, kP2InitialSpeed, {kP2XIdx, kP2YIdx},
220  // operating_point_.get());
221  // InitializeAlongRoute(lane3, 0.0, kP3InitialSpeed, {kP3XIdx, kP3YIdx},
222  // operating_point_.get());
223  // InitializeAlongRoute(lane4, 0.0, kP4InitialSpeed, {kP4XIdx, kP4YIdx},
224  // operating_point_.get());
225  Problem::ConstructInitialOperatingPoint();
226 }
227 
228 void RoundaboutMergingExample::ConstructPlayerCosts() {
229  // Set up costs for all players.
230  player_costs_.emplace_back("P1");
231  player_costs_.emplace_back("P2");
232  player_costs_.emplace_back("P3");
233  player_costs_.emplace_back("P4");
234  auto& p1_cost = player_costs_[0];
235  auto& p2_cost = player_costs_[1];
236  auto& p3_cost = player_costs_[2];
237  auto& p4_cost = player_costs_[3];
238 
239  // Stay in lanes.
240  const std::shared_ptr<QuadraticPolyline2Cost> p1_lane_cost(
241  new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP1XIdx, kP1YIdx},
242  "LaneCenter"));
243  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane_r_cost(
244  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
245  {kP1XIdx, kP1YIdx}, kLaneHalfWidth,
246  kOrientedRight, "LaneRightBoundary"));
247  const std::shared_ptr<SemiquadraticPolyline2Cost> p1_lane_l_cost(
248  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane1,
249  {kP1XIdx, kP1YIdx}, -kLaneHalfWidth,
250  !kOrientedRight, "LaneLeftBoundary"));
251  p1_cost.AddStateCost(p1_lane_cost);
252  p1_cost.AddStateCost(p1_lane_r_cost);
253  p1_cost.AddStateCost(p1_lane_l_cost);
254 
255  const std::shared_ptr<QuadraticPolyline2Cost> p2_lane_cost(
256  new QuadraticPolyline2Cost(kLaneCostWeight, lane2, {kP2XIdx, kP2YIdx},
257  "LaneCenter"));
258  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_r_cost(
259  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
260  {kP2XIdx, kP2YIdx}, kLaneHalfWidth,
261  kOrientedRight, "LaneRightBoundary"));
262  const std::shared_ptr<SemiquadraticPolyline2Cost> p2_lane_l_cost(
263  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane2,
264  {kP2XIdx, kP2YIdx}, -kLaneHalfWidth,
265  !kOrientedRight, "LaneLeftBoundary"));
266  p2_cost.AddStateCost(p2_lane_cost);
267  p2_cost.AddStateCost(p2_lane_r_cost);
268  p2_cost.AddStateCost(p2_lane_l_cost);
269 
270  const std::shared_ptr<QuadraticPolyline2Cost> p3_lane_cost(
271  new QuadraticPolyline2Cost(kLaneCostWeight, lane3, {kP3XIdx, kP3YIdx},
272  "LaneCenter"));
273  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_r_cost(
274  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3,
275  {kP3XIdx, kP3YIdx}, kLaneHalfWidth,
276  kOrientedRight, "LaneRightBoundary"));
277  const std::shared_ptr<SemiquadraticPolyline2Cost> p3_lane_l_cost(
278  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane3,
279  {kP3XIdx, kP3YIdx}, -kLaneHalfWidth,
280  !kOrientedRight, "LaneLeftBoundary"));
281  p3_cost.AddStateCost(p3_lane_cost);
282  p3_cost.AddStateCost(p3_lane_r_cost);
283  p3_cost.AddStateCost(p3_lane_l_cost);
284 
285  const std::shared_ptr<QuadraticPolyline2Cost> p4_lane_cost(
286  new QuadraticPolyline2Cost(kLaneCostWeight, lane4, {kP4XIdx, kP4YIdx},
287  "LaneCenter"));
288  const std::shared_ptr<SemiquadraticPolyline2Cost> p4_lane_r_cost(
289  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane4,
290  {kP4XIdx, kP4YIdx}, kLaneHalfWidth,
291  kOrientedRight, "LaneRightBoundary"));
292  const std::shared_ptr<SemiquadraticPolyline2Cost> p4_lane_l_cost(
293  new SemiquadraticPolyline2Cost(kLaneBoundaryCostWeight, lane4,
294  {kP4XIdx, kP4YIdx}, -kLaneHalfWidth,
295  !kOrientedRight, "LaneLeftBoundary"));
296  p4_cost.AddStateCost(p4_lane_cost);
297  p4_cost.AddStateCost(p4_lane_r_cost);
298  p4_cost.AddStateCost(p4_lane_l_cost);
299 
300  // Max/min/nominal speed costs.
301  const auto p1_min_v_cost = std::make_shared<SemiquadraticCost>(
302  kMaxVCostWeight, kP1VIdx, kMinV, !kOrientedRight, "MinV");
303  const auto p1_max_v_cost = std::make_shared<SemiquadraticCost>(
304  kMaxVCostWeight, kP1VIdx, kP1MaxV, kOrientedRight, "MaxV");
305  const auto p1_nominal_v_cost = std::make_shared<QuadraticCost>(
306  kNominalVCostWeight, kP1VIdx, kP1NominalV, "NominalV");
307  p1_cost.AddStateCost(p1_min_v_cost);
308  p1_cost.AddStateCost(p1_max_v_cost);
309  p1_cost.AddStateCost(p1_nominal_v_cost);
310 
311  const auto p2_min_v_cost = std::make_shared<SemiquadraticCost>(
312  kMaxVCostWeight, kP2VIdx, kMinV, !kOrientedRight, "MinV");
313  const auto p2_max_v_cost = std::make_shared<SemiquadraticCost>(
314  kMaxVCostWeight, kP2VIdx, kP2MaxV, kOrientedRight, "MaxV");
315  const auto p2_nominal_v_cost = std::make_shared<QuadraticCost>(
316  kNominalVCostWeight, kP2VIdx, kP2NominalV, "NominalV");
317  p2_cost.AddStateCost(p2_min_v_cost);
318  p2_cost.AddStateCost(p2_max_v_cost);
319  p2_cost.AddStateCost(p2_nominal_v_cost);
320 
321  const auto p3_min_v_cost = std::make_shared<SemiquadraticCost>(
322  kMaxVCostWeight, kP3VIdx, kMinV, !kOrientedRight, "MinV");
323  const auto p3_max_v_cost = std::make_shared<SemiquadraticCost>(
324  kMaxVCostWeight, kP3VIdx, kP3MaxV, kOrientedRight, "MaxV");
325  const auto p3_nominal_v_cost = std::make_shared<QuadraticCost>(
326  kNominalVCostWeight, kP3VIdx, kP3NominalV, "NominalV");
327  p3_cost.AddStateCost(p3_min_v_cost);
328  p3_cost.AddStateCost(p3_max_v_cost);
329  p3_cost.AddStateCost(p3_nominal_v_cost);
330 
331  const auto p4_min_v_cost = std::make_shared<SemiquadraticCost>(
332  kMaxVCostWeight, kP4VIdx, kMinV, !kOrientedRight, "MinV");
333  const auto p4_max_v_cost = std::make_shared<SemiquadraticCost>(
334  kMaxVCostWeight, kP4VIdx, kP4MaxV, kOrientedRight, "MaxV");
335  const auto p4_nominal_v_cost = std::make_shared<QuadraticCost>(
336  kNominalVCostWeight, kP4VIdx, kP4NominalV, "NominalV");
337  p4_cost.AddStateCost(p4_min_v_cost);
338  p4_cost.AddStateCost(p4_max_v_cost);
339  p4_cost.AddStateCost(p4_nominal_v_cost);
340 
341  // Penalize acceleration.
342  const auto p1_a_cost = std::make_shared<QuadraticCost>(kACostWeight, kP1AIdx,
343  0.0, "Acceleration");
344  p1_cost.AddStateCost(p1_a_cost);
345  const auto p2_a_cost = std::make_shared<QuadraticCost>(kACostWeight, kP1AIdx,
346  0.0, "Acceleration");
347  p2_cost.AddStateCost(p2_a_cost);
348  const auto p3_a_cost = std::make_shared<QuadraticCost>(kACostWeight, kP1AIdx,
349  0.0, "Acceleration");
350  p3_cost.AddStateCost(p3_a_cost);
351  const auto p4_a_cost = std::make_shared<QuadraticCost>(kACostWeight, kP1AIdx,
352  0.0, "Acceleration");
353  p4_cost.AddStateCost(p4_a_cost);
354 
355  // Penalize control effort.
356  const auto p1_omega_cost = std::make_shared<QuadraticCost>(
357  kOmegaCostWeight, kP1OmegaIdx, 0.0, "Steering");
358  const auto p1_j_cost =
359  std::make_shared<QuadraticCost>(kJerkCostWeight, kP1JerkIdx, 0.0, "Jerk");
360  p1_cost.AddControlCost(0, p1_omega_cost);
361  p1_cost.AddControlCost(0, p1_j_cost);
362 
363  const auto p2_omega_cost = std::make_shared<QuadraticCost>(
364  kOmegaCostWeight, kP2OmegaIdx, 0.0, "Steering");
365  const auto p2_j_cost =
366  std::make_shared<QuadraticCost>(kJerkCostWeight, kP2JerkIdx, 0.0, "Jerk");
367  p2_cost.AddControlCost(1, p2_omega_cost);
368  p2_cost.AddControlCost(1, p2_j_cost);
369 
370  const auto p3_omega_cost = std::make_shared<QuadraticCost>(
371  kOmegaCostWeight, kP3OmegaIdx, 0.0, "Steering");
372  const auto p3_j_cost =
373  std::make_shared<QuadraticCost>(kJerkCostWeight, kP3JerkIdx, 0.0, "Jerk");
374  p3_cost.AddControlCost(2, p3_omega_cost);
375  p3_cost.AddControlCost(2, p3_j_cost);
376 
377  const auto p4_omega_cost = std::make_shared<QuadraticCost>(
378  kOmegaCostWeight, kP4OmegaIdx, 0.0, "Steering");
379  const auto p4_j_cost =
380  std::make_shared<QuadraticCost>(kJerkCostWeight, kP4JerkIdx, 0.0, "Jerk");
381  p4_cost.AddControlCost(3, p4_omega_cost);
382  p4_cost.AddControlCost(3, p4_j_cost);
383 
384  // Pairwise proximity costs.
385  const std::shared_ptr<ProxCost> p1p2_proximity_cost(
386  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
387  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
388  const std::shared_ptr<ProxCost> p1p3_proximity_cost(
389  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
390  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
391  const std::shared_ptr<ProxCost> p1p4_proximity_cost(
392  new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
393  {kP4XIdx, kP4YIdx}, kMinProximity, "ProximityP4"));
394  p1_cost.AddStateCost(p1p2_proximity_cost);
395  // p1_cost.AddStateCost(p1p3_proximity_cost);
396  p1_cost.AddStateCost(p1p4_proximity_cost);
397 
398  const std::shared_ptr<ProxCost> p2p1_proximity_cost(
399  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
400  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
401  const std::shared_ptr<ProxCost> p2p3_proximity_cost(
402  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
403  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
404  const std::shared_ptr<ProxCost> p2p4_proximity_cost(
405  new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
406  {kP4XIdx, kP4YIdx}, kMinProximity, "ProximityP4"));
407  p2_cost.AddStateCost(p2p1_proximity_cost);
408  p2_cost.AddStateCost(p2p3_proximity_cost);
409  // p2_cost.AddStateCost(p2p4_proximity_cost);
410 
411  const std::shared_ptr<ProxCost> p3p1_proximity_cost(
412  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
413  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
414  const std::shared_ptr<ProxCost> p3p2_proximity_cost(
415  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
416  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
417  const std::shared_ptr<ProxCost> p3p4_proximity_cost(
418  new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
419  {kP4XIdx, kP4YIdx}, kMinProximity, "ProximityP4"));
420  // p3_cost.AddStateCost(p3p1_proximity_cost);
421  p3_cost.AddStateCost(p3p2_proximity_cost);
422  p3_cost.AddStateCost(p3p4_proximity_cost);
423 
424  const std::shared_ptr<ProxCost> p4p1_proximity_cost(
425  new ProxCost(kP4ProximityCostWeight, {kP4XIdx, kP4YIdx},
426  {kP1XIdx, kP1YIdx}, kMinProximity, "ProximityP1"));
427  const std::shared_ptr<ProxCost> p4p2_proximity_cost(
428  new ProxCost(kP4ProximityCostWeight, {kP4XIdx, kP4YIdx},
429  {kP2XIdx, kP2YIdx}, kMinProximity, "ProximityP2"));
430  const std::shared_ptr<ProxCost> p4p3_proximity_cost(
431  new ProxCost(kP4ProximityCostWeight, {kP4XIdx, kP4YIdx},
432  {kP3XIdx, kP3YIdx}, kMinProximity, "ProximityP3"));
433  p4_cost.AddStateCost(p4p1_proximity_cost);
434  // p4_cost.AddStateCost(p4p2_proximity_cost);
435  p4_cost.AddStateCost(p4p3_proximity_cost);
436 }
437 
438 inline std::vector<float> RoundaboutMergingExample::Xs(
439  const VectorXf& x) const {
440  return {x(kP1XIdx), x(kP2XIdx), x(kP3XIdx), x(kP4XIdx)};
441 }
442 
443 inline std::vector<float> RoundaboutMergingExample::Ys(
444  const VectorXf& x) const {
445  return {x(kP1YIdx), x(kP2YIdx), x(kP3YIdx), x(kP4YIdx)};
446 }
447 
448 inline std::vector<float> RoundaboutMergingExample::Thetas(
449  const VectorXf& x) const {
450  return {x(kP1HeadingIdx), x(kP2HeadingIdx), x(kP3HeadingIdx),
451  x(kP4HeadingIdx)};
452 }
453 
454 } // namespace ilqgames
Generated by   doxygen 1.8.11