update

source/Navigation/components/Navigator/CMakeLists.txt

@@ -32,9 +32,9 @@ armarx_add_component(
         # This component
         NavigatorInterfaces
 
-        Navigation::server
-        Navigation::util
-        Navigation::factories
+        Navigation::Server
+        Navigation::Util
+        Navigation::Factories
 
     SOURCES
         Navigator.cpp

source/Navigation/components/Navigator/test/CMakeLists.txt

-
 # Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore Navigator)
- 
-armarx_add_test(NavigatorTest NavigatorTest.cpp "${LIBS}")
+set(
+    LIBS
+    ${LIBS}
+    ArmarXCore
+    Navigator
+)
+
+armarx_add_test(
+    NavigatorTest
+    NavigatorTest.cpp
+    "${LIBS}"
+)

source/Navigation/libraries/CMakeLists.txt

@@ -9,3 +9,5 @@ add_subdirectory(factories)
 add_subdirectory(safety_control)
 add_subdirectory(server)
 add_subdirectory(util)
+
+add_subdirectory(conversions)
\ No newline at end of file

source/Navigation/libraries/algorithms/CMakeLists.txt

@@ -7,15 +7,24 @@ armarx_add_library(
     LIBS
         ArmarXCoreInterfaces
         ArmarXCore
-        Navigation::core
+        Navigation::Core
+        Navigation::Conversions
     SOURCES
         ./algorithms.cpp
+        # A*
+        ./astar/AStarPlanner.cpp
+        ./astar/Node.cpp
+        ./astar/Planner2D.cpp
     HEADERS
         ./algorithms.h
+        # A*
+        ./astar/AStarPlanner.h
+        ./astar/Node.h
+        ./astar/Planner2D.h
 )
 
 add_library(
-    Navigation::algorithms
+    Navigation::Algorithms
     ALIAS
     ${PROJECT_NAME}Algorithms
 )

source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp

+#include "AStarPlanner.h"
+
+#include <algorithm>
+#include <cmath>
+
+#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 "Navigation/libraries/conversions/eigen.h"
+
+namespace bg = boost::geometry;
+
+namespace armarx::nav::algo::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);
+    }
+
+    AStarPlanner::AStarPlanner(VirtualRobot::RobotPtr robot,
+                               VirtualRobot::SceneObjectSetPtr obstacles,
+                               float cellSize) :
+        Planner2D(robot, obstacles), cellSize(cellSize)
+    {
+    }
+
+    float AStarPlanner::heuristic(NodePtr n1, NodePtr n2)
+    {
+        return (n1->position - n2->position).norm();
+    }
+
+    void AStarPlanner::createUniformGrid()
+    {
+        ARMARX_TRACE;
+
+        ARMARX_INFO << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
+
+        //+1 for explicit rounding up
+        cols = (sceneBoundsMax.x() - sceneBoundsMin.x()) / cellSize + 1;
+        rows = (sceneBoundsMax.y() - sceneBoundsMin.y()) / cellSize + 1;
+
+        ARMARX_INFO << "Grid size: " << rows << ", " << cols;
+
+        for (size_t r = 0; r < rows; r++)
+        {
+            grid.push_back(std::vector<NodePtr>(cols));
+            for (size_t c = 0; c < cols; c++)
+            {
+                Eigen::Vector2f pos;
+                pos.x() = sceneBoundsMin.x() + c * cellSize + cellSize / 2;
+                pos.y() = sceneBoundsMin.y() + r * cellSize + cellSize / 2;
+                grid[r][c] = NodePtr(new Node(pos));
+            }
+        }
+
+        // Init successors
+        for (size_t r = 0; r < rows; r++)
+        {
+            for (size_t c = 0; c < cols; c++)
+            {
+                NodePtr candidate = grid[r][c];
+                if (!fulfillsConstraints(candidate))
+                {
+                    continue;
+                }
+
+                // Add the valid node as successor to all its neighbors
+                for (int nR = -1; nR <= 1; nR++)
+                {
+                    for (int nC = -1; nC <= 1; nC++)
+                    {
+                        const int neighborIndexX = c + nC;
+                        const int neighborIndexY = r + nR;
+                        if (neighborIndexX < 0 || neighborIndexY < 0 || (nR == 0 && nC == 0))
+                        {
+                            continue;
+                        }
+
+                        if (neighborIndexY >= static_cast<int>(rows) ||
+                            neighborIndexX >= static_cast<int>(cols))
+                        {
+                            continue;
+                        }
+
+                        grid[neighborIndexY][neighborIndexX]->successors.push_back(candidate);
+                    }
+                }
+            }
+        }
+    }
+
+    bool AStarPlanner::fulfillsConstraints(NodePtr n)
+    {
+        ARMARX_TRACE;
+        ARMARX_CHECK_NOT_NULL(n);
+        ARMARX_CHECK_NOT_NULL(robotCollisionModel);
+        ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
+        ARMARX_CHECK_NOT_NULL(obstacles);
+
+        const Eigen::Affine3f globalPose =
+            Eigen::Translation3f(conv::to3D(n->position)) *
+            Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
+
+        robotCollisionModel->setGlobalPose(globalPose.matrix());
+
+        VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
+
+        Eigen::Vector3f P1, P2;
+        int id1, id2;
+        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
+            float dist =
+                VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
+                    robotCollisionModel,
+                    obstacles->getSceneObject(i)->getCollisionModel(),
+                    P1,
+                    P2,
+                    &id1,
+                    &id2);
+
+            if (dist <= cellSize / 2)
+            {
+                return false;
+            }
+        }
+        return n->position.x() >= sceneBoundsMin.x() && n->position.x() <= sceneBoundsMax.x() &&
+               n->position.y() >= sceneBoundsMin.y() && n->position.y() <= sceneBoundsMax.y();
+    }
+
+    NodePtr AStarPlanner::closestNode(Eigen::Vector2f v)
+    {
+        float r = (v.y() - cellSize / 2 - sceneBoundsMin.y()) / cellSize;
+        float c = (v.x() - cellSize / 2 - sceneBoundsMin.x()) / cellSize;
+
+        ARMARX_CHECK(r >= 0.F);
+        ARMARX_CHECK(c >= 0.F);
+
+        ARMARX_CHECK(r <= (rows - 1));
+        ARMARX_CHECK(c <= (cols - 1));
+
+        return grid[static_cast<int>(r)][static_cast<int>(c)];
+    }
+
+    std::vector<Eigen::Vector2f> AStarPlanner::plan(Eigen::Vector2f start, Eigen::Vector2f goal)
+    {
+        ARMARX_TRACE;
+        grid.clear();
+        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();
+        ARMARX_INFO << "Setting start node";
+        NodePtr nodeStart = closestNode(start);
+
+        ARMARX_INFO << "Setting goal node";
+        NodePtr nodeGoal = closestNode(goal);
+
+        std::vector<NodePtr> closedSet;
+        std::vector<NodePtr> openSet;
+        openSet.push_back(nodeStart);
+        std::map<NodePtr, float> gScore;
+        gScore[nodeStart] = 0;
+        std::map<NodePtr, float> fScore;
+        fScore[nodeStart] = gScore[nodeStart] + heuristic(nodeStart, nodeGoal);
+
+        while (!openSet.empty())
+        {
+            float lowestScore = fScore[openSet[0]];
+            auto currentIT = openSet.begin();
+            for (auto it = openSet.begin() + 1; it != openSet.end(); it++)
+            {
+                if (fScore[*it] < lowestScore)
+                {
+                    lowestScore = fScore[*it];
+                    currentIT = it;
+                }
+            }
+            NodePtr currentBest = *currentIT;
+            if (currentBest == nodeGoal)
+            {
+                break;
+            }
+
+            openSet.erase(currentIT);
+            closedSet.push_back(currentBest);
+            for (size_t i = 0; i < currentBest->successors.size(); i++)
+            {
+                NodePtr neighbor = currentBest->successors[i];
+                if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end())
+                {
+                    continue;
+                }
+
+                float tentativeGScore = gScore[currentBest] + heuristic(currentBest, neighbor);
+                bool notInOS = std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end();
+                if (notInOS || tentativeGScore < gScore[neighbor])
+                {
+                    neighbor->predecessor = currentBest;
+                    gScore[neighbor] = tentativeGScore;
+                    fScore[neighbor] = tentativeGScore + heuristic(neighbor, nodeGoal);
+                    if (notInOS)
+                    {
+                        openSet.push_back(neighbor);
+                    }
+                }
+            }
+        }
+
+        // Found solution, now retrieve path from goal to start
+        if (nodeGoal)
+        {
+            result = nodeGoal->traversePredecessors();
+        }
+
+        // Since the graph was traversed from goal to start, we have to reverse the order
+        std::reverse(result.begin(), result.end());
+
+        return result;
+    }
+
+    void AStarPlanner::setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel)
+    {
+        robotCollisionModel = collisionModel;
+    }
+
+} // namespace armarx::nav::algo::astar

