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 c94aa639a276ffaca920f1e60254f1e3488b4422..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}")
@@ -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")
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..8d00a2013ff7165aa261510edae3053f93aa320d 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,32 @@ armarx_add_library(
LIBS
ArmarXCoreInterfaces
ArmarXCore
- Navigation::core
+ 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/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 13407ff3e4af30f52d548bd024b2dd34fd68bdef..ffe51c747db1d22bad7c77dc5b61737210f7aef4 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.cpp
+++ b/source/Navigation/libraries/global_planning/Point2Point.cpp
@@ -48,7 +48,7 @@ namespace armarx::nav::glob_plan
{
trajectory.push_back(core::TrajectoryPoint
{
- .waypoint = core::Waypoint{.pose = core::Pose(context.robot->getGlobalPose())},
+ .waypoint = core::Waypoint{.pose = core::Pose(scene.robot->getGlobalPose())},
.twist = {.linear = Eigen::Vector3f::Zero(), .angular = Eigen::Vector3f::Zero()}});
}
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/Point2PointParams.xml b/source/Navigation/libraries/global_planning/aron/Point2PointParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8c3bbe2d74d9ae4ca026e3855d98ba330b3db5ab
--- /dev/null
+++ b/source/Navigation/libraries/global_planning/aron/Point2PointParams.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::Point2PointParams'>
+ <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_conversions.cpp b/source/Navigation/libraries/global_planning/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
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..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
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/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..5e52ec37a7d8a5bd3586e70336b39ae73c864b8b 100644
--- a/source/Navigation/libraries/trajectory_control/CMakeLists.txt
+++ b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME trajectory_control)
+set(LIB_NAME ${PROJECT_NAME}TrajectoryControl)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
@@ -8,7 +8,7 @@ armarx_add_library(
ArmarXCoreInterfaces
ArmarXCore
RobotAPICore
- Navigation::core
+ Navigation::Core
SOURCES
./TrajectoryController.cpp
@@ -20,7 +20,7 @@ armarx_add_library(
./WaypointController.h
)
-add_library(Navigation::trajectory_control ALIAS trajectory_control)
+add_library(Navigation::TrajectoryControl ALIAS ${PROJECT_NAME}TrajectoryControl)
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
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 a6e8743c660791ef8159ba47189321cb5264cb31..40a73bd90266c81cebd71826e4e81b1869d02402 100644
--- a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
+++ b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
@@ -21,6 +21,7 @@
*/
#include <limits>
+#include <thread>
#include <Eigen/Geometry>
#include <Eigen/src/Geometry/AngleAxis.h>
@@ -89,9 +90,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
{
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);
+ scene.robot->setGlobalPose(
+ Eigen::Matrix4f::Identity() * core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(), false);
traj_ctrl::TrajectoryFollowingControllerParams params;
params.pidPos = {1, 0, 0};
@@ -153,6 +153,11 @@ private:
const Params params;
};
+class TimeAwareRobot: virtual public VirtualRobot::LocalRobot
+{
+
+};
+
/**
* @brief Tests the control
*
@@ -161,9 +166,8 @@ 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);
+ scene.robot->setGlobalPose(
+ Eigen::Matrix4f::Identity() * core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(), false);
traj_ctrl::TrajectoryFollowingControllerParams params;
params.pidPos = {1, 0, 0};
@@ -175,7 +179,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
core::TrajectoryPoint wpBefore;
wpBefore.waypoint.pose = core::Pose::Identity();
- wpBefore.waypoint.pose.translation().x() -= 100.F;
+ // wpBefore.waypoint.pose.translation().x() -= 100.F;
wpBefore.twist.linear = Eigen::Vector3f::UnitX();
wpBefore.twist.angular = Eigen::Vector3f::Zero();
@@ -186,14 +190,30 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
auto timestamp = std::chrono::steady_clock::now();
- TargetReachedCondition condition(wpAfter,
- TargetReachedCondition::Params{.distance{
- .linear = 50.F, .angular = simox::math::deg_to_rad(10.F)}});
+ TargetReachedCondition condition(
+ wpAfter,
+ TargetReachedCondition::Params
+ {
+ .distance{.linear = 50.F, .angular = simox::math::deg_to_rad(10.F)}});
- for (int i = 0; i < 50; i++)
+ 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);
- ARMARX_DEBUG << "Control val " << controlVal.linear;
+ 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();
@@ -202,14 +222,16 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
core::Pose dp = controlVal.poseDiff(dt);
const core::Pose newPose = core::Pose(scene.robot->getGlobalPose()) * dp;
- scene.robot->setGlobalPose(newPose.matrix());
+ 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)