+ Safety

VirtualRobot/CMakeLists.txt

@@ -172,6 +172,7 @@ SET(SOURCES
     Primitive.cpp
     Random.cpp
     Robot.cpp
+    Safety.cpp
     RobotConfig.cpp
     RobotFactory.cpp
     RobotNodeSet.cpp
@@ -377,6 +378,7 @@ SET(INCLUDES
     Primitive.h
     Random.h
     Robot.h
+    Safety.h
     RobotConfig.h
     RobotFactory.h
     RobotNodeSet.h

VirtualRobot/Safety.cpp

+#include "Safety.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/src/Core/Matrix.h>
+
+#include "MathTools.h"
+#include "VirtualRobot.h"
+
+namespace VirtualRobot
+{
+
+    Circle projectedBoundingCircle(const Robot& robot)
+    {
+        // MathTools::createConvexHull2D(const std::vector<Eigen::Vector2f> &points)
+        std::vector<Eigen::Vector2f> nodePositions;
+
+        const auto nodes = robot.getRobotNodes();
+
+        std::transform(nodes.begin(),
+                       nodes.end(),
+                       std::back_inserter(nodePositions),
+                       [](const RobotNodePtr& node)
+                       { return node->getPoseInRootFrame(); });
+
+        const MathTools::ConvexHull2D hull =
+            MathTools::createConvexHull2D(nodePositions);
+
+        std::vector<float> distances;
+
+        std::transform(hull.vertices.begin(),
+                       hull.vertices.end(),
+                       std::back_inserter(distances),
+                       [](const Eigen::Vector2f& pos) { return pos.norm(); });
+
+        const float maxDistance = *std::max(distances.begin(), distances.end());
+        return Circle{.center = Eigen::Vector2f::Zero(), .radius = maxDistance};
+    }
+} // namespace VirtualRobot
\ No newline at end of file

VirtualRobot/Safety.h

+
+#include "VirtualRobot/Robot.h"
+
+namespace VirtualRobot
+{
+
+    struct Circle
+    {
+        Eigen::Vector2f center;
+        float radius;
+    };
+
+    Circle projectedBoundingCircle(const Robot& robot);
+} // namespace VirtualRobot
\ No newline at end of file