source/Navigation/libraries/algorithms/astar/AStarPlanner.h

+#pragma once
+
+#include <cstddef>
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+
+#include "Node.h"
+#include "Planner2D.h"
+
+namespace armarx::nav::algo::astar
+{
+
+    /**
+ * The A* planner
+ */
+    class AStarPlanner : public Planner2D
+    {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        AStarPlanner(VirtualRobot::RobotPtr robot,
+                     VirtualRobot::SceneObjectSetPtr obstacles = {},
+                     float cellSize = 100.f);
+
+        std::vector<Eigen::Vector2f> plan(Eigen::Vector2f start, Eigen::Vector2f goal);
+
+        void setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel);
+
+    private:
+        float heuristic(NodePtr n1, NodePtr n2);
+        void createUniformGrid();
+        bool fulfillsConstraints(NodePtr n);
+        NodePtr closestNode(Eigen::Vector2f v);
+
+    private:
+        /// How big each cell is in the uniform grid.
+        float cellSize;
+
+        std::vector<std::vector<NodePtr>> grid;
+
+        size_t cols = 0;
+        size_t rows = 0;
+    };
+} // namespace armarx::nav::algo::astar

source/Navigation/libraries/algorithms/astar/Node.cpp

+#include "Node.h"
+
+namespace armarx::nav::algo::astar
+{
+
+    Node::Node(Eigen::Vector2f position) : position(position)
+    {
+    }
+
+    std::vector<Eigen::Vector2f> Node::traversePredecessors()
+    {
+        std::vector<Eigen::Vector2f> result;
+        result.push_back(position);
+
+        NodePtr predecessor = this->predecessor;
+        while (predecessor)
+        {
+            result.push_back(predecessor->position);
+            predecessor = predecessor->predecessor;
+        }
+        return result;
+    }
+
+} // namespace armarx::nav::algo::astar

