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/route_progress_cost.h> 53 #include <ilqgames/cost/semiquadratic_cost.h> 54 #include <ilqgames/cost/semiquadratic_norm_cost.h> 55 #include <ilqgames/cost/semiquadratic_polyline2_cost.h> 56 #include <ilqgames/dynamics/concatenated_flat_system.h> 57 #include <ilqgames/dynamics/single_player_flat_car_6d.h> 58 #include <ilqgames/dynamics/single_player_flat_unicycle_4d.h> 59 #include <ilqgames/examples/three_player_flat_overtaking_example.h> 60 #include <ilqgames/geometry/polyline2.h> 61 #include <ilqgames/solver/problem.h> 62 #include <ilqgames/utils/solver_log.h> 63 #include <ilqgames/utils/strategy.h> 64 #include <ilqgames/utils/types.h> 75 static constexpr
float kInterAxleLength = 4.0;
78 static constexpr
float kCarAuxCostWeight = 5000.0;
80 static constexpr
float kP1NominalVCostWeight = 10.0;
81 static constexpr
float kP2NominalVCostWeight = 1.0;
82 static constexpr
float kP3NominalVCostWeight = 1.0;
84 static constexpr
float kLaneCostWeight = 25.0;
85 static constexpr
float kLaneBoundaryCostWeight = 100.0;
87 static constexpr
float kMinProximity = 5.0;
88 static constexpr
float kP1ProximityCostWeight = 100.0;
89 static constexpr
float kP2ProximityCostWeight = 100.0;
90 static constexpr
float kP3ProximityCostWeight = 100.0;
91 using ProxCost = ProximityCost;
94 static constexpr
float kNominalHeadingCostWeight = 150.0;
96 static constexpr
bool kOrientedRight =
true;
99 static constexpr
float kLaneHalfWidth = 2.5;
102 static constexpr
float kP1NominalV = 15.0;
103 static constexpr
float kP2NominalV = 10.0;
104 static constexpr
float kP3NominalV = 10.0;
107 static constexpr
float kP1NominalHeading = M_PI_2;
110 static constexpr
float kP1InitialX = 2.5;
111 static constexpr
float kP1InitialY = -10.0;
113 static constexpr
float kP2InitialX = -1.0;
114 static constexpr
float kP2InitialY = -10.0;
116 static constexpr
float kP3InitialX = 2.5;
117 static constexpr
float kP3InitialY = 10.0;
119 static constexpr
float kP1InitialHeading = M_PI_2;
120 static constexpr
float kP2InitialHeading = M_PI_2;
121 static constexpr
float kP3InitialHeading = M_PI_2;
123 static constexpr
float kP1InitialSpeed = 5.0;
124 static constexpr
float kP2InitialSpeed = 5.0;
125 static constexpr
float kP3InitialSpeed = 5.25;
128 using P1 = SinglePlayerFlatCar6D;
129 using P2 = SinglePlayerFlatCar6D;
130 using P3 = SinglePlayerFlatCar6D;
132 static const Dimension kP1XIdx = P1::kPxIdx;
133 static const Dimension kP1YIdx = P1::kPyIdx;
134 static const Dimension kP1HeadingIdx = P1::kThetaIdx;
135 static const Dimension kP1PhiIdx = P1::kPhiIdx;
136 static const Dimension kP1VIdx = P1::kVIdx;
137 static const Dimension kP1AIdx = P1::kAIdx;
138 static const Dimension kP1VxIdx = P1::kVxIdx;
139 static const Dimension kP1VyIdx = P1::kVyIdx;
140 static const Dimension kP1AxIdx = P1::kAxIdx;
141 static const Dimension kP1AyIdx = P1::kAyIdx;
143 static const Dimension kP2XIdx = P1::kNumXDims + P2::kPxIdx;
144 static const Dimension kP2YIdx = P1::kNumXDims + P2::kPyIdx;
145 static const Dimension kP2HeadingIdx = P1::kNumXDims + P2::kThetaIdx;
146 static const Dimension kP2PhiIdx = P1::kNumXDims + P2::kPhiIdx;
147 static const Dimension kP2VIdx = P1::kNumXDims + P2::kVIdx;
148 static const Dimension kP2AIdx = P1::kNumXDims + P2::kAIdx;
149 static const Dimension kP2VxIdx = P1::kNumXDims + P2::kVxIdx;
150 static const Dimension kP2VyIdx = P1::kNumXDims + P2::kVyIdx;
151 static const Dimension kP2AxIdx = P1::kNumXDims + P2::kAxIdx;
152 static const Dimension kP2AyIdx = P1::kNumXDims + P2::kAyIdx;
154 static const Dimension kP3XIdx = P1::kNumXDims + P2::kNumXDims + P3::kPxIdx;
155 static const Dimension kP3YIdx = P1::kNumXDims + P2::kNumXDims + P3::kPyIdx;
156 static const Dimension kP3HeadingIdx =
157 P1::kNumXDims + P2::kNumXDims + P3::kThetaIdx;
158 static const Dimension kP3VIdx = P1::kNumXDims + P2::kNumXDims + P3::kVIdx;
159 static const Dimension kP3VxIdx = P1::kNumXDims + P2::kNumXDims + P3::kVxIdx;
160 static const Dimension kP3VyIdx = P1::kNumXDims + P2::kNumXDims + P3::kVyIdx;
163 static const Dimension kP1OmegaIdx = 0;
164 static const Dimension kP1JerkIdx = 1;
165 static const Dimension kP2OmegaIdx = 0;
166 static const Dimension kP2JerkIdx = 1;
167 static const Dimension kP3OmegaIdx = 0;
168 static const Dimension kP3AIdx = 1;
172 void ThreePlayerFlatOvertakingExample::ConstructDynamics() {
174 new ConcatenatedFlatSystem({std::make_shared<P1>(kInterAxleLength),
175 std::make_shared<P2>(kInterAxleLength),
176 std::make_shared<P3>(kInterAxleLength)}));
179 void ThreePlayerFlatOvertakingExample::ConstructInitialState() {
180 VectorXf x0 = VectorXf::Zero(dynamics_->XDim());
181 x0(kP1XIdx) = kP1InitialX;
182 x0(kP1YIdx) = kP1InitialY;
183 x0(kP1HeadingIdx) = kP1InitialHeading;
184 x0(kP1VIdx) = kP1InitialSpeed;
185 x0(kP2XIdx) = kP2InitialX;
186 x0(kP2YIdx) = kP2InitialY;
187 x0(kP2HeadingIdx) = kP2InitialHeading;
188 x0(kP2VIdx) = kP2InitialSpeed;
189 x0(kP3XIdx) = kP3InitialX;
190 x0(kP3YIdx) = kP3InitialY;
191 x0(kP3HeadingIdx) = kP3InitialHeading;
192 x0(kP3VIdx) = kP3InitialSpeed;
194 x0_ = dynamics_->ToLinearSystemState(x0);
197 void ThreePlayerFlatOvertakingExample::ConstructPlayerCosts() {
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];
207 const Polyline2 lane1(
208 {Point2(kP2InitialX, kP2InitialY), Point2(kP2InitialX, 1000.0)});
209 const Polyline2 lane2(
210 {Point2(kP3InitialX, kP3InitialY), Point2(kP3InitialX, 1000.0)});
212 const std::shared_ptr<QuadraticPolyline2Cost> p1_lane_cost(
213 new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP1XIdx, kP1YIdx},
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);
227 const std::shared_ptr<QuadraticPolyline2Cost> p2_lane_cost(
228 new QuadraticPolyline2Cost(kLaneCostWeight, lane1, {kP2XIdx, kP2YIdx},
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);
242 const std::shared_ptr<QuadraticPolyline2Cost> p3_lane_cost(
243 new QuadraticPolyline2Cost(kLaneCostWeight, lane2, {kP3XIdx, kP3YIdx},
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);
258 const std::shared_ptr<RouteProgressCost> p1_progress_cost(
259 new RouteProgressCost(kP1NominalVCostWeight, kP1NominalV, lane1,
260 {kP1XIdx, kP1YIdx},
"RouteProgress",
261 kP1InitialY - kP2InitialY));
262 p1_cost.AddStateCost(p1_progress_cost);
264 const std::shared_ptr<RouteProgressCost> p2_progress_cost(
265 new RouteProgressCost(kP2NominalVCostWeight, kP2NominalV, lane1,
266 {kP2XIdx, kP2YIdx},
"RouteProgress"));
267 p2_cost.AddStateCost(p2_progress_cost);
269 const std::shared_ptr<RouteProgressCost> p3_progress_cost(
270 new RouteProgressCost(kP3NominalVCostWeight, kP3NominalV, lane2,
271 {kP3XIdx, kP3YIdx},
"RouteProgress"));
272 p3_cost.AddStateCost(p3_progress_cost);
275 constexpr Dimension kApplyInAllDimensions = -1;
278 const auto car_aux_cost = std::make_shared<QuadraticCost>(
279 kCarAuxCostWeight, kApplyInAllDimensions, 0.0,
"Auxiliary Input");
280 p1_cost.AddControlCost(0, car_aux_cost);
281 p2_cost.AddControlCost(1, car_aux_cost);
282 p3_cost.AddControlCost(2, car_aux_cost);
285 const std::shared_ptr<ProxCost> p1p2_proximity_cost(
286 new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
287 {kP2XIdx, kP2YIdx}, kMinProximity,
"ProximityP2"));
288 const std::shared_ptr<ProxCost> p1p3_proximity_cost(
289 new ProxCost(kP1ProximityCostWeight, {kP1XIdx, kP1YIdx},
290 {kP3XIdx, kP3YIdx}, kMinProximity,
"ProximityP3"));
291 p1_cost.AddStateCost(p1p2_proximity_cost);
292 p1_cost.AddStateCost(p1p3_proximity_cost);
294 const std::shared_ptr<ProxCost> p2p1_proximity_cost(
295 new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
296 {kP1XIdx, kP1YIdx}, kMinProximity,
"ProximityP1"));
297 const std::shared_ptr<ProxCost> p2p3_proximity_cost(
298 new ProxCost(kP2ProximityCostWeight, {kP2XIdx, kP2YIdx},
299 {kP3XIdx, kP3YIdx}, kMinProximity,
"ProximityP3"));
300 p2_cost.AddStateCost(p2p1_proximity_cost);
301 p2_cost.AddStateCost(p2p3_proximity_cost);
303 const std::shared_ptr<ProxCost> p3p1_proximity_cost(
304 new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
305 {kP1XIdx, kP1YIdx}, kMinProximity,
"ProximityP1"));
306 const std::shared_ptr<ProxCost> p3p2_proximity_cost(
307 new ProxCost(kP3ProximityCostWeight, {kP3XIdx, kP3YIdx},
308 {kP2XIdx, kP2YIdx}, kMinProximity,
"ProximityP2"));
309 p3_cost.AddStateCost(p3p1_proximity_cost);
310 p3_cost.AddStateCost(p3p2_proximity_cost);
313 inline std::vector<float> ThreePlayerFlatOvertakingExample::Xs(
314 const VectorXf& xi)
const {
315 return {xi(kP1XIdx), xi(kP2XIdx), xi(kP3XIdx)};
318 inline std::vector<float> ThreePlayerFlatOvertakingExample::Ys(
319 const VectorXf& xi)
const {
320 return {xi(kP1YIdx), xi(kP2YIdx), xi(kP3YIdx)};
323 inline std::vector<float> ThreePlayerFlatOvertakingExample::Thetas(
324 const VectorXf& xi)
const {
325 const VectorXf x = dynamics_->FromLinearSystemState(xi);
326 return {x(kP1HeadingIdx), x(kP2HeadingIdx), x(kP3HeadingIdx)};