44 #include <ilqgames/constraint/single_dimension_constraint.h> 45 #include <ilqgames/cost/polyline2_signed_distance_cost.h> 46 #include <ilqgames/cost/quadratic_cost.h> 47 #include <ilqgames/dynamics/air_3d.h> 48 #include <ilqgames/examples/air_3d_example.h> 49 #include <ilqgames/geometry/draw_shapes.h> 50 #include <ilqgames/geometry/polyline2.h> 51 #include <ilqgames/solver/ilq_solver.h> 52 #include <ilqgames/solver/problem.h> 53 #include <ilqgames/solver/solver_params.h> 54 #include <ilqgames/utils/types.h> 56 #include <gflags/gflags.h> 62 DEFINE_double(rx0, 4.0,
"Initial x-position (m).");
63 DEFINE_double(ry0, 3.0,
"Initial y-position (m).");
64 DEFINE_double(rtheta0, M_PI / 4.0,
"Initial heading (rad).");
65 DEFINE_double(ve, 1.0,
"Evader speed (m/s).");
66 DEFINE_double(vp, 1.0,
"Pursuer speed (m/s).");
73 static constexpr
float kOmegaCostWeight = 0.1;
74 static constexpr
float kOmegaMax = 1.0;
81 void Air3DExample::ConstructDynamics() {
82 dynamics_.reset(
new Dyn(FLAGS_ve, FLAGS_vp));
85 void Air3DExample::ConstructInitialState() {
86 x0_ = VectorXf::Zero(dynamics_->XDim());
87 x0_(Dyn::kRxIdx) = FLAGS_rx0;
88 x0_(Dyn::kRyIdx) = FLAGS_ry0;
89 x0_(Dyn::kRThetaIdx) = FLAGS_rtheta0;
92 void Air3DExample::ConstructPlayerCosts() {
94 player_costs_.emplace_back(
"P1");
95 player_costs_.emplace_back(
"P2");
96 auto& p1_cost = player_costs_[0];
97 auto& p2_cost = player_costs_[1];
99 const auto control_cost =
100 std::make_shared<QuadraticCost>(kOmegaCostWeight, -1, 0.0,
"ControlCost");
101 p1_cost.AddControlCost(0, control_cost);
102 p2_cost.AddControlCost(1, control_cost);
105 const auto p1_omega_max_constraint =
106 std::make_shared<SingleDimensionConstraint>(
107 Dyn::kOmega1Idx, kOmegaMax,
true,
"Omega Constraint (Max)");
108 const auto p1_omega_min_constraint =
109 std::make_shared<SingleDimensionConstraint>(
110 Dyn::kOmega1Idx, -kOmegaMax,
false,
"Omega Constraint (Min)");
111 p1_cost.AddControlConstraint(0, p1_omega_max_constraint);
112 p1_cost.AddControlConstraint(0, p1_omega_min_constraint);
114 const auto p2_omega_max_constraint =
115 std::make_shared<SingleDimensionConstraint>(
116 Dyn::kOmega1Idx, kOmegaMax,
true,
"Omega Constraint (Max)");
117 const auto p2_omega_min_constraint =
118 std::make_shared<SingleDimensionConstraint>(
119 Dyn::kOmega2Idx, -kOmegaMax,
false,
"Omega Constraint (Min)");
120 p2_cost.AddControlConstraint(1, p2_omega_max_constraint);
121 p2_cost.AddControlConstraint(1, p2_omega_min_constraint);
124 const float kTargetRadius = 5.0;
125 const Polyline2 circle = DrawCircle(Point2::Zero(), kTargetRadius, 10);
127 constexpr
bool kReach =
true;
128 const std::shared_ptr<Polyline2SignedDistanceCost> p1_target_cost(
129 new Polyline2SignedDistanceCost(circle, {Dyn::kRxIdx, Dyn::kRyIdx},
131 const std::shared_ptr<Polyline2SignedDistanceCost> p2_target_cost(
132 new Polyline2SignedDistanceCost(circle, {Dyn::kRxIdx, Dyn::kRyIdx},
135 p1_cost.AddStateCost(p1_target_cost);
136 p2_cost.AddStateCost(p2_target_cost);
140 p1_cost.SetMaxOverTime();
141 p2_cost.SetMinOverTime();
144 inline std::vector<float> Air3DExample::Xs(
const VectorXf& x)
const {
145 return {0.0, x(Dyn::kRxIdx)};
148 inline std::vector<float> Air3DExample::Ys(
const VectorXf& x)
const {
149 return {0.0, x(Dyn::kRyIdx)};
152 inline std::vector<float> Air3DExample::Thetas(
const VectorXf& x)
const {
153 return {0.0, x(Dyn::kRThetaIdx)};