source/Navigation/libraries/algorithms/astar/Node.h

+#pragma once
+
+#include <vector>
+
+#include <boost/shared_ptr.hpp>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+namespace armarx::nav::algo::astar
+{
+    class Node;
+    using NodePtr = boost::shared_ptr<Node>;
+
+    /**
+    * A Node can store data to all valid neighbors (successors) and a precessor.
+    * It offers methods to determine the complete path to the starting point.
+    */
+    class Node
+    {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        Node(Eigen::Vector2f position);
+
+        Eigen::Vector2f position;
+        /// All nodes that are adjacent to this one.
+        std::vector<NodePtr> successors;
+        /// For traversal.
+        NodePtr predecessor;
+
+        /// Collects all predecessors in order to generate path to starting point.
+        std::vector<Eigen::Vector2f> traversePredecessors();
+    };
+
+} // namespace armarx::nav::algo::astar

source/Navigation/libraries/algorithms/astar/Planner2D.cpp

+#include "Planner2D.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+
+namespace armarx::nav::algo::astar
+{
+
+    Planner2D::Planner2D(VirtualRobot::RobotPtr robot, VirtualRobot::SceneObjectSetPtr obstacles)
+    {
+        sceneBoundsMin.setZero();
+        sceneBoundsMax.setZero();
+        setRobot(robot);
+        setObstacles(obstacles);
+    }
+
+    void Planner2D::setObstacles(VirtualRobot::SceneObjectSetPtr obstacles)
+    {
+        this->obstacles = obstacles;
+        if (obstacles)
+        {
+            for (size_t i = 0; i < obstacles->getCollisionModels().size(); i++)
+            {
+                VirtualRobot::BoundingBox bb = obstacles->getCollisionModels()[i]->getBoundingBox();
+                sceneBoundsMin.x() = std::min(bb.getMin().x(), sceneBoundsMin.x());
+                sceneBoundsMin.y() = std::min(bb.getMin().y(), sceneBoundsMin.y());
+                sceneBoundsMax.x() = std::max(bb.getMax().x(), sceneBoundsMax.x());
+                sceneBoundsMax.y() = std::max(bb.getMax().y(), sceneBoundsMax.y());
+            }
+        }
+    }
+
+    void Planner2D::setRobot(VirtualRobot::RobotPtr robot)
+    {
+        this->robot = robot;
+    }
+
+    void Planner2D::setRobotColModel(const std::string& robotColModelName)
+    {
+        this->robotColModelName = robotColModelName;
+    }
+
+    Eigen::Matrix4f Planner2D::positionToMatrix4f(Eigen::Vector2f pos)
+    {
+        Eigen::Matrix4f result = Eigen::Matrix4f::Identity();
+        result(0, 3) = pos.x();
+        result(1, 3) = pos.y();
+        return result;
+    }
+
+    void Planner2D::setParameter(const std::string& s, float p)
+    {
+        parameters[s] = p;
+    }
+
+    bool Planner2D::hasParameter(const std::string& s)
+    {
+        return (parameters.find(s) != parameters.end());
+    }
+
+    float Planner2D::getParameter(const std::string& s)
+    {
+        if (!hasParameter(s))
+        {
+            std::cout << "Warning, parameter " << s << " not set, returning 0" << std::endl;
+            return 0.0f;
+        }
+        return parameters[s];
+    }
+
+} // namespace armarx::nav::algo::astar

