From 79691f537b9eb164d797c3b9251c5c904e7dcc0c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 8 Jun 2021 13:50:13 +0200
Subject: [PATCH] + Safety
---
VirtualRobot/CMakeLists.txt | 2 ++
VirtualRobot/Safety.cpp | 39 +++++++++++++++++++++++++++++++++++++
VirtualRobot/Safety.h | 14 +++++++++++++
3 files changed, 55 insertions(+)
create mode 100644 VirtualRobot/Safety.cpp
create mode 100644 VirtualRobot/Safety.h
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 13c07881e..771f2da39 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/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
diff --git a/VirtualRobot/Safety.cpp b/VirtualRobot/Safety.cpp
new file mode 100644
index 000000000..9baadf4c1
--- /dev/null
+++ b/VirtualRobot/Safety.cpp
@@ -0,0 +1,39 @@
+#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
diff --git a/VirtualRobot/Safety.h b/VirtualRobot/Safety.h
new file mode 100644
index 000000000..5915ba1c0
--- /dev/null
+++ b/VirtualRobot/Safety.h
@@ -0,0 +1,14 @@
+
+#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
--
GitLab