diff --git a/source/Navigation/components/Navigator/CMakeLists.txt b/source/Navigation/components/Navigator/CMakeLists.txt
index fd803d7b2d32d908b742a00b184babe3d2f58ef4..b2281158c8b6ce0b295606b8ff1c00f8d4ad3010 100644
--- a/source/Navigation/components/Navigator/CMakeLists.txt
+++ b/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
diff --git a/source/Navigation/components/Navigator/test/CMakeLists.txt b/source/Navigation/components/Navigator/test/CMakeLists.txt
index 3751236c38c88c95cfae0db5f115c49014f31f02..56f827195a0848916206cee0b36aa1f0130b2da5 100644
--- a/source/Navigation/components/Navigator/test/CMakeLists.txt
+++ b/source/Navigation/components/Navigator/test/CMakeLists.txt
@@ -1,5 +1,13 @@
-
 # 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}"
+)
diff --git a/source/Navigation/libraries/CMakeLists.txt b/source/Navigation/libraries/CMakeLists.txt
index 308fb0d579ac797b75e8011cad781828776dac24..9174c98909259fc9f70ccd98bb720949aabf5164 100644
--- a/source/Navigation/libraries/CMakeLists.txt
+++ b/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
diff --git a/source/Navigation/libraries/algorithms/CMakeLists.txt b/source/Navigation/libraries/algorithms/CMakeLists.txt
index ca05cc52c769838f1b5cf662117a866fc6a2196d..0e49912697f4187c16f01abd328417279000c718 100644
--- a/source/Navigation/libraries/algorithms/CMakeLists.txt
+++ b/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
 )