source/Navigation/libraries/algorithms/astar/Planner2D.h

+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/SceneObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+namespace armarx::nav::algo::astar
+{
+
+    class Planner2D
+    {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        /**
+        Initialize Planner with robot and obstacles.
+        \param robot The robot
+        \param obstacles The obstacles which should be considered by the planner
+        */
+        Planner2D(VirtualRobot::RobotPtr robot,
+                  VirtualRobot::SceneObjectSetPtr obstacles = VirtualRobot::SceneObjectSetPtr());
+
+        // planners implement this method
+        virtual std::vector<Eigen::Vector2f> plan(Eigen::Vector2f start, Eigen::Vector2f goal) = 0;
+
+        /// Update obstacles
+        void setObstacles(VirtualRobot::SceneObjectSetPtr obstacles);
+
+        /// update robot
+        void setRobot(VirtualRobot::RobotPtr robot);
+
+        /// set name of RobotNode which should be used for collision detection
+        void setRobotColModel(const std::string& robotColModelName);
+
+        Eigen::Matrix4f positionToMatrix4f(Eigen::Vector2f pos);
+
+        /// set a float parameter that is identified with a string
+        void setParameter(const std::string& s, float p);
+
+        /// check if a parameter is set
+        bool hasParameter(const std::string& s);
+
+        /// get the corresponding float parameter (0 is returned when parameter string is not present)
+        float getParameter(const std::string& s);
+
+        struct point2D
+        {
+            float x;
+            float y;
+            float dirX;
+            float dirY;
+        };
+
+        virtual std::vector<point2D> getGridVisu(float xdist, float ydist, Eigen::Vector2f goal)
+        {
+            return std::vector<point2D>();
+        }
+
+    protected:
+        std::string robotColModelName;
+        //local copy of the robot's collision model that can be moved around without moving the robot
+        VirtualRobot::CollisionModelPtr robotCollisionModel;
+        VirtualRobot::RobotPtr robot;
+        VirtualRobot::SceneObjectSetPtr obstacles;
+        Eigen::Vector2f sceneBoundsMin;
+        Eigen::Vector2f sceneBoundsMax;
+
+        std::map<std::string, float> parameters;
+    };
+
+    using Planner2DPtr = boost::shared_ptr<Planner2D>;
+
+} // namespace armarx::nav::algo::astar

source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp

+#include "ChainApproximation.h"
+
+#include <iterator>
+#include <numeric>
+
+#include <Eigen/Geometry>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+namespace armarx
+{
+
+    ChainApproximation::ChainApproximation(const Points& points, const Params& params) :
+        points(points), params(params)
+    {
+        // fill indices
+        indices.resize(points.size());
+        std::iota(indices.begin(), indices.end(), 0);
+    }
+
+    ChainApproximation::ApproximationResult ChainApproximation::approximate()
+    {
+
+        int iterations = 0;
+
+        const auto maxIterConditionReached = [&]()
+        {
+            // inactive?
+            if (params.maxIterations <= 0)
+            {
+                return false;
+            }
+
+            return iterations >= params.maxIterations;
+        };
+
+        while (true)
+        {
+            if (maxIterConditionReached())
+            {
+                return ApproximationResult
+                {
+                    .condition  = ApproximationResult::TerminationCondition::IterationLimit,
+                    .iterations = iterations};
+            }
+
+            if (not approximateStep())
+            {
+                return ApproximationResult
+                {
+                    .condition       = ApproximationResult::TerminationCondition::Converged,
+                    .iterations      = iterations,
+                    .reductionFactor = 1.F - static_cast<float>(indices.size()) /
+                    static_cast<float>(points.size())};
+            }
+
+            iterations++;
+        }
+    }
+
+    ChainApproximation::Triplets ChainApproximation::getTriplets() const
+    {
+        const int nIndices = static_cast<int>(indices.size());
+
+        if (nIndices < 3)
+        {
+            return {};
+        }
+
+        Triplets triplets;
+        triplets.reserve(indices.size());
+
+        // Here, we iterate over all elements under consideration.
+        // The aim is to create a view - a sliding window - over the
+        // indices. i will always point to the centered element.
+
+        // the first element
+        triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
+
+        // intermediate elements
+        for (int i = 1; i < (nIndices - 1); i++)
+        {
+            triplets.emplace_back(indices.at(i - 1), indices.at(i), indices.at(i + 1));
+        }
+
+        // the last element
+        triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
+
+        return triplets;
+    }
+
+    std::vector<float>
+    ChainApproximation::computeDistances(const ChainApproximation::Triplets& triplets)
+    {
+        std::vector<float> distances;
+        distances.reserve(triplets.size());
+
+        std::transform(triplets.begin(),
+                       triplets.end(),
+                       std::back_inserter(distances),
+                       [&](const auto & triplet)
+        {
+            return computeDistance(triplet);
+        });
+
+        return distances;
+    }
+    float ChainApproximation::computeDistance(const ChainApproximation::Triplet& triplet) const
+    {
+        using Line = Eigen::ParametrizedLine<float, 2>;
+
+        const Eigen::Vector2f& ptBefore = points.at(triplet.a);
+        const Eigen::Vector2f& ptPivot  = points.at(triplet.b);
+        const Eigen::Vector2f& ptAfter  = points.at(triplet.c);
+
+        const auto line = Line::Through(ptBefore, ptAfter);
+        return line.distance(ptPivot);
+    }
+
+    bool ChainApproximation::approximateStep()
+    {
+        const size_t nIndices = indices.size();
+        if (nIndices <= 3)
+        {
+            return false;
+        }
+
+        const Triplets triplets            = getTriplets();
+        const std::vector<float> distances = computeDistances(triplets);
+
+        ARMARX_CHECK_EQUAL(triplets.size(), distances.size());
+        const int n = static_cast<int>(triplets.size());
+
+        std::vector<int> indicesToBeRemoved;
+
+        // TODO(fabian.reister): consider boundaries
+        for (int i = 1; i < n - 1; i++)
+        {
+            const auto& distance = distances.at(i);
+
+            // check distance criterion (necessary conditio)
+            if (distance >= params.distanceTh)
+            {
+                continue;
+            }
+
+            // better remove this element than those left and right (sufficient condition)
+            if (distance <= std::min(distances.at(i - 1), distances.at(i + 1)))
+            {
+                indicesToBeRemoved.emplace_back(triplets.at(i).b);
+            }
+        }
+
+        // termination condition
+        if (indicesToBeRemoved.empty())
+        {
+            return false;
+        }
+
+        const auto isMatch = [&](const int& idx) -> bool
+        {
+            return std::find(indicesToBeRemoved.begin(), indicesToBeRemoved.end(), idx) !=
+            indicesToBeRemoved.end();
+        };
+
+        indices.erase(std::remove_if(indices.begin(), indices.end(), isMatch), indices.end());
+
+        return true;
+    }
+    ChainApproximation::Points ChainApproximation::approximatedChain() const
+    {
+        Points extractedPoints;
+        extractedPoints.reserve(indices.size());
+
+        std::transform(indices.begin(),
+                       indices.end(),
+                       std::back_inserter(extractedPoints),
+                       [&](const auto & idx)
+        {
+            return points.at(idx);
+        });
+
+        return extractedPoints;
+    }
+
+    std::ostream& detail::operator<<(std::ostream& str, const ApproximationResult& res)
+    {
+        using TerminationCondition = ApproximationResult::TerminationCondition;
+
+        const std::string condStr = [&res]() -> std::string
+        {
+            std::string repr;
+
+            switch (res.condition)
+            {
+                case TerminationCondition::Converged:
+                    repr = "Converged";
+                    break;
+                case TerminationCondition::IterationLimit:
+                    repr = "IterationLimit";
+                    break;
+            }
+            return repr;
+        }();
+
+        str << "ApproximationResult: ["
+            << "condition: " << condStr << " | "
+            << "iterations: " << res.iterations << " | "
+            << "reduction: " << res.reductionFactor * 100 << "%]";
+
+        return str;
+    }
+} // namespace armarx

