From a18169f511f9e81a1c615b841ab4edd7eb3d7bfb Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 10 Mar 2023 16:32:12 +0100
Subject: [PATCH] adding code from armarx /manipulation
---
GeometricPlanning/CMakeLists.txt | 2 +
GeometricPlanning/util.cpp | 104 +++++++++++++++++++++++++++++++
GeometricPlanning/util.h | 44 +++++++++++++
3 files changed, 150 insertions(+)
create mode 100644 GeometricPlanning/util.cpp
create mode 100644 GeometricPlanning/util.h
diff --git a/GeometricPlanning/CMakeLists.txt b/GeometricPlanning/CMakeLists.txt
index 371f3f521..77d85645e 100644
--- a/GeometricPlanning/CMakeLists.txt
+++ b/GeometricPlanning/CMakeLists.txt
@@ -32,6 +32,7 @@ SET(SOURCES
path_primitives/CircleSegment.cpp
path_primitives/Line.cpp
visualization.cpp
+ util.cpp
)
SET(HEADERS
@@ -45,6 +46,7 @@ SET(HEADERS
path_primitives/Line.h
assert/assert.h
visualization.h
+ util.h
)
ADD_LIBRARY(GeometricPlanning SHARED) # sources will be added later
diff --git a/GeometricPlanning/util.cpp b/GeometricPlanning/util.cpp
new file mode 100644
index 000000000..fa5f3b22c
--- /dev/null
+++ b/GeometricPlanning/util.cpp
@@ -0,0 +1,104 @@
+/**
+ * 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 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "util.h"
+
+#include <algorithm>
+#include <optional>
+#include <tuple>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/Robot.h>
+
+namespace simox::geometric_planning
+{
+
+
+ std::vector<std::string>
+ nodesMatching(const std::vector<std::string>& allNodeNames, const std::string& identifier)
+ {
+
+ std::vector<std::string> handleNodeNames;
+ std::copy_if(allNodeNames.begin(),
+ allNodeNames.end(),
+ std::back_inserter(handleNodeNames),
+ [&identifier](const auto& nodeName)
+ {
+ // the second check is a bit hacky: we can define transformation nodes in URDF which might match, e.g. "parent->child"
+ return simox::alg::ends_with(nodeName, identifier) and
+ not simox::alg::contains(nodeName, '>');
+ });
+
+ return handleNodeNames;
+ }
+
+ std::optional<std::tuple<VirtualRobot::RobotNodePtr, VirtualRobot::GraspPtr>>
+ findGraspByName(const VirtualRobot::Robot& robot,
+ const std::string& graspSetName,
+ const std::string& graspName)
+ {
+ for (const auto& [node, graspSets] : allGraspSets(robot))
+ {
+ if (not graspSets.empty())
+ {
+ // ARMARX_VERBOSE << "Found grasp sets attached to node " << node->getName();
+ for (const auto& graspSet : graspSets)
+ {
+ // check if grasp set name matches if provided
+ if (not graspSetName.empty() and graspSet->getName() != graspSetName)
+ {
+ continue;
+ }
+
+ for (const auto& grasp : graspSet->getGrasps())
+ {
+ if (graspName == grasp->getName())
+ {
+ return std::make_tuple(node, grasp);
+ }
+ }
+ }
+ }
+ }
+
+ return std::nullopt;
+ }
+
+ std::map<VirtualRobot::RobotNodePtr, std::vector<VirtualRobot::GraspSetPtr>>
+ allGraspSets(const VirtualRobot::Robot& robot)
+ {
+ std::map<VirtualRobot::RobotNodePtr, std::vector<VirtualRobot::GraspSetPtr>> nodeGraspSets;
+
+ for (const auto& node : robot.getRobotNodes())
+ {
+ const auto graspSets = node->getAllGraspSets();
+ for (const auto& graspSet : graspSets)
+ {
+ nodeGraspSets[node].push_back(graspSet);
+ }
+ }
+
+ return nodeGraspSets;
+ }
+
+} // namespace simox::geometric_planning
diff --git a/GeometricPlanning/util.h b/GeometricPlanning/util.h
new file mode 100644
index 000000000..2bc0e7357
--- /dev/null
+++ b/GeometricPlanning/util.h
@@ -0,0 +1,44 @@
+/**
+ * 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 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <map>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+namespace simox::geometric_planning
+{
+
+ std::vector<std::string> nodesMatching(const std::vector<std::string>& allNodeNames,
+ const std::string& identifier);
+
+ std::optional<std::tuple<VirtualRobot::RobotNodePtr, VirtualRobot::GraspPtr>>
+ findGraspByName(const VirtualRobot::Robot& robot,
+ const std::string& graspSetName,
+ const std::string& graspName);
+
+ std::map<VirtualRobot::RobotNodePtr, std::vector<VirtualRobot::GraspSetPtr>>
+ allGraspSets(const VirtualRobot::Robot& robot);
+
+
+} // namespace simox::geometric_planning
--
GitLab