diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index d408bd1b341ca823ba5bfd3606102dee36146359..5507cdeb2f34973cd052216cd079f8087617595c 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -8,6 +8,7 @@ armarx_add_library(algorithms
./algorithms.cpp
# A*
./astar/AStarPlanner.cpp
+ ./astar/NavigationCostMap.cpp
./astar/Node.cpp
./astar/Planner2D.cpp
# smoothing
@@ -17,6 +18,7 @@ armarx_add_library(algorithms
./algorithms.h
# A*
./astar/AStarPlanner.h
+ ./astar/NavigationCostMap.h
./astar/Node.h
./astar/Planner2D.h
# smoothing
diff --git a/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp b/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08bfd27ad97f59aae72bbce55a41e31fdd9ae2c8
--- /dev/null
+++ b/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
@@ -0,0 +1,199 @@
+#include "NavigationCostMap.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/detail/intersects/interface.hpp>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <VirtualRobot/BoundingBox.h>
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/XML/ObjectIO.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/conversions/eigen.h>
+
+namespace bg = boost::geometry;
+
+namespace armarx::navigation::algorithm::astar
+{
+
+ bool
+ intersects(const VirtualRobot::BoundingBox& bb1, const VirtualRobot::BoundingBox& bb2)
+ {
+ using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
+ using box_t = bg::model::box<point_t>;
+
+ const auto toPoint = [](const Eigen::Vector3f& pt)
+ { return point_t(pt.x(), pt.y(), pt.z()); };
+
+ box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
+ box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
+
+ return bg::intersects(box1, box2);
+ }
+
+ NavigationCostMap::NavigationCostMap(VirtualRobot::RobotPtr robot,
+ VirtualRobot::SceneObjectSetPtr obstacles,
+ const Parameters& parameters) :
+ robot(robot), obstacles(obstacles), parameters(parameters)
+ {
+ }
+
+ NavigationCostMap::Position
+ NavigationCostMap::toPosition(const NavigationCostMap::Index& index) const
+ {
+ Eigen::Vector2f pos;
+ // TODO check if x and y must be switched
+ pos.x() = sceneBoundsMin.x() + index.x() * parameters.cellSize + parameters.cellSize / 2;
+ pos.y() = sceneBoundsMin.y() + index.y() * parameters.cellSize + parameters.cellSize / 2;
+
+ return pos;
+ }
+
+ void
+ NavigationCostMap::createUniformGrid()
+ {
+ ARMARX_TRACE;
+
+ ARMARX_DEBUG << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
+
+ //+1 for explicit rounding up
+ c_x = (sceneBoundsMax.x() - sceneBoundsMin.x()) / parameters.cellSize + 1;
+ c_y = (sceneBoundsMax.y() - sceneBoundsMin.y()) / parameters.cellSize + 1;
+
+ ARMARX_DEBUG << "Grid size: " << c_y << ", " << c_x;
+
+ grid = Eigen::ArrayXf(c_x, c_y);
+ }
+
+
+ float
+ NavigationCostMap::computeCost(const NavigationCostMap::Position& position)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel);
+ ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
+ ARMARX_CHECK_NOT_NULL(obstacles);
+
+ const core::Pose globalPose = Eigen::Translation3f(conv::to3D(position)) *
+ Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
+
+ robotCollisionModel->setGlobalPose(globalPose.matrix());
+
+ VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
+
+ Eigen::Vector3f P1;
+ Eigen::Vector3f P2;
+ int id1, id2;
+
+ float minDistance = std::numeric_limits<float>::max();
+
+ // TODO omp...
+ for (size_t i = 0; i < obstacles->getSize(); i++)
+ {
+ // cheap collision check
+ // VirtualRobot::BoundingBox obstacleBbox =
+ // obstacles->getSceneObject(i)->getCollisionModel()->getBoundingBox(true);
+ // if (not intersects(robotBbox, obstacleBbox))
+ // {
+ // continue;
+ // }
+
+ // precise collision check
+ const float dist =
+ VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
+ robotCollisionModel,
+ obstacles->getSceneObject(i)->getCollisionModel(),
+ P1,
+ P2,
+ &id1,
+ &id2);
+
+ // check if objects collide
+ if (dist <= parameters.cellSize / 2)
+ {
+ minDistance = 0;
+ break;
+ }
+
+ minDistance = std::min(minDistance, dist);
+ }
+ // return n->position.x() >= sceneBoundsMin.x() && n->position.x() <= sceneBoundsMax.x() &&
+ // n->position.y() >= sceneBoundsMin.y() && n->position.y() <= sceneBoundsMax.y();
+
+ return minDistance;
+ }
+
+ NavigationCostMap::Vertex
+ NavigationCostMap::toVertex(Eigen::Vector2f v)
+ {
+ float v_y = (v.y() - parameters.cellSize / 2 - sceneBoundsMin.y()) / parameters.cellSize;
+ float v_x = (v.x() - parameters.cellSize / 2 - sceneBoundsMin.x()) / parameters.cellSize;
+
+ ARMARX_CHECK(v_y >= 0.F);
+ ARMARX_CHECK(v_x >= 0.F);
+
+ ARMARX_CHECK(v_y <= (c_y - 1));
+ ARMARX_CHECK(v_x <= (c_x - 1));
+
+ return Vertex{.index = Index{static_cast<int>(v_x), static_cast<int>(v_y)}, .position = v};
+ }
+
+ void
+ NavigationCostMap::createCostmap()
+ {
+ ARMARX_TRACE;
+ std::vector<Eigen::Vector2f> result;
+
+ ARMARX_CHECK_NOT_NULL(robot);
+
+ // ARMARX_CHECK(robot->hasRobotNode(robotColModelName)) << "No robot node " << robotColModelName;
+ // ARMARX_CHECK(robot->getRobotNode(robotColModelName)->getCollisionModel())
+ // << "No collision model for robot node " << robotColModelName;
+
+ // robotCollisionModel = robot->getRobotNode(robotColModelName)->getCollisionModel()->clone();
+ robotCollisionModel->setGlobalPose(robot->getGlobalPose());
+
+ // static auto cylinder = VirtualRobot::ObjectIO::loadObstacle("/home/fabi/cylinder.xml");
+ // ARMARX_CHECK_NOT_NULL(cylinder);
+ // cylinder->setGlobalPose(robot->getGlobalPose());
+
+ // robotCollisionModel = cylinder->getCollisionModel();
+
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel);
+
+ createUniformGrid();
+
+ fillGridCosts();
+ }
+
+ void
+ NavigationCostMap::setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel)
+ {
+ robotCollisionModel = collisionModel;
+ }
+
+ void NavigationCostMap::fillGridCosts()
+ {
+ for (int x = 0; x < c_x; x++)
+ {
+ for (int y = 0; y < c_y; y++)
+ {
+ const Index index{x, y};
+ const Position position = toPosition(index);
+
+ grid(x, y) = computeCost(position);
+ }
+ }
+ }
+
+} // namespace armarx::navigation::algorithm::astar
diff --git a/source/armarx/navigation/algorithms/astar/NavigationCostMap.h b/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6bacdcf88c82bf2a97c76be19d6251e33b380db
--- /dev/null
+++ b/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
@@ -0,0 +1,79 @@
+#pragma once
+
+#include <cstddef>
+
+#include <Eigen/Core>
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+
+#include "Node.h"
+#include "Planner2D.h"
+
+namespace armarx::navigation::algorithm::astar
+{
+
+
+ /**
+ * The A* planner
+ */
+ class NavigationCostMap
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ struct Parameters
+ {
+ // if set to false, distance to obstacles will be computed and not only a binary collision check
+ bool binaryGrid{false};
+
+ /// How big each cell is in the uniform grid.
+ float cellSize = 100.f;
+ };
+
+ NavigationCostMap(VirtualRobot::RobotPtr robot,
+ VirtualRobot::SceneObjectSetPtr obstacles,
+ const Parameters& parameters);
+
+ void createCostmap();
+
+ void setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel);
+
+ using Index = Eigen::Array2i;
+ using Position = Eigen::Vector2f;
+
+ struct Vertex
+ {
+ Index index; // row corresponds to y; column corresponds to x
+ Position position;
+ };
+
+ private:
+ void createUniformGrid();
+ float computeCost(const NavigationCostMap::Position& position);
+ Vertex toVertex(Position v);
+
+ void fillGridCosts();
+
+ private:
+
+ Eigen::ArrayXf grid;
+
+ Position toPosition(const Index& index) const;
+
+
+
+ size_t c_x = 0;
+ size_t c_y = 0;
+
+
+ VirtualRobot::RobotPtr robot;
+ VirtualRobot::SceneObjectSetPtr obstacles;
+
+ VirtualRobot::CollisionModelPtr robotCollisionModel;
+
+ const Eigen::Vector2f sceneBoundsMin;
+ const Eigen::Vector2f sceneBoundsMax;
+
+ const Parameters parameters;
+ };
+} // namespace armarx::navigation::algorithm::astar