source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx
+{
+
+    namespace detail
+    {
+        struct ChainApproximationParams
+        {
+            float distanceTh; // [mm]
+
+            int maxIterations{-1};
+        };
+
+
+        struct ApproximationResult
+        {
+            enum class TerminationCondition
+            {
+                Converged,
+                IterationLimit
+            };
+            TerminationCondition condition;
+
+            int iterations;
+
+            float reductionFactor{0.F};
+        };
+
+        std::ostream& operator<<(std::ostream& str, const ApproximationResult& res);
+    } // namespace detail
+
+    class ChainApproximation
+    {
+    public:
+        using Point  = Eigen::Vector2f;
+        using Points = std::vector<Point>;
+
+        using Params = detail::ChainApproximationParams;
+        using ApproximationResult = detail::ApproximationResult;
+
+        ChainApproximation(const Points& points, const Params& params);
+
+        ApproximationResult approximate();
+
+        [[nodiscard]] Points approximatedChain() const;
+
+    private:
+        struct Triplet
+        {
+            Triplet(const int& a, const int& b, const int& c) : a(a), b(b), c(c) {}
+
+            int a;
+            int b;
+            int c;
+        };
+
+        using Triplets = std::vector<Triplet>;
+
+        Triplets getTriplets() const;
+
+        std::vector<float> computeDistances(const Triplets& triplets);
+        float computeDistance(const Triplet& triplet) const;
+
+        bool approximateStep();
+
+        Points points;
+
+        std::vector<int> indices;
+
+        const Params params;
+    };
+
+} // namespace armarx

source/Navigation/libraries/client/CMakeLists.txt

-set(LIB_NAME       client)
+set(LIB_NAME       ${PROJECT_NAME}Client)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -10,11 +10,11 @@ armarx_add_library(
         # RobotAPI
         aron
         # Navigation
-        Navigation::core
-        Navigation::global_planning
-        Navigation::local_planning
-        Navigation::trajectory_control
-        Navigation::safety_control
+        Navigation::Core
+        Navigation::GlobalPlanning
+        Navigation::LocalPlanning
+        Navigation::TrajectoryControl
+        Navigation::SafetyControl
     SOURCES  
         # ./Navigator.cpp
         ./NavigationStackConfig.cpp
@@ -25,7 +25,7 @@ armarx_add_library(
         ./NavigatorComponentPlugin.h
 )
 
-add_library(Navigation::client ALIAS client)
+add_library(Navigation::Client ALIAS ${PROJECT_NAME}Client)
 
 #find_package(MyLib QUIET)
 #armarx_build_if(MyLib_FOUND "MyLib not available")

source/Navigation/libraries/conversions/CMakeLists.txt

+set(LIB_NAME       ${PROJECT_NAME}Conversions)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+    LIBS     
+        ArmarXCoreInterfaces
+        ArmarXCore
+    SOURCES  
+        ./eigen.cpp
+    HEADERS  
+        ./eigen.h
+)
+
+add_library(Navigation::Conversions ALIAS ${PROJECT_NAME}Conversions)
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    target_include_directories(conversions PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)

