Commit 6d8539d9 authored by David Speck's avatar David Speck

added expansion controller

parent f733e56d
......@@ -718,6 +718,9 @@ fast_downward_plugin(
symple/open_list
symple/closed_list
symple/sym_astar
symple/expansion_controller/expansion_controller
symple/expansion_controller/astar_controller
symple/expansion_controller/nbs_controller
symple/plan_reconstruction
symple/sym_enums
# symple/heuristics/bidirectional_potential
......
#include "../sym_astar.h"
#include "../sym_variables.h"
#include "../symple_search.h"
#include "astar_controller.h"
namespace symple
{
AStarController::AStarController(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> fwd_astar,
std::shared_ptr<SymAstar> bwd_astar)
: ExpansionController(sym_vars, fwd_astar, bwd_astar) {}
SearchStatus AStarController::expansion_step(
const std::vector<TransitionRelation> &merged_trs,
const SympleParams &params) const
{
if (fwd_astar->open_list_empty() || bwd_astar->open_list_empty())
{
if (fwd_astar->has_collision() || bwd_astar->has_collision())
{
return SearchStatus::SOLVED;
}
return SearchStatus::FAILED;
}
// Image
bool opt_sol = false;
Dir step_dir = next_step_dir(params);
std::cout << step_dir << ": " << std::flush;
if (step_dir == Dir::FWD)
{
opt_sol = fwd_astar->step(merged_trs);
std::cout << " [" << fwd_astar->last_step_time() << "s] ";
}
else if (step_dir == Dir::BWD)
{
opt_sol = bwd_astar->step(merged_trs);
std::cout << " [" << bwd_astar->last_step_time() << "s] ";
}
std::cout << "Overall: "
<< fwd_astar->get_overall_time() + bwd_astar->get_overall_time()
<< "s" << std::endl;
// Optimal solution?
return opt_sol ? SearchStatus::SOLVED : SearchStatus::IN_PROGRESS;
}
Dir AStarController::next_step_dir(const SympleParams &params) const
{
if (params.search_dir != Dir::BID)
{
return params.search_dir;
}
if (fwd_astar->last_step_time() <= bwd_astar->last_step_time())
{
return Dir::FWD;
}
return Dir::BWD;
}
} // namespace symple
\ No newline at end of file
#ifndef SYMPLE_EXPANDSION_CONTROLLER_ASTAR_CONTROLLER_H
#define SYMPLE_EXPANDSION_CONTROLLER_ASTAR_CONTROLLER_H
#include "expansion_controller.h"
#include <memory>
namespace symple {
class SymAstar;
class SymVariables;
class TransitionRelation;
class AStarController : public ExpansionController {
public:
AStarController(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> fwd_astar,
std::shared_ptr<SymAstar> bwd_astar);
virtual SearchStatus
expansion_step(const std::vector<TransitionRelation> &merged_trs,
const SympleParams &params) const override;
protected:
Dir next_step_dir(const SympleParams &params) const;
};
} // namespace symple
#endif
\ No newline at end of file
#include "expansion_controller.h"
#include "../sym_astar.h"
#include "../sym_variables.h"
#include "../symple_search.h"
namespace symple {
ExpansionController::ExpansionController(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> fwd_astar,
std::shared_ptr<SymAstar> bwd_astar)
: sym_vars(sym_vars), fwd_astar(fwd_astar), bwd_astar(bwd_astar) {}
} // namespace symple
\ No newline at end of file
#ifndef SYMPLE_EXPANDSION_CONTROLLER_EXPANDSION_CONTROLLER_H
#define SYMPLE_EXPANDSION_CONTROLLER_EXPANDSION_CONTROLLER_H
#include "../symple_search.h"
#include <memory>
namespace symple {
class SymAstar;
class SymVariables;
class TransitionRelation;
class ExpansionController {
public:
ExpansionController(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> fwd_astar,
std::shared_ptr<SymAstar> bwd_astar);
virtual SearchStatus
expansion_step(const std::vector<TransitionRelation> &merged_trs,
const SympleParams &params) const = 0;
protected:
std::shared_ptr<SymVariables> sym_vars;
std::shared_ptr<SymAstar> fwd_astar;
std::shared_ptr<SymAstar> bwd_astar;
};
} // namespace symple
#endif
\ No newline at end of file
#include "nbs_controller.h"
#include "../Meddly/operations_extension/userOperations.h"
#include "../open_list.h"
#include "../sym_astar.h"
#include "../sym_variables.h"
#include "../symple_search.h"
namespace symple
{
NbsController::NbsController(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> fwd_astar,
std::shared_ptr<SymAstar> bwd_astar)
: ExpansionController(sym_vars, fwd_astar, bwd_astar) {}
SearchStatus
NbsController::expansion_step(const std::vector<TransitionRelation> &merged_trs,
const SympleParams & /*params*/) const
{
if (fwd_astar->open_list_empty() || bwd_astar->open_list_empty())
{
if (fwd_astar->has_collision() || bwd_astar->has_collision())
{
return SearchStatus::SOLVED;
}
return SearchStatus::FAILED;
}
// Image
std::cout << "NBS: " << std::flush;
bool opt_sol = false;
auto best_pairs = most_promising_pairs();
auto best_fwd_pairs = sym_vars->min_abstract_all(best_pairs, true);
MEDDLY::apply(USER_OPS::EVPLUS, fwd_astar->get_open()->g_values(),
best_fwd_pairs, best_fwd_pairs);
opt_sol = fwd_astar->step(merged_trs, best_fwd_pairs);
std::cout << " [" << fwd_astar->last_step_time() << "s] ";
if (!opt_sol)
{
fwd_astar->get_open()->remove_states(best_fwd_pairs);
auto best_bwd_pairs = sym_vars->min_abstract_all(best_pairs, false);
best_bwd_pairs = sym_vars->rename_all(best_bwd_pairs, true);
MEDDLY::apply(USER_OPS::EVPLUS, bwd_astar->get_open()->g_values(),
best_bwd_pairs, best_bwd_pairs);
opt_sol = bwd_astar->step(merged_trs, best_bwd_pairs);
std::cout << " [" << bwd_astar->last_step_time() << "s] ";
if (!opt_sol)
{
bwd_astar->get_open()->remove_states(best_bwd_pairs);
}
}
std::cout << "Overall: "
<< fwd_astar->get_overall_time() + bwd_astar->get_overall_time()
<< "s" << std::endl;
// Optimal solution?
return opt_sol ? SearchStatus::SOLVED : SearchStatus::IN_PROGRESS;
}
MEDDLY::dd_edge NbsController::most_promising_pairs() const
{
MEDDLY::dd_edge result = sym_vars->get_constant_edge(0);
// Max of f-values of both directions
auto fwd_f_values = fwd_astar->get_open()->f_values();
auto bwd_f_values = bwd_astar->get_open()->f_values();
bwd_f_values = sym_vars->rename_all(bwd_f_values); // => pairs of states
MEDDLY::apply(USER_OPS::INTERSECTIONMAX, fwd_f_values, bwd_f_values, result);
// Max with added g-values
auto g_values_added = sym_vars->get_constant_edge(0);
auto fwd_g_values = fwd_astar->get_open()->g_values();
auto bwd_g_values = bwd_astar->get_open()->g_values();
bwd_g_values = sym_vars->rename_all(bwd_g_values); // => pairs of states
MEDDLY::apply(USER_OPS::EVPLUS, fwd_g_values, bwd_g_values, g_values_added);
MEDDLY::apply(USER_OPS::INTERSECTIONMAX, result, g_values_added, result);
// Get min pairs
float min_value = 0;
result.getEdgeValue(min_value);
auto min_states = sym_vars->get_constant_edge(0);
MEDDLY::apply(USER_OPS::EQUALS, sym_vars->get_constant_edge(min_value),
result, min_states);
MEDDLY::apply(USER_OPS::MINUS, sym_vars->get_constant_edge(1), min_states,
min_states);
MEDDLY::apply(USER_OPS::MULTIPLY, sym_vars->get_constant_edge(std::numeric_limits<float>::infinity()),
min_states, result);
return result;
}
} // namespace symple
\ No newline at end of file
#ifndef SYMPLE_EXPANDSION_CONTROLLER_NBS_CONTROLLER_H
#define SYMPLE_EXPANDSION_CONTROLLER_NBS_CONTROLLER_H
#include "expansion_controller.h"
#include <memory>
namespace symple
{
class SymAstar;
class SymVariables;
class TransitionRelation;
class NbsController : public ExpansionController
{
public:
NbsController(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> fwd_astar,
std::shared_ptr<SymAstar> bwd_astar);
virtual SearchStatus
expansion_step(const std::vector<TransitionRelation> &merged_trs,
const SympleParams &params) const override;
protected:
MEDDLY::dd_edge most_promising_pairs() const;
};
} // namespace symple
#endif
\ No newline at end of file
#include "open_list.h"
#include <limits>
#include "../Meddly/operations_extension/userOperations.h"
#include "heuristics/bidirectional_potential.h"
#include "sym_variables.h"
#include <limits>
namespace symple {
namespace symple
{
OpenList::OpenList(const MEDDLY::dd_edge& start,
OpenList::OpenList(const MEDDLY::dd_edge &start,
std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> other_dir)
: sym_vars(sym_vars), other_dir(other_dir), open(start) {}
bool OpenList::empty() { return open == sym_vars->get_constant_edge(INFTY); }
void OpenList::insert(const MEDDLY::dd_edge& in) {
void OpenList::insert(const MEDDLY::dd_edge &in)
{
MEDDLY::apply(USER_OPS::UNIONMIN, open, in, open);
}
// TODO(speckd): create a min-extract operation
MEDDLY::dd_edge OpenList::extract_most_promising() {
MEDDLY::dd_edge OpenList::get_most_promising() const
{
float min_cost;
open.getEdgeValue(min_cost);
MEDDLY::dd_edge res = sym_vars->get_constant_edge(min_cost);
MEDDLY::apply(USER_OPS::EQUALS, open, res, res);
// Remove states from open
MEDDLY::dd_edge not_min =
sym_vars->get_constant_edge(std::numeric_limits<float>::infinity());
MEDDLY::apply(USER_OPS::MULTIPLY, not_min, res, not_min);
MEDDLY::apply(USER_OPS::EVPLUS, not_min, open, open);
// Set costs for results
MEDDLY::apply(USER_OPS::MINUS, sym_vars->get_constant_edge(1), res, res);
MEDDLY::apply(
USER_OPS::MULTIPLY,
sym_vars->get_constant_edge(std::numeric_limits<float>::infinity()), res,
res);
MEDDLY::apply(USER_OPS::MULTIPLY, sym_vars->get_constant_edge(std::numeric_limits<float>::infinity()),
res, res);
MEDDLY::apply(USER_OPS::EVPLUS, res, sym_vars->get_constant_edge(min_cost),
res);
// std::cout << open.getNodeCount() << " => " << res.getNodeCount() <<
// std::endl;
return res;
}
void OpenList::remove_states(const MEDDLY::dd_edge states)
{
// Remove states from open
MEDDLY::dd_edge not_states = sym_vars->get_constant_edge(0);
MEDDLY::apply(USER_OPS::PARTIALCOMPLEMENT, states, not_states);
MEDDLY::apply(USER_OPS::EVPLUS, not_states, open, open);
}
// TODO(speckd): create a min-extract operation
MEDDLY::dd_edge OpenList::extract_most_promising()
{
auto res = get_most_promising();
remove_states(res);
return res;
}
MEDDLY::dd_edge OpenList::f_values() const
{
return open;
}
MEDDLY::dd_edge OpenList::g_values() const
{
return open;
}
int OpenList::min_g_value() const {
float OpenList::min_g_value() const
{
float result = -1;
open.getEdgeValue(result);
return result;
}
int OpenList::min_f_value() const { return min_g_value(); }
float OpenList::min_f_value() const { return min_g_value(); }
MEDDLY::dd_edge OpenList::get_heuristic() const {
MEDDLY::dd_edge OpenList::get_heuristic() const
{
return sym_vars->get_constant_edge(0);
}
......@@ -58,4 +79,4 @@ void OpenList::update_heuristic() {}
MEDDLY::dd_edge OpenList::get_open_list() const { return open; }
} // namespace symple
} // namespace symple
......@@ -9,23 +9,27 @@ class SymVariables;
class SymAstar;
class OpenList {
public:
OpenList(const MEDDLY::dd_edge& start, std::shared_ptr<SymVariables> sym_vars,
public:
OpenList(const MEDDLY::dd_edge &start, std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> other_dir);
virtual void insert(const MEDDLY::dd_edge& in);
virtual void insert(const MEDDLY::dd_edge &in);
virtual MEDDLY::dd_edge extract_most_promising();
virtual MEDDLY::dd_edge get_most_promising() const;
virtual void remove_states(const MEDDLY::dd_edge states);
MEDDLY::dd_edge get_open_list() const;
virtual int min_g_value() const;
virtual int min_f_value() const;
virtual float min_g_value() const;
virtual float min_f_value() const;
virtual MEDDLY::dd_edge f_values() const;
virtual MEDDLY::dd_edge g_values() const;
bool empty();
virtual MEDDLY::dd_edge get_heuristic() const;
virtual void update_heuristic();
protected:
protected:
std::shared_ptr<SymVariables> sym_vars;
std::shared_ptr<SymAstar> other_dir;
MEDDLY::dd_edge open;
};
} // namespace symple
} // namespace symple
#endif
#include "potential_open_list.h"
#include <limits>
#include "../Meddly/operations_extension/userOperations.h"
#include "../abstract_task.h"
#include "../potentials/potential_heuristic.h"
......@@ -8,32 +7,37 @@
#include "heuristics/potential_symbolic_heuristic.h"
#include "sym_astar.h"
#include "sym_variables.h"
#include <limits>
namespace symple {
namespace symple
{
PotentialOpenList::PotentialOpenList(const MEDDLY::dd_edge& start,
PotentialOpenList::PotentialOpenList(const MEDDLY::dd_edge &start,
std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> other_dir,
std::shared_ptr<AbstractTask> task,
std::shared_ptr<Heuristic> pot_heuristic)
: OpenList(start, sym_vars, other_dir),
task(task),
potenial_heuristic(nullptr),
required_heursistic_update(false),
i(0) {
: OpenList(start, sym_vars, other_dir), task(task),
potenial_heuristic(nullptr), required_heursistic_update(false)
{
potenial_heuristic =
std::dynamic_pointer_cast<PotentialSymbolicHeuristic>(pot_heuristic);
if (potenial_heuristic) {
if (potenial_heuristic)
{
potenial_heuristic->init(sym_vars);
} else {
}
else
{
std::cerr << "Symple only supports blind or symbolic potential heuristic!"
<< std::endl;
utils::exit_with(utils::ExitCode::SEARCH_INPUT_ERROR);
}
}
MEDDLY::dd_edge PotentialOpenList::extract_most_promising() {
if (required_heursistic_update) {
MEDDLY::dd_edge PotentialOpenList::get_most_promising() const
{
if (required_heursistic_update)
{
required_heursistic_update = false;
potenial_heuristic->update_other_dir_heuristic(
other_dir->get_open()->get_open_list());
......@@ -54,26 +58,29 @@ MEDDLY::dd_edge PotentialOpenList::extract_most_promising() {
MEDDLY::apply(USER_OPS::MULTIPLY, cmpl, res, cmpl);
MEDDLY::apply(USER_OPS::MINUS, sym_vars->get_constant_edge(1), res, res);
MEDDLY::apply(
USER_OPS::MULTIPLY,
sym_vars->get_constant_edge(std::numeric_limits<float>::infinity()), res,
res);
MEDDLY::apply(USER_OPS::MULTIPLY, sym_vars->get_constant_edge(std::numeric_limits<float>::infinity()),
res, res);
MEDDLY::apply(USER_OPS::INTERSECTIONMAX, res, open, res);
// Remove from open list
MEDDLY::apply(USER_OPS::EVPLUS, open, cmpl, open);
i++;
return res;
}
void PotentialOpenList::update_heuristic() {
void PotentialOpenList::update_heuristic()
{
// required_heursistic_update = true;
}
int PotentialOpenList::min_g_value() const { return OpenList::min_g_value(); }
MEDDLY::dd_edge PotentialOpenList::f_values() const
{
MEDDLY::dd_edge f_values = sym_vars->get_constant_edge(0);
MEDDLY::apply(USER_OPS::EVPLUS, open, potenial_heuristic->get_heuristic(),
f_values);
return f_values;
}
float PotentialOpenList::min_g_value() const { return OpenList::min_g_value(); }
int PotentialOpenList::min_f_value() const {
float PotentialOpenList::min_f_value() const
{
float min_value = -1;
auto f_values = sym_vars->get_constant_edge(0);
MEDDLY::apply(USER_OPS::EVPLUS, open, potenial_heuristic->get_heuristic(),
......@@ -82,8 +89,9 @@ int PotentialOpenList::min_f_value() const {
return min_value;
}
MEDDLY::dd_edge PotentialOpenList::get_heuristic() const {
MEDDLY::dd_edge PotentialOpenList::get_heuristic() const
{
return potenial_heuristic->get_heuristic();
}
} // namespace symple
} // namespace symple
#ifndef SYMPLE_POTENTIAL_OPEN_LIST_H
#define SYMPLE_POTENTIAL_OPEN_LIST_H
#include <meddly.h>
#include "heuristics/bidirectional_potential.h"
#include "open_list.h"
#include <meddly.h>
class AbstractTask;
class Heuristic;
......@@ -13,26 +13,25 @@ class SymVariables;
class PotentialSymbolicHeuristic;
class PotentialOpenList : public OpenList {
public:
PotentialOpenList(const MEDDLY::dd_edge& start,
public:
PotentialOpenList(const MEDDLY::dd_edge &start,
std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<SymAstar> other_dir,
std::shared_ptr<AbstractTask> task,
std::shared_ptr<Heuristic> pot_heuristic);
// void insert(const MEDDLY::dd_edge& in) override;
MEDDLY::dd_edge extract_most_promising() override;
int min_g_value() const override;
int min_f_value() const override;
virtual MEDDLY::dd_edge get_most_promising() const override;
virtual MEDDLY::dd_edge f_values() const override;
float min_g_value() const override;
float min_f_value() const override;
MEDDLY::dd_edge get_heuristic() const override;
void update_heuristic() override;
protected:
protected:
std::shared_ptr<AbstractTask> task;
std::shared_ptr<PotentialSymbolicHeuristic> potenial_heuristic;
bool required_heursistic_update;
int i;
mutable bool required_heursistic_update;
};
} // namespace symple
} // namespace symple
#endif
#include "sym_astar.h"
#include <limits>
#include "../Meddly/operations_extension/userOperations.h"
#include "../abstract_task.h"
#include "../heuristic.h"
......@@ -12,19 +11,17 @@
#include "potential_open_list.h"
#include "sym_variables.h"
#include "transition_relation.h"
#include <limits>
namespace symple {
namespace symple
{
SymAstar::SymAstar(std::shared_ptr<SymVariables> sym_vars,
std::shared_ptr<AbstractTask> task, bool fwd)
: other_direction(nullptr),
sym_vars(sym_vars),
task(task),
fwd(fwd),
step_time(0),
overall_time(0),
min_g_last_step(0) {}
Plan SymAstar::get_plan(const std::vector<TransitionRelation> &singel_trs) {
: other_direction(nullptr), sym_vars(sym_vars), task(task), fwd(fwd),
step_time(0), overall_time(0), min_g_last_step(0) {}
Plan SymAstar::get_plan(const std::vector<TransitionRelation> &singel_trs)
{
float cost = -1;
collision.getEdgeValue(cost);
......@@ -33,7 +30,7 @@ Plan SymAstar::get_plan(const std::vector<TransitionRelation> &singel_trs) {
PlanReconstruction reconstructor(sym_vars, task);
std::vector<int> target_values = reconstructor.min_partial_state(collision);
MEDDLY::dd_edge target_state =
sym_vars->get_state_edge(target_values); // important to delete the cost
sym_vars->get_state_edge(target_values); // important to delete the cost
MEDDLY::dd_edge allReachabelStates = sym_vars->get_constant_edge(0);
MEDDLY::apply(USER_OPS::UNIONMIN, open_list->get_open_list(),
......@@ -51,13 +48,17 @@ Plan SymAstar::get_plan(const std::vector<TransitionRelation> &singel_trs) {
MEDDLY::dd_edge start_reached = sym_vars->get_constant_edge(-1);
MEDDLY::apply(USER_OPS::INTERSECTIONMAX, target_state, start, start_reached);
while (start_reached ==
sym_vars->get_constant_edge(std::numeric_limits<float>::infinity())) {
sym_vars->get_constant_edge(std::numeric_limits<float>::infinity()))
{
int id = -1;
if (fwd) {
if (fwd)
{
std::tie(id, target_state) = reconstructor.fwd_reconstruct_plan_step(
target_state, closed_list, singel_trs);
plan.insert(plan.begin(), OperatorID(id));
} else {
}
else
{
std::tie(id, target_state) = reconstructor.bwd_reconstruct_plan_step(
target_state, closed_list, singel_trs);
plan.push_back(OperatorID(id));
......@@ -69,20 +70,26 @@ Plan SymAstar::get_plan(const std::vector<TransitionRelation> &singel_trs) {
}
std::cout << "DONE!" << std::endl;
return plan;
} // TODO fix
} // TODO fix
void SymAstar::init(const MEDDLY::dd_edge &start,
std::shared_ptr<SymAstar> other_dir,
std::shared_ptr<Heuristic> heuristic) {
std::shared_ptr<Heuristic> heuristic)
{
this->start = start;
other_direction = other_dir;
if (std::dynamic_pointer_cast<blind_search_heuristic::BlindSearchHeuristic>(
heuristic) != nullptr) {
heuristic) != nullptr)
{
open_list = std::make_shared<OpenList>(start, sym_vars, other_direction);
} else if (std::dynamic_pointer_cast<PotentialSymbolicHeuristic>(heuristic)) {
}
else if (std::dynamic_pointer_cast<PotentialSymbolicHeuristic>(heuristic))
{
open_list = std::make_shared<PotentialOpenList>(
start, sym_vars, other_direction, task, heuristic);
} else {
}
else
{
std::cerr << "Symple only supports blind or potential heuristic!"
<< std::endl;
utils::exit_with(utils::ExitCode::SEARCH_INPUT_ERROR);
......@@ -92,22 +99,39 @@ void SymAstar::init(const MEDDLY::dd_edge &start,
collision = sym_vars->get_constant_edge(INFTY);
}
bool SymAstar::step(const std::vector<TransitionRelation> &trs) {
bool SymAstar::step(const std::vector<TransitionRelation> &trs)
{
MEDDLY::dd_edge states = open_list->get_most_promising();
auto solved = step(trs, states);
if (!solved)
{
open_list->remove_states(states);
}
return solved;
}
bool SymAstar::step(const std::vector<TransitionRelation> &trs,
const MEDDLY::dd_edge states)
{
utils::Timer timer;
if (check_collision()) {
if (check_collision())
{
USER_OPS::to_dot(open_list->get_open_list(), "open.dot");
step_time = timer();
overall_time += step_time;
return true;
}
MEDDLY::dd_edge new_states = sym_vars->get_constant_edge(INFTY);
MEDDLY::dd_edge states = open_list->extract_most_promising();
// std::cout << "image..." << std::flush;
for (auto &tr : trs) {
if (fwd) {
for (auto &tr : trs)
{
if (fwd)
{
MEDDLY::apply(USER_OPS::UNIONMIN, new_states, tr.image(states),
new_states);
} else {
}
else
{
MEDDLY::apply(USER_OPS::UNIONMIN, new_states, tr.preimage(states),
new_states);
}
......@@ -127,7 +151,8 @@ bool SymAstar::step(const std::vector<TransitionRelation> &trs) {
}
// TODO(speckd): refactor
bool SymAstar::check_collision() {
bool SymAstar::check_collision()
{
MEDDLY::dd_edge cur_collision = sym_vars->get_constant_edge(-1);
MEDDLY::dd_edge states_this_dir = sym_vars->get_constant_edge(-1);
MEDDLY::dd_edge states_other_dir = sym_vars->get_constant_edge(-1);
......@@ -144,7 +169,8 @@ bool SymAstar::check_collision() {
cur_collision.getEdgeValue(cur_collision_cost);
float old_collision_cost = -1;