diff --git a/VirtualRobot/RobotNodeSet.cpp b/VirtualRobot/RobotNodeSet.cpp index 1ef28241d6e9348ed29ef34be2abebaea9dce348..5ec6e8ce65d9d36b3a5806832357268093b42191 100644 --- a/VirtualRobot/RobotNodeSet.cpp +++ b/VirtualRobot/RobotNodeSet.cpp @@ -3,6 +3,7 @@ #include "SceneObjectSet.h" #include "Robot.h" #include "RobotConfig.h" +#include "VirtualRobot.h" #include "VirtualRobotException.h" #include "CollisionDetection/CollisionChecker.h" #include <set> @@ -551,9 +552,15 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF((i >= (int)robotNodes.size() || i < 0), "Index out of bounds:" << i << ", (should be between 0 and " << (robotNodes.size() - 1)); - return robotNodes[i]; + return robotNodes.at(i); + } + + RobotNodePtr& RobotNodeSet::getNode(const std::string& nodeName) + { + return getNode(getRobotNodeIndex(nodeName)); } + RobotNodePtr& RobotNodeSet::operator[](int i) { THROW_VR_EXCEPTION_IF((i >= (int)robotNodes.size() || i < 0), "Index out of bounds:" << i << ", (should be between 0 and " << (robotNodes.size() - 1)); @@ -803,6 +810,41 @@ namespace VirtualRobot return false; } + bool RobotNodeSet::mirror(const RobotNodeSet& targetNodeSet) + { + const NodeMapping nodeMapping = getRobot()->getNodeMapping(); + const auto nodeNames = getNodeNames(); + + for(const auto& targetNode : targetNodeSet.getAllRobotNodes()) + { + // if node exists in both node set, just copy the joint value + if(hasRobotNode(targetNode->getName())) + { + targetNode->setJointValue(getNode(targetNode->getName())->getJointValue()); + continue; + } + + // otherwise, check if mirroring is possible + const auto nodeIt = nodeMapping.find(targetNode->getName()); + if(nodeIt == nodeMapping.end()) + { + return false; + } + + const NodeMappingElement& mapping = nodeIt->second; + if(not hasRobotNode(mapping.node)) + { + return false; + } + + // mirror it + const auto& sourceNode = getNode(mapping.node); + targetNode->setJointValue(mapping.sign * sourceNode->getJointValue()); + } + + return true; + } + bool RobotNodeSet::isKinematicRoot(RobotNodePtr robotNode) { RobotNodePtr node; diff --git a/VirtualRobot/RobotNodeSet.h b/VirtualRobot/RobotNodeSet.h index f80eaeb4964d18d3116290b6ddd7d06e949b6bd6..950a8548d2eec2d11601822133c013c86bb4f743 100644 --- a/VirtualRobot/RobotNodeSet.h +++ b/VirtualRobot/RobotNodeSet.h @@ -141,6 +141,7 @@ namespace VirtualRobot RobotNodePtr& operator[](int i); RobotNodePtr& getNode(int i); + RobotNodePtr& getNode(const std::string& nodeName); // implement container interface for easy access inline auto begin() @@ -222,6 +223,8 @@ namespace VirtualRobot //! this is forbidden for RobotNodeSets, a call will throw an exception bool removeSceneObject(SceneObjectPtr sceneObject) override; + [[nodiscard]] bool mirror(const RobotNodeSet& targetNodeSet); + protected: /*! * Tests if the given robot node is a valid kinematic root