source/Navigation/libraries/conversions/eigen.cpp

+#include "eigen.h"
+
+#include <algorithm>
+
+namespace armarx::nav::conv
+{
+
+    std::vector<Eigen::Vector3f> to3D(const std::vector<Eigen::Vector2f>& v)
+
+    {
+        std::vector<Eigen::Vector3f> v3;
+        v3.reserve(v.size());
+
+        std::transform(
+            v.begin(),
+            v.end(),
+            std::back_inserter(v3),
+            static_cast<Eigen::Vector3f (*)(const Eigen::Vector2f&)>(&to3D));
+
+        return v3;
+    }
+
+}  // namespace armarx::nav::conv

source/Navigation/libraries/conversions/eigen.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace armarx::nav::conv
+{
+
+    inline Eigen::Vector2f to2D(const Eigen::Vector3f& v2)
+    {
+        return Eigen::Vector2f{v2.x(), v2.y()};
+    }
+
+    inline Eigen::Vector3f to3D(const Eigen::Vector2f& v2)
+    {
+        return Eigen::Vector3f{v2.x(), v2.y(), 0.F};
+    }
+
+    inline Eigen::Affine3f to3D(const Eigen::Affine2f& p2)
+    {
+        Eigen::Affine3f pose            = Eigen::Affine3f::Identity();
+        pose.linear().block<2, 2>(0, 0) = p2.linear();
+        pose.translation()              = to3D(p2.translation());
+
+        return pose;
+    }
+
+    std::vector<Eigen::Vector3f> to3D(const std::vector<Eigen::Vector2f>& v);
+
+} // namespace armarx::conversions

source/Navigation/libraries/conversions/test/CMakeLists.txt

+# Libs required for the tests
+set(
+    LIBS
+    ${LIBS}
+    ArmarXCore
+    Navigation::Conversions
+)
+
+armarx_add_test(
+    conversionsTest
+    conversionsTest.cpp
+    "${LIBS}"
+)

source/Navigation/libraries/conversions/test/conversionsTest.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    Navigation::ArmarXObjects::conversions
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::conversions
+
+#define ARMARX_BOOST_TEST
+
+#include <Navigation/Test.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+    BOOST_CHECK_EQUAL(true, true);
+}

source/Navigation/libraries/core/CMakeLists.txt

-set(LIB_NAME core)
+set(LIB_NAME ${PROJECT_NAME}Core)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -28,9 +28,9 @@ armarx_add_library(
 )
 
 add_library(
-    Navigation::core
+    Navigation::Core
     ALIAS
-    core
+    ${PROJECT_NAME}Core
 )
 
 # find_package(MyLib QUIET) armarx_build_if(MyLib_FOUND "MyLib not available")

source/Navigation/libraries/factories/CMakeLists.txt

-set(LIB_NAME factories)
+set(LIB_NAME ${PROJECT_NAME}Factories)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -7,11 +7,11 @@ armarx_add_library(
     LIBS
         ArmarXCoreInterfaces
         ArmarXCore
-        Navigation::core
-        Navigation::global_planning
-        Navigation::local_planning
-        Navigation::trajectory_control
-        Navigation::safety_control
+        Navigation::Core
+        Navigation::GlobalPlanning
+        Navigation::LocalPlanning
+        Navigation::TrajectoryControl
+        Navigation::SafetyControl
     SOURCES
         ./GlobalPlannerFactory.cpp
         ./LocalPlannerFactory.cpp
@@ -27,9 +27,9 @@ armarx_add_library(
 )
 
 add_library(
-    Navigation::factories
+    Navigation::Factories
     ALIAS
-    factories
+    ${PROJECT_NAME}Factories
 )
 
 # find_package(MyLib QUIET) armarx_build_if(MyLib_FOUND "MyLib not available")