diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index bc5f88c866919ba58f167387bd89cea2b3b679da..9f54a572433e1ec67af34aa7c7d938d1acd47eb9 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -1242,5 +1242,27 @@ namespace VirtualRobot
propagatingJointValuesEnabled = enabled;
}
+ void Robot::validateNodeMapping(const NodeMapping& nodeMapping) const
+ {
+ for(const auto& nodeMap : nodeMapping)
+ {
+ THROW_VR_EXCEPTION_IF(not hasRobotNode(nodeMap.first), "Node '" + nodeMap.first + " not found in the node set!");
+ THROW_VR_EXCEPTION_IF(not hasRobotNode(nodeMap.second.node), "Node '" + nodeMap.second.node + " not found in the node set!");
+ }
+ }
+
+ void Robot::registerNodeMapping(const NodeMapping& nodeMapping){
+
+ validateNodeMapping(nodeMapping);
+
+ this->nodeMapping = nodeMapping;
+ }
+
+ const NodeMapping& Robot::getNodeMapping() const
+ {
+ return nodeMapping;
+ }
+
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index d9e35d52bc4569228c5069a4a3f8e7e3d1313205..b75e3e9fe4b1497b9f3e9a36b15c855b4edf420e 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -40,6 +40,16 @@ namespace VirtualRobot
class Visualization;
class RobotNode;
+
+ struct NodeMappingElement
+ {
+ std::string node;
+
+ float sign;
+ };
+
+ using NodeMapping = std::unordered_map<std::string, NodeMappingElement>;
+
/*!
This is the main object defining the kinematic structure of a robot.
@@ -167,6 +177,11 @@ namespace VirtualRobot
virtual std::vector<std::string> getRobotNodeSetNames() const;
virtual void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const = 0;
+ /*!
+ * Node mapping
+ */
+ const NodeMapping& getNodeMapping() const;
+
/**
*
*/
@@ -430,8 +445,15 @@ namespace VirtualRobot
return passive;
}
+ void registerNodeMapping(const NodeMapping& nodeMapping);
+
+
protected:
Robot();
+
+ void validateNodeMapping(const NodeMapping& nodeMapping) const;
+
+
/*!
Goes through all RobotNodes and if no visualization is present:
* the collision model is checked and in case it owns a visualization
@@ -456,6 +478,9 @@ namespace VirtualRobot
//float radianToMMfactor = 10;
+ private:
+ NodeMapping nodeMapping;
+
};
/**
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
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 07f09e22fe3b2e1717c800b9fa0f1788b31f027f..aa8c33c3cb303d17eb1807a276947d22716486de 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -627,6 +627,43 @@ namespace VirtualRobot
}
}
+
+ NodeMapping BaseIO::processNodeMapping(rapidxml::xml_node<char>* XMLNode, RobotPtr robot)
+ {
+ std::string parentName = processNameAttribute(XMLNode, true);
+ rapidxml::xml_node<>* node = XMLNode->first_node();
+
+ NodeMapping nodeMapping;
+ while (node != nullptr)
+ {
+ std::string nodeName = getLowerCase(node->name());
+
+ if (nodeName == "mapping")
+ {
+ const std::string from = processStringAttribute("from", node, true);
+ const std::string to = processStringAttribute("to", node, true);
+ const float sign = processFloatAttribute("sign", node, true);
+
+ THROW_VR_EXCEPTION_IF(not(sign == 1 or sign == -1), "'sign' attribute has to be either '1' or '-1'");
+ THROW_VR_EXCEPTION_IF(from.empty(), "'from' attribute is empty!");
+ THROW_VR_EXCEPTION_IF(to.empty(), "'to' attribute is empty!");
+
+ // allow bidirectional lookup
+ nodeMapping.emplace(from, NodeMappingElement{.node = from, .sign = sign});
+ nodeMapping.emplace(to, NodeMappingElement{.node = to, .sign = sign});
+ }
+ else
+ {
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <NodeMapping> with name " << parentName);
+ }
+
+ node = node->next_sibling();
+ }
+
+ return nodeMapping;
+ }
+
+
/**
* This method takes a rapidxml::xml_node and returns the value of the
* first tag it finds with name \p attributeName.
@@ -1680,6 +1717,7 @@ namespace VirtualRobot
return rns;
}
+
bool BaseIO::processFloatValueTags(rapidxml::xml_node<char>* XMLNode, int dim, Eigen::VectorXf& stroreResult)
{
if (!XMLNode || dim <= 0)
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index 601eed2239da8a7ece7de1a53d5523e1b17ddad6..be962a3d0899ac08f3559dca5bf91fa5ea686694 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -43,6 +43,7 @@ namespace rapidxml
namespace VirtualRobot
{
+
/*!
Several basic XML IO methods.
\see RobotIO, SceneIO, ObjectIO
@@ -97,6 +98,8 @@ namespace VirtualRobot
static void getAllAttributes(rapidxml::xml_node<char>* node, const std::string& attrString, std::vector<std::string>& storeValues);
static void processDHNode(rapidxml::xml_node<char>* dhXMLNode, DHParameter& dh);
+ static NodeMapping processNodeMapping(rapidxml::xml_node<char>* XMLNode, RobotPtr robot);
+
static std::string toXML(const Eigen::Matrix4f& m, std::string ident = "\t");
static std::vector<VisualizationNodePtr> processVisuFiles(rapidxml::xml_node<char>* visualizationXMLNode, const std::string& basePath, std::string& fileType);
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 19ba0f9617485ab554890b9b190675f03850858d..d7a2d7dcede9e49c4c48e53bb01f716df0560352 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -16,6 +16,7 @@
#include "../Visualization/TriMeshModel.h"
#include "../RobotConfig.h"
#include "../RuntimeEnvironment.h"
+#include "VirtualRobot.h"
#include "rapidxml.hpp"
#include "mujoco/RobotMjcf.h"
#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
@@ -820,7 +821,9 @@ namespace VirtualRobot
std::vector<rapidxml::xml_node<char>* > robotNodeSetNodes;
std::vector<rapidxml::xml_node<char>* > endeffectorNodes;
- processRobotChildNodes(robotXMLNode, robo, robotRoot, basePath, childrenFromRobotFilesMap, robotNodeSetNodes, endeffectorNodes, loadMode);
+ NodeMapping nodeMapping;
+
+ processRobotChildNodes(robotXMLNode, robo, robotRoot, basePath, childrenFromRobotFilesMap, robotNodeSetNodes, endeffectorNodes, nodeMapping, loadMode);
// process childfromrobot tags
std::map< RobotNodePtr, std::vector< ChildFromRobotDef > >::iterator iter = childrenFromRobotFilesMap.begin();
@@ -906,6 +909,8 @@ namespace VirtualRobot
}
}
+ robo->registerNodeMapping(nodeMapping);
+
return robo;
}
@@ -1023,6 +1028,7 @@ namespace VirtualRobot
std::vector<ChildFromRobotDef> >& childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
+ NodeMapping& nodeMapping,
RobotDescription loadMode)
{
std::vector<RobotNodePtr> robotNodes;
@@ -1105,6 +1111,11 @@ namespace VirtualRobot
{
//skip
}
+ else if ("nodemapping" == nodeName)
+ {
+ VR_INFO << "Found robot node mapping";
+ nodeMapping = processNodeMapping(XMLNode, robo);
+ }
else
{
THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl);
@@ -1120,6 +1131,7 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION("Error while initializing Robot" << endl);
}
+
}
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index f4d2cd0b84c8e01c756e90127f7aebecdd29bda2..b1e9d11a1be94af79f3a62b2c6019111d296176c 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -116,6 +116,7 @@ namespace VirtualRobot
std::vector<ChildFromRobotDef> > & childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
+ NodeMapping& nodeMapping,
RobotDescription loadMode = eFull);
static RobotNodePtr processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
RobotPtr robo,