diff --git a/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp b/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5439b27c8601d0d414bce1290f1c5b225aea62a3
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
@@ -0,0 +1,273 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/astar/AStarPlanner.h b/source/Navigation/libraries/algorithms/astar/AStarPlanner.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab21d6b923d23a86e71022bef5c40e21bffa789c
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/astar/AStarPlanner.h
@@ -0,0 +1,44 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/astar/Node.cpp b/source/Navigation/libraries/algorithms/astar/Node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf04818259bc80e4adcd697fb7547d7a2c13cfb3
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/astar/Node.cpp
@@ -0,0 +1,24 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/astar/Node.h b/source/Navigation/libraries/algorithms/astar/Node.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba9a8aeff819e835a6b40f40b2644c3a00ad63e2
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/astar/Node.h
@@ -0,0 +1,37 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/astar/Planner2D.cpp b/source/Navigation/libraries/algorithms/astar/Planner2D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d49193ff87e9e7807866867109833e31fe73afa
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/astar/Planner2D.cpp
@@ -0,0 +1,73 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/astar/Planner2D.h b/source/Navigation/libraries/algorithms/astar/Planner2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb00a3243e8356e50ebdade744d241aa8b5ea775
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/astar/Planner2D.h
@@ -0,0 +1,76 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..db730742119908c3a71ad4a629eafd5579a6efbe
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.cpp
@@ -0,0 +1,214 @@
+#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
diff --git a/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7b5e6cdaeb46a8909f4f572863e08c92751a649
--- /dev/null
+++ b/source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h
@@ -0,0 +1,97 @@
+/*
+ * 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
diff --git a/source/Navigation/libraries/client/CMakeLists.txt b/source/Navigation/libraries/client/CMakeLists.txt
index 62414c3e6d54932347e1f7d454f413be1c3efdea..2192ec9917664631f51b7f9b7b997cc1259185c2 100644
--- a/source/Navigation/libraries/client/CMakeLists.txt
+++ b/source/Navigation/libraries/client/CMakeLists.txt
@@ -1,4 +1,4 @@
-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")
diff --git a/source/Navigation/libraries/conversions/CMakeLists.txt b/source/Navigation/libraries/conversions/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fdefb9c1c7e987e0cd5fc8f9ac604fdb12b4b9bc
--- /dev/null
+++ b/source/Navigation/libraries/conversions/CMakeLists.txt
@@ -0,0 +1,27 @@
+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)
diff --git a/source/Navigation/libraries/conversions/eigen.cpp b/source/Navigation/libraries/conversions/eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8998e0a8da7cc8c291ee28eac0f76b3194f862f8
--- /dev/null
+++ b/source/Navigation/libraries/conversions/eigen.cpp
@@ -0,0 +1,23 @@
+#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
diff --git a/source/Navigation/libraries/conversions/eigen.h b/source/Navigation/libraries/conversions/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..859aa3ccbc7352e420ca34786dc65a3657ba39e8
--- /dev/null
+++ b/source/Navigation/libraries/conversions/eigen.h
@@ -0,0 +1,53 @@
+/*
+ * 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
diff --git a/source/Navigation/libraries/conversions/test/CMakeLists.txt b/source/Navigation/libraries/conversions/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3e29bbb4ad054f362ce447bed039d26945b720d9
--- /dev/null
+++ b/source/Navigation/libraries/conversions/test/CMakeLists.txt
@@ -0,0 +1,13 @@
+# Libs required for the tests
+set(
+    LIBS
+    ${LIBS}
+    ArmarXCore
+    Navigation::Conversions
+)
+
+armarx_add_test(
+    conversionsTest
+    conversionsTest.cpp
+    "${LIBS}"
+)
diff --git a/source/Navigation/libraries/conversions/test/conversionsTest.cpp b/source/Navigation/libraries/conversions/test/conversionsTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a73a9f8408f93254a2ed8b1b1d143414f43aa4a0
--- /dev/null
+++ b/source/Navigation/libraries/conversions/test/conversionsTest.cpp
@@ -0,0 +1,35 @@
+/*
+ * 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);
+}
diff --git a/source/Navigation/libraries/core/CMakeLists.txt b/source/Navigation/libraries/core/CMakeLists.txt
index 44373e94c8c8d2c050bb9caa90bce821daf8ed71..ae8c4b147cec9941aa9eb5eb61d988dab827f658 100644
--- a/source/Navigation/libraries/core/CMakeLists.txt
+++ b/source/Navigation/libraries/core/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME core)
+set(LIB_NAME ${PROJECT_NAME}Core)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -17,6 +17,7 @@ armarx_add_library(
     SOURCES
         StaticScene.cpp
         Trajectory.cpp
+        types.cpp
     HEADERS
         types.h
         MemoryReferencedElement.h
@@ -27,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")
diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp
index 3f463a004c469746ed07df7f8424bcdbc55867b3..469c511ffcd762f27bd0d91d6a4efe7cdb2e165c 100644
--- a/source/Navigation/libraries/core/Trajectory.cpp
+++ b/source/Navigation/libraries/core/Trajectory.cpp
@@ -9,7 +9,7 @@
 
 namespace armarx::nav::core
 {
-    TrajectoryPoint
+    Projection
     Trajectory::getProjection(const Pose& pose,
                               const VelocityInterpolations& velocityInterpolation) const
     {
@@ -20,7 +20,7 @@ namespace armarx::nav::core
 
         float distance = std::numeric_limits<float>::max();
 
-        TrajectoryPoint bestProj;
+        Projection bestProj;
 
         for (size_t i = 0; i < points.size() - 1; i++)
         {
@@ -45,17 +45,20 @@ namespace armarx::nav::core
 
                 math::LinearInterpolatedPose ip(
                     wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
-                bestProj.waypoint.pose = ip.Get(t);
+
+                bestProj.wayPointBefore = wpBefore;
+                bestProj.wayPointAfter = wpAfter;
+                bestProj.projection.waypoint.pose = ip.Get(t);
 
                 switch (velocityInterpolation)
                 {
                     case VelocityInterpolations::LinearInterpolation:
-                        bestProj.twist.linear = (wpAfter.twist.linear - wpBefore.twist.linear) * t;
-                        bestProj.twist.angular =
-                            (wpAfter.twist.angular - wpBefore.twist.angular) * t;
+                        bestProj.projection.velocity = (wpAfter.velocity - wpBefore.velocity) * t;
+                        bestProj.projection.velocity =
+                            (wpAfter.velocity - wpBefore.velocity) * t;
                         break;
                     case VelocityInterpolations::LastWaypoint:
-                        bestProj.twist = wpBefore.twist;
+                        bestProj.projection.velocity = wpBefore.velocity;
                 }
 
                 distance = currentDistance;
diff --git a/source/Navigation/libraries/core/Trajectory.h b/source/Navigation/libraries/core/Trajectory.h
index 5f2a1c07b2cee954affcbe137b9296065702cd13..64beb80492072a34f8e931ab9c7ff3c2f792cf48 100644
--- a/source/Navigation/libraries/core/Trajectory.h
+++ b/source/Navigation/libraries/core/Trajectory.h
@@ -29,7 +29,15 @@ namespace armarx::nav::core
     struct TrajectoryPoint
     {
         Waypoint waypoint;
-        Twist twist;
+        float velocity;
+    };
+
+    struct Projection
+    {
+        TrajectoryPoint projection;
+
+        TrajectoryPoint wayPointBefore;
+        TrajectoryPoint wayPointAfter;
     };
 
     enum class VelocityInterpolations
@@ -45,7 +53,8 @@ namespace armarx::nav::core
         {
         }
 
-        TrajectoryPoint getProjection(const Pose& pose, const VelocityInterpolations& velocityInterpolation) const;
+        Projection getProjection(const Pose& pose,
+                                 const VelocityInterpolations& velocityInterpolation) const;
 
     private:
         std::vector<TrajectoryPoint> points;
diff --git a/source/Navigation/libraries/core/types.cpp b/source/Navigation/libraries/core/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a8e5e149b4caed0957cd6129ec0f14747c3f0cfe
--- /dev/null
+++ b/source/Navigation/libraries/core/types.cpp
@@ -0,0 +1,10 @@
+#include "types.h"
+
+namespace armarx::nav::core
+{
+    core::Pose Twist::poseDiff(const float dt) const
+    {
+        return core::Pose(Eigen::Translation3f(linear * dt)) *
+               core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
+    }
+} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/types.h b/source/Navigation/libraries/core/types.h
index b00682034dd561e7edce9141fb8303c04aedf523..6b820ea7af87deb15f4e47e6e2decacc5ab715dd 100644
--- a/source/Navigation/libraries/core/types.h
+++ b/source/Navigation/libraries/core/types.h
@@ -53,6 +53,8 @@ namespace armarx::nav::core
     {
         Eigen::Vector3f linear;
         Eigen::Vector3f angular;
+
+        core::Pose poseDiff(float dt) const;
     };
 
     struct Waypoint
@@ -60,8 +62,6 @@ namespace armarx::nav::core
         Pose pose;
     };
 
-
-
     struct Scene
     {
         std::optional<core::StaticScene> staticScene = std::nullopt;
diff --git a/source/Navigation/libraries/factories/CMakeLists.txt b/source/Navigation/libraries/factories/CMakeLists.txt
index dbd781fcec5e89dcb5fc7cd442afee3c101f86bb..064c7e4d9caa8434abd07bd1bf37a65c1cab481d 100644
--- a/source/Navigation/libraries/factories/CMakeLists.txt
+++ b/source/Navigation/libraries/factories/CMakeLists.txt
@@ -1,4 +1,4 @@
-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")
diff --git a/source/Navigation/libraries/factories/GlobalPlannerFactory.h b/source/Navigation/libraries/factories/GlobalPlannerFactory.h
index 7213befc78c3d572232e8d53b5831ddd83c5948f..17e9eded8edfeadd038984a8f0a692b3cb034bd0 100644
--- a/source/Navigation/libraries/factories/GlobalPlannerFactory.h
+++ b/source/Navigation/libraries/factories/GlobalPlannerFactory.h
@@ -25,6 +25,10 @@
 
 namespace armarx::nav::fac
 {
+    /**
+    * @brief Factory to create a glob_plan::GlobalPlanner
+    *
+    */
     class GlobalPlannerFactory
     {
     public:
diff --git a/source/Navigation/libraries/factories/LocalPlannerFactory.h b/source/Navigation/libraries/factories/LocalPlannerFactory.h
index 57724fd878f2a468805efb63985f03626a97f9b0..f3d9c5a2bfbe188ffafb85fed2f655f086615097 100644
--- a/source/Navigation/libraries/factories/LocalPlannerFactory.h
+++ b/source/Navigation/libraries/factories/LocalPlannerFactory.h
@@ -27,6 +27,10 @@
 namespace armarx::nav::fac
 {
 
+    /**
+     * @brief Factory to create a loc_plan::LocalPlanner
+     *
+     */
     class LocalPlannerFactory
     {
     public:
diff --git a/source/Navigation/libraries/factories/NavigationStackFactory.h b/source/Navigation/libraries/factories/NavigationStackFactory.h
index f25b73c5753af575c29f5242ce07116f0118f2ce..5688efe571edee4214bfa58b5489ac442ff24855 100644
--- a/source/Navigation/libraries/factories/NavigationStackFactory.h
+++ b/source/Navigation/libraries/factories/NavigationStackFactory.h
@@ -24,7 +24,10 @@
 #include "Navigation/libraries/server/NavigationStack.h"
 namespace armarx::nav::fac
 {
-
+    /**
+     * @brief Factory to create a server::NavigationStack
+     *
+     */
     class NavigationStackFactory
     {
     public:
diff --git a/source/Navigation/libraries/factories/SafetyControllerFactory.h b/source/Navigation/libraries/factories/SafetyControllerFactory.h
index 6b303c2d3b5771f1d9b7ef0a3e9fab348a5f8e86..0cb7c2ad0616210647d4692d5bdc6da4f2dc55c5 100644
--- a/source/Navigation/libraries/factories/SafetyControllerFactory.h
+++ b/source/Navigation/libraries/factories/SafetyControllerFactory.h
@@ -28,6 +28,10 @@
 namespace armarx::nav::fac
 {
 
+    /**
+        * @brief Factory to create a safe_ctrl::SafetyController
+        *
+        */
     class SafetyControllerFactory
     {
     public:
diff --git a/source/Navigation/libraries/factories/TrajectoryControllerFactory.h b/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
index 361f104afb2cc9b217ea884dd52a21603d385be6..e2e95bd0b74d5fb7d5c27ecc459e8d7c046000a0 100644
--- a/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
+++ b/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
@@ -29,6 +29,10 @@
 namespace armarx::nav::fac
 {
 
+    /**
+        * @brief Factory to create a traj_ctrl::TrajectoryController
+        *
+        */
     class TrajectoryControllerFactory
     {
     public:
diff --git a/source/Navigation/libraries/global_planning/AStar.cpp b/source/Navigation/libraries/global_planning/AStar.cpp
index ba64f1b318951b61c6f3d582279c6b3153abecb8..cd2502b38e4cc96b7e38d649b6174f082d9a5b3c 100644
--- a/source/Navigation/libraries/global_planning/AStar.cpp
+++ b/source/Navigation/libraries/global_planning/AStar.cpp
@@ -1,5 +1,7 @@
 #include "AStar.h"
 
+#include "Navigation/libraries/algorithms/astar/AStarPlanner.h"
+
 namespace armarx::nav::glob_plan
 {
 
@@ -29,8 +31,14 @@ namespace armarx::nav::glob_plan
 
     core::TrajectoryPtr AStar::plan(const core::Pose& goal)
     {
-        // TODO implement
+        algo::astar::AStarPlanner planner(scene.robot, scene.staticScene->objects, 100.f);
+        // planner.setRobotColModel("Platform-colmodel");
+
+        // TODO!
+        // planner.setRobotCollisionModel(robotCollisionModel());
 
+        // TODO
         return {};
+
     }
 } // namespace armarx::nav::glob_plan
diff --git a/source/Navigation/libraries/global_planning/AStar.h b/source/Navigation/libraries/global_planning/AStar.h
index d08f9748c273b0eb891fe4bfeb9284d5b18e40a9..f56a95de393a4a3ce70e35dcfd0b63c3120b2173 100644
--- a/source/Navigation/libraries/global_planning/AStar.h
+++ b/source/Navigation/libraries/global_planning/AStar.h
@@ -27,6 +27,10 @@
 namespace armarx::nav::glob_plan
 {
 
+    /**
+     * @brief Parameters for AStar
+     *
+     */
     struct AStarParams : public GlobalPlannerParams
     {
         Algorithms algorithm() const override;
diff --git a/source/Navigation/libraries/global_planning/CMakeLists.txt b/source/Navigation/libraries/global_planning/CMakeLists.txt
index 6b1b3c3cbac46984213a509681b4bbf3b12d265b..4d6be9f26b4b169ff4951e9754211661eeda74f3 100644
--- a/source/Navigation/libraries/global_planning/CMakeLists.txt
+++ b/source/Navigation/libraries/global_planning/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME global_planning)
+set(LIB_NAME ${PROJECT_NAME}GlobalPlanning)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -7,21 +7,34 @@ armarx_add_library(
     LIBS
         ArmarXCoreInterfaces
         ArmarXCore
-        Navigation::core
+        aroncommon
+        Navigation::Core
+        Navigation::Algorithms
     SOURCES
         ./GlobalPlanner.cpp
         ./AStar.cpp
         ./Point2Point.cpp
+        ./aron_conversions.cpp
     HEADERS
         ./GlobalPlanner.h
         ./AStar.h
         ./Point2Point.h
+        ./aron_conversions.h
 )
 
 add_library(
-    Navigation::global_planning
+    Navigation::GlobalPlanning
     ALIAS
-    global_planning
+    ${PROJECT_NAME}GlobalPlanning
+)
+
+armarx_enable_aron_file_generation_for_target(
+    TARGET_NAME
+        "${LIB_NAME}"
+    ARON_FILES
+        aron/GlobalPlannerParams.xml
+        aron/Point2PointParams.xml
+        # aron/AStarParams.xml
 )
 
 # find_package(MyLib QUIET) armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/Navigation/libraries/global_planning/GlobalPlanner.cpp b/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
index 8f68dec64b2cf162781a266bc1984480d4fac67c..ac00c7c0a37ae28e16b3bf88e52b24fffd602247 100644
--- a/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
+++ b/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
@@ -4,7 +4,7 @@
 
 namespace armarx::nav::glob_plan
 {
-    GlobalPlanner::GlobalPlanner(const core::Scene& context) : context(context)
+    GlobalPlanner::GlobalPlanner(const core::Scene& context) : scene(context)
     {
     }
 
diff --git a/source/Navigation/libraries/global_planning/GlobalPlanner.h b/source/Navigation/libraries/global_planning/GlobalPlanner.h
index b03aec235bc5305374c47857e1775b2e9f417eef..dca8734fec672b5b2b99144ff9adf395ee3e7f23 100644
--- a/source/Navigation/libraries/global_planning/GlobalPlanner.h
+++ b/source/Navigation/libraries/global_planning/GlobalPlanner.h
@@ -34,6 +34,10 @@
 namespace armarx::nav::glob_plan
 {
 
+    /**
+         * @brief Parameters for GlobalPlanner
+         *
+         */
     struct GlobalPlannerParams
     {
         virtual Algorithms algorithm() const = 0;
@@ -43,13 +47,13 @@ namespace armarx::nav::glob_plan
     class GlobalPlanner
     {
     public:
-        GlobalPlanner(const core::Scene& context);
+        GlobalPlanner(const core::Scene& scene);
         virtual ~GlobalPlanner() = default;
 
         virtual core::TrajectoryPtr plan(const core::Pose& goal) = 0;
 
     protected:
-        const core::Scene context;
+        const core::Scene scene;
 
     private:
     };
diff --git a/source/Navigation/libraries/global_planning/Point2Point.cpp b/source/Navigation/libraries/global_planning/Point2Point.cpp
index 0fcfdc513e87ab2afc8b76f93c5b3ca639d2013f..7db0e0544227a6b0135311ce487e974f60e44e39 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.cpp
+++ b/source/Navigation/libraries/global_planning/Point2Point.cpp
@@ -12,6 +12,8 @@
 #include "Navigation/libraries/core/types.h"
 #include "Navigation/libraries/global_planning/GlobalPlanner.h"
 #include "Navigation/libraries/global_planning/core.h"
+#include <Navigation/libraries/global_planning/aron/Point2PointParams.aron.generated.h>
+#include <Navigation/libraries/global_planning/aron_conversions.h>
 
 namespace armarx::nav::glob_plan
 {
@@ -25,12 +27,23 @@ namespace armarx::nav::glob_plan
 
     aron::datanavigator::DictNavigatorPtr Point2PointParams::toAron() const
     {
-        return nullptr; // TODO implement
+        arondto::Point2PointParams dto;
+
+        Point2PointParams bo;
+        aron_conv::toAron(dto, bo);
+
+        return dto.toAron();
     }
 
     Point2PointParams Point2PointParams::FromAron(const aron::datanavigator::DictNavigatorPtr& dict)
     {
-        return Point2PointParams(); // TODO implement
+        arondto::Point2PointParams dto;
+        dto.fromAron(dict);
+
+        Point2PointParams bo;
+        aron_conv::fromAron(dto, bo);
+
+        return bo;
     }
 
     // Point2Point
@@ -48,14 +61,14 @@ namespace armarx::nav::glob_plan
         {
             trajectory.push_back(core::TrajectoryPoint
             {
-                .waypoint = core::Waypoint{.pose = Eigen::Affine3f(context.robot->getGlobalPose())},
-                .twist = {.linear = Eigen::Vector3f::Zero(), .angular = Eigen::Vector3f::Zero()}});
+                .waypoint = core::Waypoint{.pose = core::Pose(scene.robot->getGlobalPose())},
+                .velocity = params.velocity});
         }
 
         trajectory.push_back(core::TrajectoryPoint
         {
             .waypoint = core::Waypoint{.pose = goal},
-            .twist = {.linear = Eigen::Vector3f::Zero(), .angular = Eigen::Vector3f::Zero()}});
+            .velocity = params.velocity});
 
         return std::make_shared<core::Trajectory>(trajectory);
     }
diff --git a/source/Navigation/libraries/global_planning/Point2Point.h b/source/Navigation/libraries/global_planning/Point2Point.h
index 112f81bf8a8c9178c1c500b8038139fba00d784b..62ccf935b1f8e6def7c3b5a115f0c9f4b1b74e69 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.h
+++ b/source/Navigation/libraries/global_planning/Point2Point.h
@@ -27,6 +27,10 @@
 namespace armarx::nav::glob_plan
 {
 
+    /**
+         * @brief Parameters for Point2Point
+         *
+         */
     struct Point2PointParams : public GlobalPlannerParams
     {
         bool includeStartPose{false};
diff --git a/source/Navigation/libraries/global_planning/aron/GlobalPlannerParams.xml b/source/Navigation/libraries/global_planning/aron/GlobalPlannerParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0e908374fbc7e261b1a8540e2424df1de9b77b59
--- /dev/null
+++ b/source/Navigation/libraries/global_planning/aron/GlobalPlannerParams.xml
@@ -0,0 +1,26 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+    </CodeIncludes>
+    <AronIncludes>
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <Object name='armarx::nav::glob_plan::arondto::GlobalPlannerParams'>
+            <!-- <ObjectChild key='resolution'>
+                <float />
+            </ObjectChild>
+            <ObjectChild key='frame'>
+                <string />
+            </ObjectChild>
+            <ObjectChild key='pose'>
+                <Pose />
+            </ObjectChild> -->
+            
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
diff --git a/source/Navigation/libraries/global_planning/aron/Point2PointParams.xml b/source/Navigation/libraries/global_planning/aron/Point2PointParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..84e2f0fdcd3075f62dc541462bdd77b9ac85403d
--- /dev/null
+++ b/source/Navigation/libraries/global_planning/aron/Point2PointParams.xml
@@ -0,0 +1,25 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+        <Include include="<Navigation/libraries/global_planning/aron/GlobalPlannerParams.aron.generated.h>" />
+    </CodeIncludes>
+    <AronIncludes>
+        <Include include="<Navigation/libraries/global_planning/aron/GlobalPlannerParams.xml>" />
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <!-- <Object name='armarx::nav::glob_plan::arondto::Point2PointParams' extends="armarx::nav::glob_plan::arondto::GlobalPlannerParams"> -->
+        <Object name='armarx::nav::glob_plan::arondto::Point2PointParams'>
+             <ObjectChild key='includeStartPose'>
+                <bool />
+            </ObjectChild>
+            <ObjectChild key='velocity'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
diff --git a/source/Navigation/libraries/global_planning/aron_conversions.cpp b/source/Navigation/libraries/global_planning/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..36055448fb3bf3f411e8d3d09d9651f52480adbd
--- /dev/null
+++ b/source/Navigation/libraries/global_planning/aron_conversions.cpp
@@ -0,0 +1,34 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "Navigation/libraries/global_planning/GlobalPlanner.h"
+#include "Navigation/libraries/global_planning/Point2Point.h"
+#include "Navigation/libraries/global_planning/aron/GlobalPlannerParams.aron.generated.h"
+#include "Navigation/libraries/global_planning/aron/Point2PointParams.aron.generated.h"
+// #include "Navigation/libraries/global_planning/aron/AStarParams.aron.generated.h"
+
+namespace armarx::nav::glob_plan::aron_conv
+{
+    void toAron(arondto::GlobalPlannerParams& dto, const GlobalPlanner& bo)
+    {
+    }
+
+    void fromAron(const arondto::GlobalPlannerParams& dto, GlobalPlanner& bo)
+    {
+    }
+
+    void toAron(arondto::Point2PointParams& dto, const Point2PointParams& bo)
+    {
+        // fromAron(static_cast<const arondto::GlobalPlannerParams&>(dto), static_cast<const GlobalPlannerParams&>(bo));
+        aron::toAron(dto.includeStartPose, bo.includeStartPose);
+        aron::toAron(dto.velocity, bo.velocity);
+    }
+
+    void fromAron(const arondto::Point2PointParams& dto, Point2PointParams& bo)
+    {
+        // fromAron(static_cast<arondto::GlobalPlannerParams&>(dto), static_cast<GlobalPlannerParams&>(bo));
+        aron::fromAron(dto.includeStartPose, bo.includeStartPose);
+        aron::fromAron(dto.velocity, bo.velocity);
+    }
+} // namespace armarx::nav::glob_plan::aron_conv
diff --git a/source/Navigation/libraries/global_planning/aron_conversions.h b/source/Navigation/libraries/global_planning/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..64443b331c40e11cdc43696db889932cb186ed11
--- /dev/null
+++ b/source/Navigation/libraries/global_planning/aron_conversions.h
@@ -0,0 +1,22 @@
+
+namespace armarx::nav::glob_plan
+{
+    struct GlobalPlanner;
+    struct Point2PointParams;
+
+    namespace arondto
+    {
+        struct GlobalPlannerParams;
+        struct Point2PointParams;
+    } // namespace aron
+} // namespace armarx::nav::glob_plan
+
+namespace armarx::nav::glob_plan::aron_conv
+{
+    void toAron(arondto::GlobalPlannerParams& dto, const GlobalPlanner& bo);
+    void fromAron(const arondto::GlobalPlannerParams& dto, GlobalPlanner& bo);
+
+    void toAron(arondto::Point2PointParams& dto, const Point2PointParams& bo);
+    void fromAron(const arondto::Point2PointParams& dto, Point2PointParams& bo);
+
+} // namespace armarx::nav::glob_plan::aron_conv
diff --git a/source/Navigation/libraries/local_planning/CMakeLists.txt b/source/Navigation/libraries/local_planning/CMakeLists.txt
index 13951db67c2a18fb948ed92d06544b5761a0d34c..d6c3357818db4b1cc78a0040bd138eafa96f219d 100644
--- a/source/Navigation/libraries/local_planning/CMakeLists.txt
+++ b/source/Navigation/libraries/local_planning/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME       local_planning)
+set(LIB_NAME       ${PROJECT_NAME}LocalPlanning)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -7,7 +7,7 @@ armarx_add_library(
     LIBS     
         ArmarXCoreInterfaces
         ArmarXCore
-        Navigation::core
+        Navigation::Core
 
     SOURCES  
         ./LocalPlanner.cpp
@@ -17,7 +17,7 @@ armarx_add_library(
         ./TimedElasticBands.h
 )
 
-add_library(Navigation::local_planning ALIAS local_planning)
+add_library(Navigation::LocalPlanning ALIAS ${PROJECT_NAME}LocalPlanning)
 
 #find_package(MyLib QUIET)
 #armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/Navigation/libraries/safety_control/CMakeLists.txt b/source/Navigation/libraries/safety_control/CMakeLists.txt
index 774ba2ecd94d2a57012582d1e37c9bc3a2e457fb..fbdd4a39159abd5a002d4312d9041a6cc5280a03 100644
--- a/source/Navigation/libraries/safety_control/CMakeLists.txt
+++ b/source/Navigation/libraries/safety_control/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME       safety_control)
+set(LIB_NAME       ${PROJECT_NAME}SafetyControl)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -7,7 +7,7 @@ armarx_add_library(
     LIBS     
         ArmarXCoreInterfaces
         ArmarXCore
-        Navigation::core
+        Navigation::Core
 
     SOURCES  
         ./SafetyController.cpp
@@ -17,7 +17,7 @@ armarx_add_library(
         ./LaserBasedProximity.h
 )
 
-add_library(Navigation::safety_control ALIAS safety_control)
+add_library(Navigation::SafetyControl ALIAS ${PROJECT_NAME}SafetyControl)
 
 #find_package(MyLib QUIET)
 #armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/Navigation/libraries/server/CMakeLists.txt b/source/Navigation/libraries/server/CMakeLists.txt
index 6dad64d05f1ed44ea1ba45588016e3ab56194eb3..8672862e0009a9e2382cb348ce085cfcf5bab8b0 100644
--- a/source/Navigation/libraries/server/CMakeLists.txt
+++ b/source/Navigation/libraries/server/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME server)
+set(LIB_NAME ${PROJECT_NAME}Server)
 
 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  
         ./Navigator.cpp
@@ -29,7 +29,7 @@ armarx_add_library(
         ./execution/DummyExecutor.h
 )
 
-add_library(Navigation::server ALIAS server)
+add_library(Navigation::Server ALIAS ${PROJECT_NAME}Server)
 
 #find_package(MyLib QUIET)
 #armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index da9c759445f807965e78bc55e699700a1a2f5447..caa798e80b4c8b0aae25ea38f548ddc9cd7a6bb3 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -76,7 +76,7 @@ namespace armarx::nav::server
         ARMARX_CHECK_NOT_NULL(stack.globalPlanner);
 
         StackResult res;
-        res.globalTrajectory = stack.globalPlanner->plan(Eigen::Affine3f(waypoints.at(0)));
+        res.globalTrajectory = stack.globalPlanner->plan(core::Pose(waypoints.at(0)));
 
         if (stack.localPlanner)
         {
diff --git a/source/Navigation/libraries/server/execution/DummyExecutor.h b/source/Navigation/libraries/server/execution/DummyExecutor.h
index d5d075e58910c4c1dd358bbf9af8fa77765cd269..c5bf0cb99fa1e70f46f9323eef3d8df1fbaf1842 100644
--- a/source/Navigation/libraries/server/execution/DummyExecutor.h
+++ b/source/Navigation/libraries/server/execution/DummyExecutor.h
@@ -42,7 +42,7 @@ namespace armarx::nav::server
 
             //lastUpdated = now;
 
-            const Eigen::Affine3f diff; // = Eigen::Affine3f(Eigen::Translation3f(twist.linear * dt)) * Eigen::Affine3f(Eigen::AngleAxisf(dt, twist.angular)));
+            const core::Pose diff; // TODO
 
             robot->setGlobalPose(robot->getGlobalPose() * diff.matrix());
         }
diff --git a/source/Navigation/libraries/server/test/CMakeLists.txt b/source/Navigation/libraries/server/test/CMakeLists.txt
index 8369f7dacec5d18e8466d697d39c880cb7765826..6992770d42fabcdbf83a441b3a70a8dbbc098038 100644
--- a/source/Navigation/libraries/server/test/CMakeLists.txt
+++ b/source/Navigation/libraries/server/test/CMakeLists.txt
@@ -3,9 +3,9 @@ set(
     LIBS
     ${LIBS}
     ArmarXCore
-    Navigation::client
-    Navigation::factories
-    Navigation::server
+    Navigation::Client
+    Navigation::Factories
+    Navigation::Server
 )
 
 armarx_add_test(
diff --git a/source/Navigation/libraries/trajectory_control/CMakeLists.txt b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
index 2133a29081729ef2629c56753bdbf2cd4e04675d..81a695d336a8d4bbd4cdf0d5753dc1df9b520da6 100644
--- a/source/Navigation/libraries/trajectory_control/CMakeLists.txt
+++ b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
@@ -1,34 +1,48 @@
-set(LIB_NAME       trajectory_control)
+set(LIB_NAME ${PROJECT_NAME}TrajectoryControl)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
 
 armarx_add_library(
-    LIBS     
+    LIBS
         ArmarXCoreInterfaces
         ArmarXCore
         RobotAPICore
-        Navigation::core
-
-    SOURCES  
+        Navigation::Core
+    SOURCES
         ./TrajectoryController.cpp
         ./TrajectoryFollowingController.cpp
         ./WaypointController.cpp
-    HEADERS  
+        ./aron_conversions.cpp
+    HEADERS
         ./TrajectoryController.h
         ./TrajectoryFollowingController.h
         ./WaypointController.h
+        ./aron_conversions.h
+)
+
+add_library(
+    Navigation::TrajectoryControl
+    ALIAS
+    ${PROJECT_NAME}TrajectoryControl
 )
 
-add_library(Navigation::trajectory_control ALIAS trajectory_control)
+armarx_enable_aron_file_generation_for_target(
+    TARGET_NAME
+        "${LIB_NAME}"
+    ARON_FILES
+        aron/PIDParams.xml
+        aron/TrajectoryControllerParams.xml
+        aron/TrajectoryFollowingControllerParams.xml
+        aron/WaypointControllerParams.xml
+        # aron/AStarParams.xml
+)
 
-#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(trajectory_control PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
+# 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(trajectory_control PUBLIC ${MyLib_INCLUDE_DIRS})
+# endif()
 
 # add unit tests
 add_subdirectory(test)
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index eda53b12037a83f4440bb80219d7016e05795ecc..1028b28d0bb3d55cb2af16ac1775a1cbd0239852 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -13,6 +13,8 @@
 #include "Navigation/libraries/core/Trajectory.h"
 #include "Navigation/libraries/core/types.h"
 #include "Navigation/libraries/trajectory_control/TrajectoryController.h"
+#include "Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h"
+#include "Navigation/libraries/trajectory_control/aron_conversions.h"
 #include "Navigation/libraries/trajectory_control/core.h"
 
 namespace armarx::nav::traj_ctrl
@@ -26,13 +28,24 @@ namespace armarx::nav::traj_ctrl
 
     aron::datanavigator::DictNavigatorPtr TrajectoryFollowingControllerParams::toAron() const
     {
-        return nullptr; // TODO implement
+        arondto::TrajectoryFollowingControllerParams dto;
+
+        TrajectoryFollowingControllerParams bo;
+        aron_conv::toAron(dto, bo);
+
+        return dto.toAron();
     }
 
     TrajectoryFollowingControllerParams
     TrajectoryFollowingControllerParams::FromAron(const aron::datanavigator::DictNavigatorPtr& dict)
     {
-        return TrajectoryFollowingControllerParams(); // TODO implement
+        arondto::TrajectoryFollowingControllerParams dto;
+        dto.fromAron(dict);
+
+        TrajectoryFollowingControllerParams bo;
+        aron_conv::fromAron(dto, bo);
+
+        return bo;
     }
 
     TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
@@ -89,9 +102,10 @@ namespace armarx::nav::traj_ctrl
 
         const core::Pose currentPose(context.robot->getGlobalPose());
 
-        const auto targetPose = trajectory->getProjection(currentPose, core::VelocityInterpolations::LastWaypoint);
+        const auto projectedPose =
+            trajectory->getProjection(currentPose, core::VelocityInterpolations::LastWaypoint);
 
-        ARMARX_INFO << targetPose.waypoint.pose.translation();
+        ARMARX_INFO << projectedPose.projection.waypoint.pose.translation();
 
         // const auto asXYZRPY = [](const core::Pose & pose)
         // {
@@ -101,21 +115,27 @@ namespace armarx::nav::traj_ctrl
         //     return p;
         // };
 
-        using simox::math::mat4f_to_rpy;
         using simox::math::mat4f_to_pos;
+        using simox::math::mat4f_to_rpy;
 
-        pidPos.update(mat4f_to_pos(currentPose.matrix()), mat4f_to_pos(targetPose.waypoint.pose.matrix()));
-        pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(targetPose.waypoint.pose.matrix()));
+        pidPos.update(mat4f_to_pos(currentPose.matrix()),
+                      mat4f_to_pos(projectedPose.projection.waypoint.pose.matrix()));
+        pidOri.update(mat4f_to_rpy(currentPose.matrix()),
+                      mat4f_to_rpy(projectedPose.projection.waypoint.pose.matrix()));
 
-        ARMARX_INFO << "Feed forward velocity " << targetPose.twist.linear;
+        const Eigen::Vector3f feedforwardVelocity =
+            (projectedPose.wayPointAfter.waypoint.pose.translation() -
+             projectedPose.wayPointBefore.waypoint.pose.translation())
+            .normalized() *
+            projectedPose.projection.velocity;
+
+        ARMARX_INFO << "Feed forward velocity " << feedforwardVelocity;
         ARMARX_INFO << "Control value " << pidPos.getControlValue();
 
         // TODO: better handling if feed forward velocity is not set. need movement direction ...
-        core::Twist twist =
-        {
-            .linear = pidPos.getControlValue() + targetPose.twist.linear,
-            .angular = pidOri.getControlValue() + targetPose.twist.angular
-        };
+        core::Twist twist = {.linear = pidPos.getControlValue() + feedforwardVelocity,
+                             .angular = pidOri.getControlValue()
+                            };
 
         return applyTwistLimits(twist);
     }
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
index 3e4ac7e056ff4f1cd9a83a7e494c851b3ef62c12..26369304b0161191042237784d8e2ce485fb8c08 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
@@ -42,6 +42,7 @@ namespace armarx::nav::traj_ctrl
 
         Algorithms algorithm() const override;
         aron::datanavigator::DictNavigatorPtr toAron() const override;
+
         static TrajectoryFollowingControllerParams
         FromAron(const aron::datanavigator::DictNavigatorPtr& dict);
     };
diff --git a/source/Navigation/libraries/trajectory_control/WaypointController.cpp b/source/Navigation/libraries/trajectory_control/WaypointController.cpp
index 2e3282800a87b763386832b06c9e85c72588ef61..10eea28757ef247c179d4cbcf9ea29c2aca9cc20 100644
--- a/source/Navigation/libraries/trajectory_control/WaypointController.cpp
+++ b/source/Navigation/libraries/trajectory_control/WaypointController.cpp
@@ -1,7 +1,9 @@
 #include "WaypointController.h"
 
 #include "Navigation/libraries/trajectory_control/TrajectoryController.h"
+#include "Navigation/libraries/trajectory_control/aron_conversions.h"
 #include "Navigation/libraries/trajectory_control/core.h"
+#include "Navigation/libraries/trajectory_control/aron/WaypointControllerParams.aron.generated.h"
 
 namespace armarx::nav::traj_ctrl
 {
@@ -14,13 +16,24 @@ namespace armarx::nav::traj_ctrl
 
     aron::datanavigator::DictNavigatorPtr WaypointControllerParams::toAron() const
     {
-        return nullptr; // TODO implement
+        arondto::WaypointControllerParams dto;
+
+        WaypointControllerParams bo;
+        aron_conv::toAron(dto, bo);
+
+        return dto.toAron();
     }
 
     WaypointControllerParams
     WaypointControllerParams::FromAron(const aron::datanavigator::DictNavigatorPtr& dict)
     {
-        return WaypointControllerParams(); // TODO implement
+        arondto::WaypointControllerParams dto;
+        dto.fromAron(dict);
+
+        WaypointControllerParams bo;
+        aron_conv::fromAron(dto, bo);
+
+        return bo;
     }
 
     // WaypointController
diff --git a/source/Navigation/libraries/trajectory_control/aron/PIDParams.xml b/source/Navigation/libraries/trajectory_control/aron/PIDParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f31aa05d7a93af2e80b1d6b54f62e09aed5c3786
--- /dev/null
+++ b/source/Navigation/libraries/trajectory_control/aron/PIDParams.xml
@@ -0,0 +1,20 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <GenerateTypes>
+
+        <Object name='armarx::nav::traj_ctrl::arondto::PIDParams'>
+            <ObjectChild key='Kp'>
+                <float />
+            </ObjectChild>
+             <ObjectChild key='Ki'>
+                <float />
+            </ObjectChild>
+             <ObjectChild key='Kd'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
diff --git a/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml b/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5309967bfc4cb1de0b44348ef60c5442c95bead5
--- /dev/null
+++ b/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml
@@ -0,0 +1,25 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+        <!-- <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" /> -->
+    </CodeIncludes>
+    <AronIncludes>
+        <!-- <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml>" /> -->
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <!-- <Object name='armarx::nav::glob_plan::arondto::Point2PointParams' extends="armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams"> -->
+        <Object name='armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams'>
+             <!-- <ObjectChild key='includeStartPose'>
+                <bool />
+            </ObjectChild>
+            <ObjectChild key='velocity'>
+                <float />
+            </ObjectChild> -->
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
diff --git a/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml b/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..3688cf308e11a8c97805bf1bb528c5160a0f290a
--- /dev/null
+++ b/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
@@ -0,0 +1,27 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+        <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" />
+        <Include include="<Navigation/libraries/trajectory_control/aron/PIDParams.aron.generated.h>" />
+    </CodeIncludes>
+    <AronIncludes>
+        <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml>" />
+        <Include include="<Navigation/libraries/trajectory_control/aron/PIDParams.xml>" />
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <!-- <Object name='armarx::nav::traj_ctrl::arondto::TrajectoryFollowingControllerParams' extends="armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams"> -->
+        <Object name='armarx::nav::traj_ctrl::arondto::TrajectoryFollowingControllerParams'>
+            <ObjectChild key='pidPos'>
+                <armarx::nav::traj_ctrl::arondto::PIDParams />
+            </ObjectChild>
+            <ObjectChild key='pidOri'>
+                <armarx::nav::traj_ctrl::arondto::PIDParams />
+            </ObjectChild>
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
diff --git a/source/Navigation/libraries/trajectory_control/aron/WaypointControllerParams.xml b/source/Navigation/libraries/trajectory_control/aron/WaypointControllerParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4deee7d399e6dfd6896a51a7e74f7563229ef839
--- /dev/null
+++ b/source/Navigation/libraries/trajectory_control/aron/WaypointControllerParams.xml
@@ -0,0 +1,25 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+        <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" />
+    </CodeIncludes>
+    <AronIncludes>
+        <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml>" />
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <!-- <Object name='armarx::nav::traj_ctrl::arondto::WaypointControllerParams' extends="armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams"> -->
+        <Object name='armarx::nav::traj_ctrl::arondto::WaypointControllerParams'>
+             <ObjectChild key='includeStartPose'>
+                <bool />
+            </ObjectChild>
+            <ObjectChild key='velocity'>
+                <float />
+            </ObjectChild>
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
diff --git a/source/Navigation/libraries/trajectory_control/aron_conversions.cpp b/source/Navigation/libraries/trajectory_control/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..604fcf9422a1d8af30533da4327d6f241ec4ee03
--- /dev/null
+++ b/source/Navigation/libraries/trajectory_control/aron_conversions.cpp
@@ -0,0 +1,45 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
+#include "Navigation/libraries/trajectory_control/TrajectoryFollowingController.h"
+#include "Navigation/libraries/trajectory_control/WaypointController.h"
+#include "Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h"
+#include "Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h"
+#include "Navigation/libraries/trajectory_control/aron/WaypointControllerParams.aron.generated.h"
+
+namespace armarx::nav::traj_ctrl::aron_conv
+{
+    void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo)
+    {
+
+    }
+
+    void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo)
+    {
+
+    }
+
+    void toAron(arondto::TrajectoryFollowingControllerParams& dto, const TrajectoryFollowingControllerParams& bo)
+    {
+
+    }
+
+    void fromAron(const arondto::TrajectoryFollowingControllerParams& dto, TrajectoryFollowingControllerParams& bo)
+    {
+
+    }
+
+    void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo)
+    {
+
+    }
+
+    void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo)
+    {
+
+    }
+
+
+} // namespace armarx::nav::traj_ctrl::aron_conv
diff --git a/source/Navigation/libraries/trajectory_control/aron_conversions.h b/source/Navigation/libraries/trajectory_control/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..95115c7c94af68409bf73ac883b4d6f45add4c2b
--- /dev/null
+++ b/source/Navigation/libraries/trajectory_control/aron_conversions.h
@@ -0,0 +1,50 @@
+/*
+ * 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
+
+
+namespace armarx::nav::traj_ctrl
+{
+    struct TrajectoryControllerParams;
+    struct TrajectoryFollowingControllerParams;
+    struct WaypointControllerParams;
+
+    namespace arondto
+    {
+        class TrajectoryControllerParams;
+        class TrajectoryFollowingControllerParams;
+        class WaypointControllerParams;
+    } // namespace arondto
+} // namespace armarx::nav::traj_ctrl
+
+namespace armarx::nav::traj_ctrl::aron_conv
+{
+    void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
+    void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
+
+    void toAron(arondto::TrajectoryFollowingControllerParams& dto, const TrajectoryFollowingControllerParams& bo);
+    void fromAron(const arondto::TrajectoryFollowingControllerParams& dto, TrajectoryFollowingControllerParams& bo);
+
+    void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo);
+    void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo);
+
+}  // namespace armarx::nav::traj_ctrl::aron_conv
diff --git a/source/Navigation/libraries/trajectory_control/test/CMakeLists.txt b/source/Navigation/libraries/trajectory_control/test/CMakeLists.txt
index 521263224bc08f8b19d960eb20901bdee37e144f..d6045e3de69de3a9c11ecac9e92315da7e3e6f66 100644
--- a/source/Navigation/libraries/trajectory_control/test/CMakeLists.txt
+++ b/source/Navigation/libraries/trajectory_control/test/CMakeLists.txt
@@ -1,5 +1,13 @@
-
 # Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore trajectory_control)
- 
-armarx_add_test(trajectory_controlTest trajectory_controlTest.cpp "${LIBS}")
\ No newline at end of file
+set(
+    LIBS
+    ${LIBS}
+    ArmarXCore
+    Navigation::TrajectoryControl
+)
+
+armarx_add_test(
+    trajectory_controlTest
+    trajectory_controlTest.cpp
+    "${LIBS}"
+)
diff --git a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
index a9cda01677ac16992f8250a3646216eaba8ef519..34f77cfda0ddd946243fa68228dc693a61444c0d 100644
--- a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
+++ b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
@@ -21,11 +21,14 @@
  */
 
 #include <limits>
+#include <thread>
 
 #include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
 #include <Eigen/src/Geometry/Transform.h>
 #include <Eigen/src/Geometry/Translation.h>
 
+#include <SimoxUtility/math/convert/deg_to_rad.h>
 #include <VirtualRobot/Robot.h>
 
 #include "Navigation/libraries/core/Trajectory.h"
@@ -59,10 +62,9 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
     traj_ctrl::TrajectoryFollowingController controller(params, scene);
 
     core::TrajectoryPoint wpBefore;
-    wpBefore.waypoint.pose = Eigen::Affine3f::Identity();
+    wpBefore.waypoint.pose = core::Pose::Identity();
     wpBefore.waypoint.pose.translation().x() -= 100.F;
-    wpBefore.twist.linear = Eigen::Vector3f::UnitX();
-    wpBefore.twist.angular = Eigen::Vector3f::Zero();
+    wpBefore.velocity = 100.0;
 
     core::TrajectoryPoint wpAfter = wpBefore;
     wpAfter.waypoint.pose.translation().x() += 2000.F;
@@ -79,16 +81,16 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
     BOOST_CHECK_EQUAL(true, true);
 }
 
-
 /**
- * @brief Tests the control 
- * 
+ * @brief Tests the control
+ *
  */
 BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
 {
     core::Scene scene;
     scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
-    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity() * Eigen::Affine3f(Eigen::Translation3f(0, 100, 0)).matrix(), false);
+    scene.robot->setGlobalPose(
+        Eigen::Matrix4f::Identity() * core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(), false);
 
     traj_ctrl::TrajectoryFollowingControllerParams params;
     params.pidPos = {1, 0, 0};
@@ -99,10 +101,9 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
     traj_ctrl::TrajectoryFollowingController controller(params, scene);
 
     core::TrajectoryPoint wpBefore;
-    wpBefore.waypoint.pose = Eigen::Affine3f::Identity();
+    wpBefore.waypoint.pose = core::Pose::Identity();
     wpBefore.waypoint.pose.translation().x() -= 100.F;
-    wpBefore.twist.linear = Eigen::Vector3f::UnitX();
-    wpBefore.twist.angular = Eigen::Vector3f::Zero();
+    wpBefore.velocity = 100.0;
 
     core::TrajectoryPoint wpAfter = wpBefore;
     wpAfter.waypoint.pose.translation().x() += 2000.F;
@@ -118,3 +119,116 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
 
     BOOST_CHECK_EQUAL(true, true);
 }
+
+class TargetReachedCondition
+{
+public:
+    struct Params
+    {
+        struct
+        {
+            float linear;
+            float angular;
+        } distance;
+    };
+
+    TargetReachedCondition(const core::TrajectoryPoint& point, const Params& params) :
+        point(point), params(params)
+    {
+    }
+
+    bool check(const core::Pose& pose) const
+    {
+        const float dp = (pose.translation() - point.waypoint.pose.translation()).norm();
+        const float dr =
+            Eigen::AngleAxisf(pose.linear().inverse() * point.waypoint.pose.linear()).angle();
+
+        return (dp <= params.distance.linear) and (dr <= params.distance.angular);
+    }
+
+private:
+    core::TrajectoryPoint point;
+    const Params params;
+};
+
+class TimeAwareRobot : virtual public VirtualRobot::LocalRobot
+{
+};
+
+/**
+ * @brief Tests the control
+ *
+ */
+BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
+{
+    core::Scene scene;
+    scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
+    scene.robot->setGlobalPose(
+        Eigen::Matrix4f::Identity() * core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(), false);
+
+    traj_ctrl::TrajectoryFollowingControllerParams params;
+    params.pidPos = {1, 0, 0};
+    params.pidOri = {1, 0, 0};
+    params.limits.linearVelocity = std::numeric_limits<float>::max();
+    params.limits.angularVelocity = std::numeric_limits<float>::max();
+
+    traj_ctrl::TrajectoryFollowingController controller(params, scene);
+
+    core::TrajectoryPoint wpBefore;
+    wpBefore.waypoint.pose = core::Pose::Identity();
+    // wpBefore.waypoint.pose.translation().x() -= 100.F;
+    wpBefore.velocity = 100.0;
+
+    core::TrajectoryPoint wpAfter = wpBefore;
+    wpAfter.waypoint.pose.translation().x() += 2000.F;
+
+    core::TrajectoryPtr traj(new core::Trajectory({wpBefore, wpAfter}));
+
+    auto timestamp = std::chrono::steady_clock::now();
+
+    TargetReachedCondition condition(
+        wpAfter,
+        TargetReachedCondition::Params
+    {
+        .distance{.linear = 50.F, .angular = simox::math::deg_to_rad(10.F)}});
+
+    const auto isFinite = [](const auto & x)
+    {
+        return ((x - x).array() == (x - x).array()).all();
+        // return (x.array() != 0.F).any();
+    };
+
+    for (int i = 0; i < 10000; i++)
+    {
+        core::Twist controlVal = controller.control(traj);
+        BOOST_CHECK(isFinite(controlVal.linear));
+        BOOST_CHECK(isFinite(controlVal.angular));
+
+        if (not isFinite(controlVal.linear) or not isFinite(controlVal.angular))
+        {
+            return;
+        }
+
+        ARMARX_INFO << "Control val " << controlVal.linear;
+
+        auto now = std::chrono::steady_clock::now();
+
+        const float dt = std::chrono::duration<float>(now - timestamp).count();
+
+        core::Pose dp = controlVal.poseDiff(dt);
+
+        const core::Pose newPose = core::Pose(scene.robot->getGlobalPose()) * dp;
+        scene.robot->setGlobalPose(newPose.matrix(), false);
+
+        if (condition.check(newPose))
+        {
+            break;
+        }
+
+        ARMARX_INFO << "new pos " << newPose.translation();
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    BOOST_CHECK(condition.check(core::Pose(scene.robot->getGlobalPose())));
+}
diff --git a/source/Navigation/libraries/util/CMakeLists.txt b/source/Navigation/libraries/util/CMakeLists.txt
index 395c536d70a5ade7e294eb93b21f61e47a5aa33a..54a787228e52078f1b1ce6e7dfac1ec2663d9172 100644
--- a/source/Navigation/libraries/util/CMakeLists.txt
+++ b/source/Navigation/libraries/util/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME       util)
+set(LIB_NAME       ${PROJECT_NAME}Util)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -18,7 +18,7 @@ armarx_add_library(
         ./util.h
 )
 
-add_library(Navigation::util ALIAS util)
+add_library(Navigation::Util ALIAS ${PROJECT_NAME}Util)
 
 #find_package(MyLib QUIET)
 #armarx_build_if(MyLib_FOUND "MyLib not available")
@@ -29,4 +29,4 @@ add_library(Navigation::util ALIAS util)
 #endif()
 
 # add unit tests
-add_subdirectory(test)
+# add_subdirectory(test)