Line data Source code
1 : #include "Dynamic/Policy/QLearner.hpp"
2 :
3 : #include <spdlog/spdlog.h>
4 :
5 : #include <functional>
6 : #include <limits>
7 : #include <optional>
8 : #include <random>
9 : #include <stdexcept>
10 :
11 : #include "Dynamic/Env/Edge.hpp"
12 : #include "Log/ProgressLogger.hpp"
13 : #include "data/SUMO/Network.hpp"
14 : #include "utils/reference_wrapper.hpp"
15 :
16 : using namespace std;
17 : using namespace Dynamic;
18 :
19 : const double QLearner::Logger::ALPHA_D = 1e-6;
20 :
21 0 : QLearner::Action::Action(Env::Connection& connection_, Env::Lane& lane_):
22 : connection(connection_),
23 0 : lane(lane_) {}
24 :
25 0 : QLearner::Action::Action(const Action& action):
26 0 : connection(action.connection),
27 0 : lane(action.lane) {}
28 :
29 0 : QLearner::Action& QLearner::Action::operator=(const Action& other) {
30 0 : new(this) Action(other.connection, other.lane);
31 0 : return *this;
32 : }
33 :
34 0 : bool QLearner::Action::operator==(const Action& other) const {
35 0 : return connection == other.connection && lane == other.lane;
36 : }
37 :
38 0 : bool QLearner::Action::operator!=(const Action& other) const {
39 0 : return !(*this == other);
40 : }
41 :
42 0 : bool QLearner::Action::operator<(const Action& other) const {
43 0 : if(connection != other.connection)
44 0 : return connection < other.connection;
45 0 : return lane < other.lane;
46 : }
47 :
48 0 : QLearner::State::State(Env::Lane& lane):
49 0 : reference_wrapper<Env::Lane>(lane) {}
50 :
51 0 : QLearner::State QLearner::State::apply(Action action) const {
52 0 : assert(get() == action.connection.fromLane);
53 :
54 0 : return State(action.lane);
55 : }
56 :
57 0 : vector<QLearner::Action> QLearner::State::possibleActions() {
58 0 : vector<Action> actions;
59 :
60 0 : for(Env::Connection& connection: get().getOutgoingConnections()) {
61 0 : for(Env::Lane& lane: connection.toLane.edge.lanes) {
62 0 : actions.push_back(Action(connection, lane));
63 : }
64 : }
65 :
66 0 : return actions;
67 : }
68 :
69 0 : vector<QLearner::Action> QLearner::State::possibleActions() const {
70 0 : vector<Action> actions;
71 :
72 0 : for(Env::Connection& connection: get().getOutgoingConnections()) {
73 0 : for(Env::Lane& lane: connection.toLane.edge.lanes) {
74 0 : actions.push_back(Action(connection, lane));
75 : }
76 : }
77 :
78 0 : return actions;
79 : }
80 :
81 0 : QLearner::QLearner(
82 : Env::Env& env_,
83 : const SUMO::Network& network_,
84 : const Dynamic::SUMOAdapter& adapter_,
85 : const Env::TAZ& destinationTAZ_,
86 : optional<reference_wrapper<QLearner::Logger>> policyLogger_,
87 : Reward alpha_,
88 : Reward gamma_,
89 : Reward xi_,
90 : Reward eta_,
91 : float epsilon_
92 0 : ):
93 : env(env_),
94 : network(network_),
95 : adapter(adapter_),
96 : destinationTAZ(destinationTAZ_),
97 : alpha(alpha_),
98 : gamma(gamma_),
99 : xi(xi_),
100 : eta(eta_),
101 : epsilon(epsilon_),
102 : QMatrix(),
103 0 : policyLogger(policyLogger_) {
104 0 : Alg::Graph G = env.toGraph();
105 0 : Alg::Graph GT = G.transpose();
106 :
107 0 : list<Env::Node> startNodes;
108 0 : for(const Env::Edge& edge: destinationTAZ.sinks) {
109 0 : startNodes.push_back(edge.v);
110 : }
111 :
112 0 : sp.solveList(GT, startNodes);
113 0 : }
114 :
115 0 : QLearner::Reward& QLearner::Qref(const State& s, const Action& a) {
116 0 : auto& q = QMatrix[s];
117 :
118 0 : auto it = q.find(a);
119 0 : if(it != q.end()) return it->second;
120 :
121 0 : return q[a] = estimateInitialValue(s, a);
122 :
123 : // State sNew = s.apply(a);
124 :
125 : // auto it = QMatrix.find(sNew);
126 : // if(it != QMatrix.end()) return it->second;
127 :
128 : // return QMatrix[sNew] = estimateInitialValue(s, a);
129 : }
130 :
131 0 : QLearner::Reward QLearner::Q(const State& s, const Action& a) const {
132 0 : auto& q = QMatrix[s];
133 :
134 0 : auto it = q.find(a);
135 0 : if(it != q.end()) return it->second;
136 :
137 0 : return q[a] = estimateInitialValue(s, a);
138 :
139 : // State sNew = s.apply(a);
140 :
141 : // auto it = QMatrix.find(sNew);
142 : // if(it != QMatrix.end()) return it->second;
143 :
144 : // return QMatrix[sNew] = estimateInitialValue(s, a);
145 : }
146 :
147 0 : QLearner::Reward QLearner::estimateInitialValue(const State& s, const Action& a) const {
148 0 : State sStar = s.apply(a);
149 :
150 0 : bool hasValidNextAction = false;
151 0 : vector<Action> nextActions = sStar.possibleActions();
152 0 : for(Action& nextAction: nextActions) {
153 0 : State sNewNew = sStar.apply(nextAction);
154 :
155 0 : if(sp.hasVisited(sNewNew.get().edge.u)) {
156 0 : hasValidNextAction = true;
157 0 : break;
158 : }
159 : }
160 :
161 0 : if(
162 0 : destinationTAZ.sinks.find(sStar.get().edge) == destinationTAZ.sinks.end() && // Not a sink
163 : !hasValidNextAction // No valid next action
164 : ) {
165 : return -numeric_limits<Reward>::infinity();
166 : }
167 :
168 : // clang-format off
169 0 : const double t = (
170 0 : sp.hasVisited(sStar.get().edge.u) ?
171 0 : sp.getPathWeight(sStar.get().edge.u) :
172 : numeric_limits<double>::infinity()
173 0 : );
174 : // clang-format on
175 :
176 0 : return -t;
177 : }
178 :
179 0 : QLearner::Reward QLearner::estimateOptimalValue(const State& s) const {
180 0 : Reward q = -numeric_limits<Reward>::infinity();
181 :
182 0 : for(const Action& a: s.possibleActions()) {
183 0 : q = max(q, Q(s, a));
184 : }
185 :
186 0 : return q;
187 : }
188 :
189 0 : QLearner::Reward QLearner::estimateOptimalFutureValue(const State& s, const Action& a) const {
190 0 : State sNew = s.apply(a);
191 :
192 0 : return estimateOptimalValue(sNew);
193 : }
194 :
195 0 : QLearner::Action QLearner::heuristicPolicy(const State& s) const {
196 0 : const Env::Lane& currentLane = s.get();
197 0 : const Env::Edge& currentEdge = currentLane.edge;
198 :
199 0 : const SUMO::Network::Edge::Lane& currentSumoLane =
200 0 : network.getEdge(adapter.toSumoEdge(currentEdge.id))
201 0 : .lanes.at(currentLane.index);
202 :
203 0 : SUMO::Coord now = currentSumoLane.getShape().front();
204 :
205 0 : Reward bestH = -numeric_limits<Reward>::infinity();
206 0 : Action bestA(Env::Connection::INVALID, Env::Lane::INVALID);
207 :
208 0 : vector<SUMO::Coord> sinksPos;
209 0 : for(const Env::Edge& e: destinationTAZ.sinks) {
210 0 : SUMO::Coord sinkPos = network.getEdge(adapter.toSumoEdge(e.id)).getShape().front();
211 0 : sinksPos.push_back(sinkPos);
212 : }
213 :
214 0 : for(Action& a: s.possibleActions()) {
215 0 : const SUMO::Network::Edge::Lane& nextSumoLane =
216 0 : network.getEdge(adapter.toSumoEdge(a.lane.edge.id))
217 0 : .lanes.at(a.lane.index);
218 :
219 0 : SUMO::Coord next = nextSumoLane.getShape().back();
220 :
221 : // Get closest sink
222 0 : SUMO::Coord destination;
223 0 : Length dBest = numeric_limits<Length>::infinity();
224 0 : for(const SUMO::Coord& sinkPos: sinksPos) {
225 0 : Length d = SUMO::Coord::Distance(next, sinkPos);
226 0 : if(d < dBest) {
227 0 : dBest = d;
228 0 : destination = sinkPos;
229 : }
230 : }
231 0 : assert(dBest < numeric_limits<Length>::infinity());
232 :
233 : // Determine angle
234 0 : Vector2 v1 = next - now;
235 0 : Vector2 v2 = destination - now;
236 :
237 0 : double theta = Vector2::Angle(v1, v2);
238 :
239 0 : Reward h = -theta;
240 :
241 0 : if(h > bestH) {
242 0 : bestH = h;
243 0 : bestA = a;
244 : }
245 : }
246 :
247 0 : assert(bestA.connection != Env::Connection::INVALID);
248 :
249 0 : return bestA;
250 : }
251 :
252 0 : QLearner::Reward QLearner::heuristic(const State& st, const Action& at) const {
253 0 : #pragma GCC diagnostic push
254 0 : #pragma GCC diagnostic ignored "-Wfloat-equal"
255 0 : if(xi == 0.0) return 0.0;
256 0 : #pragma GCC diagnostic pop
257 :
258 0 : Action bestA = heuristicPolicy(st);
259 :
260 0 : if(at != bestA) return 0.0;
261 :
262 0 : Reward q = Q(st, at);
263 :
264 0 : if(q <= -numeric_limits<Reward>::infinity())
265 : return 0;
266 :
267 0 : Reward H = estimateOptimalValue(st) - q + eta;
268 :
269 : // assert(!isnan(H));
270 : // assert(H < numeric_limits<Reward>::infinity());
271 : // assert(H > -numeric_limits<Reward>::infinity());
272 :
273 0 : Reward h = xi * H;
274 :
275 0 : return h;
276 : }
277 :
278 0 : QLearner::Reward QLearner::tabu(const State& s, const Action& a, const Env::Vehicle& vehicle) const {
279 0 : State sNext = s.apply(a);
280 0 : size_t n = vehicle.path.count(sNext.get());
281 :
282 : // Reward r = -20.0 * (exp(n) - 1.0);
283 0 : Reward r = -10.0 * (Reward)(n * n) * exp((Reward)n / 20.0);
284 : // Reward r = 0.0;
285 :
286 0 : if(n >= 20) {
287 0 : stringstream ss;
288 0 : bool first = true;
289 0 : for(const auto& [t, lane]: vehicle.path) {
290 0 : ss << (first ? "" : " ") << lane.get().idAsString();
291 0 : first = false;
292 : }
293 0 : spdlog::warn(
294 : "Vehicle {} has been on lane {} for {} times, path is {}",
295 0 : vehicle.id,
296 0 : sNext.get().idAsString(),
297 : n,
298 0 : ss.str()
299 : );
300 : }
301 :
302 0 : return r;
303 : }
304 :
305 0 : void QLearner::updateMatrix(const State& s, const Action& a, Reward r) {
306 0 : Reward& q = Qref(s, a);
307 :
308 0 : Reward qPrev = q;
309 0 : Reward f = estimateOptimalFutureValue(s, a);
310 :
311 0 : Reward qNew = (r + gamma * f);
312 :
313 0 : q += alpha * (qNew - q);
314 :
315 0 : if(policyLogger.has_value()) {
316 0 : auto& logger = policyLogger.value().get();
317 0 : auto &D = logger.D, &DA = logger.DA;
318 :
319 0 : const double Delta = q - qPrev;
320 0 : D += Logger::ALPHA_D * (Delta - D);
321 0 : DA += Logger::ALPHA_D * (abs(Delta) - DA);
322 : }
323 0 : }
324 :
325 0 : void QLearner::setAlpha(Reward alpha_) {
326 0 : alpha = alpha_;
327 0 : }
328 :
329 0 : void QLearner::setEpsilon(float epsilon_) {
330 0 : epsilon = epsilon_;
331 0 : }
332 :
333 0 : void QLearner::dump() const {
334 0 : stringstream ss;
335 0 : ss << "Dumping QLearner, destination TAZ is " << destinationTAZ.id << endl;
336 :
337 0 : for(const auto& [state, m]: QMatrix) {
338 0 : for(const auto& [action, q]: m) {
339 0 : ss
340 0 : << " destTAZ " << destinationTAZ.id
341 : << ", a(conn: "
342 0 : << action.connection.fromLane.idAsString() << " → "
343 0 : << action.connection.toLane.idAsString()
344 0 : << ", lane: " << action.lane.idAsString()
345 0 : << "), q=" << q
346 0 : << "\n";
347 : }
348 : }
349 :
350 0 : cerr << ss.rdbuf();
351 0 : }
352 :
353 0 : QLearner::Policy::Policy(
354 : QLearner& qLearner_,
355 : Env::Vehicle::ID vehicleID_,
356 : mt19937& gen_
357 0 : ):
358 : qLearner(qLearner_),
359 : vehicleID(vehicleID_),
360 0 : gen(gen_) {}
361 :
362 : const double LAMBDA_INITIAL_LANE = 0.05;
363 :
364 0 : Env::Lane& QLearner::Policy::pickInitialLane(Vehicle& vehicle, Env::Env&) {
365 0 : vector<QLearner::Action> actions;
366 0 : vector<Reward> qVector;
367 0 : for(Env::Edge& edge: vehicle.fromTAZ.sources) {
368 0 : for(Env::Lane& lane: edge.lanes) {
369 0 : State s = lane;
370 :
371 0 : auto sActions = s.possibleActions();
372 :
373 0 : for(QLearner::Action& a: sActions) {
374 0 : Reward q = qLearner.Q(s, a);
375 :
376 0 : if(q <= -numeric_limits<Reward>::infinity()) continue;
377 :
378 0 : actions.push_back(a);
379 0 : qVector.push_back(q);
380 : }
381 : }
382 : }
383 :
384 0 : if(actions.empty()) {
385 : // clang-format off
386 0 : throw logic_error(
387 0 : "Could not find suitable lane to start vehicle on; "s +
388 0 : "origin TAZ " + to_string(vehicle.fromTAZ.id) +
389 0 : ", goal TAZ " + to_string(vehicle.toTAZ.id)
390 0 : );
391 : // clang-format on
392 : }
393 :
394 0 : vector<double> chances;
395 :
396 0 : const Reward qMax = *max_element(qVector.begin(), qVector.end());
397 :
398 0 : for(const QLearner::Action& a: actions) {
399 0 : State s = a.connection.fromLane;
400 :
401 0 : Reward q = qLearner.Q(s, a);
402 0 : Reward Dq = qMax - q;
403 :
404 0 : double p = exp(-LAMBDA_INITIAL_LANE * Dq);
405 :
406 0 : chances.push_back(p);
407 : }
408 :
409 0 : discrete_distribution<size_t> dist(chances.begin(), chances.end());
410 :
411 0 : size_t n = dist(gen);
412 :
413 0 : Env::Lane& startLane = actions.at(n).connection.fromLane;
414 :
415 0 : return startLane;
416 : }
417 :
418 : /**
419 : * @brief Coefficient of the exponential distribution used to pick among the
420 : * best actions.
421 : *
422 : * The smaller LAMBDA is, the more likely it is to select an action other than
423 : * the best.
424 : *
425 : * LAMBDA = ln(2)/x
426 : *
427 : * This equation means that, for the chance of an action with penalty x to be 0.5, LAMBDA needs to conform to this equation.
428 : *
429 : * The current value LAMBDA = 0.1 means the x that gives a 50% chance is 6.931.
430 : */
431 : const double LAMBDA = 100;
432 :
433 0 : shared_ptr<Env::Action> QLearner::Policy::pickConnection(Env::Env& envir) {
434 0 : Env::Vehicle& vehicle = envir.getVehicle(vehicleID);
435 :
436 0 : auto& sinks = vehicle.toTAZ.sinks;
437 0 : if(sinks.find(vehicle.position.lane.edge) != sinks.end()) {
438 0 : return make_shared<QLearner::Policy::ActionLeave>(vehicle.position.lane, qLearner);
439 : }
440 :
441 0 : State s = vehicle.position.lane;
442 :
443 0 : vector<pair<double, QLearner::Action>> actions;
444 0 : {
445 0 : vector<QLearner::Action> actionsVtr = s.possibleActions();
446 :
447 0 : actions.reserve(actionsVtr.size());
448 :
449 0 : for(const QLearner::Action& a: actionsVtr) {
450 0 : Reward q = qLearner.Q(s, a);
451 0 : q += qLearner.heuristic(s, a);
452 0 : q += qLearner.tabu(s, a, vehicle);
453 :
454 0 : if(q <= -numeric_limits<Reward>::infinity()) continue;
455 :
456 0 : actions.emplace_back(q, a);
457 : }
458 :
459 0 : sort(actions.begin(), actions.end(), [](const auto& a, const auto& b) -> bool {
460 : return a.first > b.first;
461 : });
462 :
463 0 : assert(actions.empty() || actions.back().first > -numeric_limits<Reward>::infinity());
464 : }
465 :
466 0 : if(actions.empty())
467 0 : return make_shared<ActionLeave>(vehicle.position.lane, qLearner);
468 :
469 0 : uniform_real_distribution<float> probDistribution(0.0, 1.0);
470 :
471 0 : float p = probDistribution(gen);
472 0 : if(p < qLearner.epsilon) {
473 : // Pick random connection
474 0 : uniform_int_distribution<size_t> actionsDistribution(0, actions.size() - 1);
475 :
476 0 : const QLearner::Action& a = actions.at(actionsDistribution(gen)).second;
477 :
478 0 : return make_shared<QLearner::Policy::Action>(a.connection, a.lane, qLearner);
479 : } else {
480 : // Pick among the best
481 0 : vector<double> chances;
482 0 : chances.reserve(actions.size());
483 0 : for(auto it = actions.begin(); it != actions.end(); ++it) {
484 0 : double delta = actions.front().first - it->first;
485 0 : double chance = exp(-LAMBDA * delta);
486 0 : it->first = chance;
487 0 : chances.emplace_back(chance);
488 : }
489 :
490 0 : discrete_distribution<size_t> actionsDistribution(chances.begin(), chances.end());
491 :
492 0 : size_t n = actionsDistribution(gen);
493 :
494 0 : const QLearner::Action& a = actions.at(n).second;
495 :
496 0 : return make_shared<QLearner::Policy::Action>(a.connection, a.lane, qLearner);
497 : }
498 : }
499 :
500 0 : QLearner::Policy::Action::Action(Env::Connection& connection_, Env::Lane& lane_, QLearner& qLearner_):
501 : Env::Action(connection_, lane_),
502 0 : qLearner(qLearner_) {}
503 :
504 0 : void QLearner::Policy::Action::reward(Reward r) {
505 0 : assert(r <= 0);
506 :
507 0 : auto& sinks = qLearner.destinationTAZ.sinks;
508 0 : if(sinks.find(connection.toLane.edge) != sinks.end()) {
509 0 : return;
510 : }
511 :
512 0 : State s = connection.fromLane;
513 0 : QLearner::Action a = {connection, lane};
514 0 : qLearner.updateMatrix(s, a, r);
515 : }
516 :
517 0 : QLearner::Policy::ActionLeave::ActionLeave(Env::Lane& stateLane_, QLearner& qLearner_):
518 : Action(Env::Connection::LEAVE, Env::Lane::INVALID, qLearner_),
519 0 : stateLane(stateLane_) {}
520 :
521 0 : void QLearner::Policy::ActionLeave::reward(Reward) {
522 0 : auto& sinks = qLearner.destinationTAZ.sinks;
523 0 : if(sinks.find(stateLane.edge) == sinks.end()) {
524 0 : for(Env::Connection& conn: stateLane.edge.getIncomingConnections()) {
525 0 : State s = conn.fromLane;
526 0 : QLearner::Action a = {conn, stateLane};
527 :
528 0 : qLearner.updateMatrix(s, a, -numeric_limits<Reward>::infinity());
529 : }
530 : }
531 0 : }
532 :
533 0 : QLearner::Policy::Factory::Factory(
534 : Env::Env& env_,
535 : const SUMO::NetworkTAZs& sumo_,
536 : const Dynamic::SUMOAdapter& adapter_,
537 : random_device::result_type seed,
538 : optional<reference_wrapper<QLearner::Logger>> policyLogger_
539 0 : ):
540 : env(env_),
541 : sumo(sumo_),
542 : adapter(adapter_),
543 : gen(seed),
544 0 : policyLogger(policyLogger_) {}
545 :
546 0 : shared_ptr<Policy> QLearner::Policy::Factory::create(
547 : Vehicle::ID id,
548 : Time,
549 : const Env::TAZ&,
550 : const Env::TAZ& toTAZ
551 : ) {
552 0 : auto it = qLearners.find(toTAZ.id);
553 0 : if(it == qLearners.end()) {
554 : // clang-format off
555 0 : it = qLearners.emplace(
556 0 : toTAZ.id,
557 0 : Dynamic::QLearner(
558 : env,
559 0 : sumo.network,
560 : adapter,
561 : toTAZ,
562 : policyLogger
563 : )
564 0 : ).first;
565 : // clang-format on
566 : }
567 :
568 0 : QLearner& qL = it->second;
569 :
570 0 : return make_shared<QLearner::Policy>(qL, id, gen);
571 : }
572 :
573 0 : void QLearner::Policy::Factory::dump() const {
574 0 : for(const auto& pr: qLearners) {
575 0 : const QLearner& qL = pr.second;
576 0 : qL.dump();
577 : }
578 0 : }
579 :
580 0 : QLearner::Logger::Logger(Reward alpha_):
581 0 : alpha(alpha_) {}
582 :
583 0 : void QLearner::Logger::header(Log::ProgressLogger& logger) {
584 0 : logger
585 0 : << "d\t"
586 0 : << "absDrift\t";
587 0 : }
588 :
589 0 : void QLearner::Logger::log(Log::ProgressLogger& logger) {
590 0 : double d = (DA > 0.0 ? D / DA : 0.0);
591 :
592 0 : logger
593 0 : << d << "\t"
594 0 : << DA / alpha << "\t";
595 0 : }
596 :
597 0 : void QLearner::Logger::setAlpha(Reward alpha_) {
598 0 : alpha = alpha_;
599 0 : }
|
