RobotIO.cpp

#include "RobotIO.h"

#include <fstream>
#include <iostream>
#include <vector>

#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/filesystem/remove_trailing_separator.h>
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
#include <VirtualRobot/Import/RobotImporterFactory.h>
#include <VirtualRobot/Nodes/FourBar/Joint.h>

#include "../CollisionDetection/CollisionModel.h"
#include "../EndEffector/EndEffector.h"
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNodeFactory.h"
#include "../Nodes/RobotNodeFixedFactory.h"
#include "../Nodes/RobotNodeHemisphere.h"
#include "../Nodes/RobotNodePrismatic.h"
#include "../RobotConfig.h"
#include "../RobotFactory.h"
#include "../RobotNodeSet.h"
#include "../RuntimeEnvironment.h"
#include "../Transformation/DHParameter.h"
#include "../VirtualRobotException.h"
#include "../Visualization/TriMeshModel.h"
#include "../Visualization/VisualizationFactory.h"
#include "../Visualization/VisualizationNode.h"
#include "Nodes/RobotNodeFourBar.h"
#include "VirtualRobot.h"
#include "mujoco/RobotMjcf.h"
#include "rapidxml.hpp"

namespace VirtualRobot
{
    using std::endl;

    std::map<std::string, int> RobotIO::robot_name_counter;

    RobotIO::RobotIO() = default;

    RobotIO::~RobotIO() = default;

    void
    RobotIO::processChildNode(rapidxml::xml_node<char>* childXMLNode,
                              std::vector<std::string>& childrenNames)
    {
        THROW_VR_EXCEPTION_IF(!childXMLNode, "NULL data for childXMLNode in processChildNode()")

        rapidxml::xml_attribute<>* attr;
        attr = childXMLNode->first_attribute("name", 0, false);
        THROW_VR_EXCEPTION_IF(!attr, "Expecting 'name' attribute in <Child> tag..." << endl)

        std::string s(attr->value());
        childrenNames.push_back(s);
    }

    void
    RobotIO::processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode,
                                       const std::string& nodeName,
                                       std::vector<ChildFromRobotDef>& childrenFromRobot)
    {
        rapidxml::xml_attribute<>* attr;

        THROW_VR_EXCEPTION_IF(!childXMLNode,
                              "NULL data for childXMLNode in processChildFromRobotNode()")

        ChildFromRobotDef d;

        rapidxml::xml_node<>* fileXMLNode = childXMLNode->first_node("file", 0, false);
        int counter = 0;

        while (fileXMLNode)
        {
            d.filename = fileXMLNode->value();

            d.importEEF = true;
            attr = fileXMLNode->first_attribute("importEEF", 0, false);

            if (attr)
            {
                if (!isTrue(attr->value()))
                {
                    d.importEEF = false;
                }
            }

            childrenFromRobot.push_back(d);
            fileXMLNode = fileXMLNode->next_sibling("file", 0, false);
        }

        THROW_VR_EXCEPTION_IF(((!counter) == 0),
                              "Missing file for <ChildFromRobot> tag (in node '" << nodeName
                                                                                 << "')." << endl);
    }

    /**
     * This method processes Limits tags.
     * The values for the attributes "lo" and "hi" are extracted based on the
     * "unit" or "units" attribute.
     *
     * The values are stored in \p jointLimitLo and \p jointLimitHi
     */
    void
    RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode,
                               float& jointLimitLo,
                               float& jointLimitHi,
                               bool& limitless)
    {
        THROW_VR_EXCEPTION_IF(!limitsXMLNode, "NULL data for limitsXMLNode in processLimitsNode()");

        Units unit = getUnitsAttribute(limitsXMLNode, Units::eIgnore);

        try
        {
            jointLimitLo = getFloatByAttributeName(limitsXMLNode, "lo");
        }
        catch (...)
        {
            if (unit.isLength())
            {
                VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -1000 [mm]."
                           << std::endl;
                jointLimitLo = -1000.0f;
                unit = Units("mm");
            }
            else
            {
                VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -180 [deg]."
                           << std::endl;
                jointLimitLo = float(-M_PI);
                unit = Units("rad");
            }
        }

        try
        {
            jointLimitHi = getFloatByAttributeName(limitsXMLNode, "hi");
        }
        catch (...)
        {
            if (unit.isLength())
            {
                VR_WARNING << "No 'hi' attribute in <Limits> tag. Assuming 1000 [mm]." << std::endl;
                jointLimitLo = 1000.0f;
                unit = Units("mm");
            }
            else
            {
                VR_WARNING << "No 'hi' attribute in <Limits> tag. Assuming 180 [deg]." << std::endl;
                jointLimitHi = float(M_PI);
                unit = Units("rad");
            }
        }

        // if values are stored as degrees convert them to radian
        if (unit.isAngle())
        {
            jointLimitLo = unit.toRadian(jointLimitLo);
            jointLimitHi = unit.toRadian(jointLimitHi);
            unit = Units("rad");
        }

        if (unit.isLength())
        {
            jointLimitLo = unit.toMillimeter(jointLimitLo);
            jointLimitHi = unit.toMillimeter(jointLimitHi);
            unit = Units("mm");
        }

        // limitless attribute
        rapidxml::xml_attribute<>* llAttr = limitsXMLNode->first_attribute("limitless");
        if (llAttr != nullptr)
        {
            limitless = isTrue(llAttr->value());
            if (limitless && unit.isAngle() &&
                (unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360))
            {
                VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes "
                              "should equal 2*pi [rad] or 360 [deg]."
                           << endl
                           << "Setting 'lo' to -pi and 'hi' to pi [rad]..." << std::endl;
                jointLimitLo = float(-M_PI);
                jointLimitHi = float(M_PI);
            }
        }
    }

    RobotNodePtr
    RobotIO::processJointNode(rapidxml::xml_node<char>* jointXMLNode,
                              const std::string& robotNodeName,
                              RobotPtr robot,
                              VisualizationNodePtr visualizationNode,
                              CollisionModelPtr collisionModel,
                              SceneObject::Physics& physics,
                              RobotNode::RobotNodeType rntype,
                              Eigen::Matrix4f& transformationMatrix)
    {
        float jointLimitLow = float(-M_PI);
        float jointLimitHigh = float(M_PI);
        bool limitless = false;

        Eigen::Matrix4f preJointTransform = transformationMatrix; //Eigen::Matrix4f::Identity();
        Eigen::Vector3f axis = Eigen::Vector3f::Zero();
        Eigen::Vector3f translationDir = Eigen::Vector3f::Zero();

        std::vector<std::string> propagateJVName;
        std::vector<float> propagateJVFactor;

        rapidxml::xml_attribute<>* attr;
        float jointOffset = 0.0f;
        float initialvalue = 0.0f;

        if (!jointXMLNode)
        {
            // no <Joint> tag -> fixed joint
            RobotNodePtr robotNode;
            RobotNodeFactoryPtr fixedNodeFactory =
                RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr);
            if (fixedNodeFactory)
            {
                robotNode = fixedNodeFactory->createRobotNode(robot,
                                                              robotNodeName,
                                                              visualizationNode,
                                                              collisionModel,
                                                              jointLimitLow,
                                                              jointLimitHigh,
                                                              jointOffset,
                                                              preJointTransform,
                                                              axis,
                                                              translationDir,
                                                              physics,
                                                              rntype);
            }
            return robotNode;
        }

        std::string jointType;
        attr = jointXMLNode->first_attribute("type", 0, false);
        if (attr)
        {
            jointType = getLowerCase(attr->value());
        }
        else
        {
            VR_WARNING << "No 'type' attribute for <Joint> tag. "
                       << "Assuming fixed joint for RobotNode " << robotNodeName << "!"
                       << std::endl;
            jointType = RobotNodeFixedFactory::getName();
        }

        attr = jointXMLNode->first_attribute("offset", 0, false);
        if (attr)
        {
            jointOffset = convertToFloat(attr->value());
        }

        attr = jointXMLNode->first_attribute("initialvalue", 0, false);
        if (attr)
        {
            initialvalue = convertToFloat(attr->value());
        }

        rapidxml::xml_node<>* node = jointXMLNode->first_node();
        rapidxml::xml_node<>* tmpXMLNodeAxis = nullptr;
        rapidxml::xml_node<>* tmpXMLNodeTranslation = nullptr;
        rapidxml::xml_node<>* limitsNode = nullptr;

        float maxVelocity = -1.0f; // m/s
        float maxAcceleration = -1.0f; // m/s^2
        float maxTorque = -1.0f; // Nm
        bool scaleVisu = false;
        Eigen::Vector3f scaleVisuFactor = Eigen::Vector3f::Zero();

        std::optional<RobotNodeHemisphere::XmlInfo> hemisphere;
        std::optional<RobotNodeFourBar::XmlInfo> fourBarXmlInfo;

        while (node)
        {
            const std::string nodeName = getLowerCase(node->name());

            if (nodeName == "dh")
            {
                THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! "
                                   "Use "
                                   "<RobotNode><Transform><DH>...</DH></Transform><Joint>...</"
                                   "Joint></RobotNode> structure.")
                //THROW_VR_EXCEPTION_IF(dhXMLNode, "Multiple DH definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
                //dhXMLNode = node;
            }
            else if (nodeName == "limits")
            {
                THROW_VR_EXCEPTION_IF(limitsNode,
                                      "Multiple limits definitions in <Joint> tag of robot node <"
                                          << robotNodeName << ">." << endl);
                limitsNode = node;
                processLimitsNode(limitsNode, jointLimitLow, jointLimitHigh, limitless);
            }
            else if (nodeName == "prejointtransform")
            {
                THROW_VR_EXCEPTION(
                    "PreJointTransform is DEPRECATED! "
                    "Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> "
                    "structure")
                //THROW_VR_EXCEPTION_IF(prejointTransformNode, "Multiple preJoint definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
                //prejointTransformNode = node;
            }
            else if (nodeName == "axis")
            {
                THROW_VR_EXCEPTION_IF(tmpXMLNodeAxis,
                                      "Multiple axis definitions in <Joint> tag of robot node <"
                                          << robotNodeName << ">." << endl);
                tmpXMLNodeAxis = node;
            }
            else if (nodeName == "translationdirection")
            {
                THROW_VR_EXCEPTION_IF(
                    tmpXMLNodeTranslation,
                    "Multiple translation definitions in <Joint> tag of robot node <"
                        << robotNodeName << ">." << endl);
                tmpXMLNodeTranslation = node;
            }
            else if (nodeName == "postjointtransform")
            {
                THROW_VR_EXCEPTION(
                    "postjointtransform is DEPRECATED and not longer allowed! "
                    "Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> "
                    "structure")
                //THROW_VR_EXCEPTION_IF(postjointTransformNode, "Multiple postjointtransform definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
                //postjointTransformNode = node;
            }
            else if (nodeName == "maxvelocity")
            {
                maxVelocity = getFloatByAttributeName(node, "value");

                // convert to m/s
                std::vector<Units> unitsAttr = getUnitsAttributes(node);
                Units uTime("sec");
                Units uLength("m");
                Units uAngle("rad");

                for (auto& i : unitsAttr)
                {
                    if (i.isTime())
                    {
                        uTime = i;
                    }

                    if (i.isLength())
                    {
                        uLength = i;
                    }

                    if (i.isAngle())
                    {
                        uAngle = i;
                    }
                }

                float factor = 1.0f;

                if (uTime.isMinute())
                {
                    factor /= 60.0f;
                }

                if (uTime.isHour())
                {
                    factor /= 3600.0f;
                }

                if (uLength.isMillimeter())
                {
                    factor *= 0.001f;
                }

                if (uAngle.isDegree())
                {
                    factor *= float(M_PI / 180.0);
                }

                maxVelocity *= factor;
            }
            else if (nodeName == "maxacceleration")
            {
                maxAcceleration = getFloatByAttributeName(node, "value");

                // convert to m/s^2
                std::vector<Units> unitsAttr = getUnitsAttributes(node);
                Units uTime("sec");
                Units uLength("m");
                Units uAngle("rad");

                for (auto& i : unitsAttr)
                {
                    if (i.isTime())
                    {
                        uTime = i;
                    }

                    if (i.isLength())
                    {
                        uLength = i;
                    }

                    if (i.isAngle())
                    {
                        uAngle = i;
                    }
                }

                float factor = 1.0f;

                if (uTime.isMinute())
                {
                    factor /= 3600.0f;
                }

                if (uTime.isHour())
                {
                    factor /= 12960000.0f;
                }

                if (uLength.isMillimeter())
                {
                    factor *= 0.001f;
                }

                if (uAngle.isDegree())
                {
                    factor *= float(M_PI / 180.0);
                }

                maxAcceleration *= factor;
            }
            else if (nodeName == "maxtorque")
            {
                maxTorque = getFloatByAttributeName(node, "value");
                // convert to Nm
                std::vector<Units> unitsAttr = getUnitsAttributes(node);
                Units uLength("m");

                for (auto& i : unitsAttr)
                {
                    if (i.isLength())
                    {
                        uLength = i;
                    }
                }

                float factor = 1.0f;

                if (uLength.isMillimeter())
                {
                    factor *= 1000.0f;
                }

                maxTorque *= factor;
            }
            else if (nodeName == "propagatejointvalue")
            {
                rapidxml::xml_attribute<>* attrPropa;
                attrPropa = node->first_attribute("name", 0, false);
                THROW_VR_EXCEPTION_IF(!attrPropa,
                                      "Expecting 'name' attribute in <PropagateJointValue> tag..."
                                          << endl);

                std::string s(attrPropa->value());
                propagateJVName.push_back(s);
                float f = 1.0f;
                attrPropa = node->first_attribute("factor", 0, false);

                if (attrPropa)
                {
                    f = convertToFloat(attrPropa->value());
                }

                propagateJVFactor.push_back(f);
            }
            else if (nodeName == "scalevisualization")
            {
                scaleVisu = true;
                scaleVisuFactor[0] = getFloatByAttributeName(node, "x");
                scaleVisuFactor[1] = getFloatByAttributeName(node, "y");
                scaleVisuFactor[2] = getFloatByAttributeName(node, "z");
            }
            else if (nodeName == "hemisphere")
            {
                hemisphere.emplace();

                std::string roleString = processStringAttribute("role", node, true);
                roleString = simox::alg::to_lower(roleString);
                try
                {
                    hemisphere->role = RobotNodeHemisphere::RoleFromString(roleString);
                }
                catch (const std::out_of_range& e)
                {
                    THROW_VR_EXCEPTION("Invalid role in hemisphere joint: " << e.what())
                }

                switch (hemisphere->role)
                {
                    case RobotNodeHemisphere::Role::FIRST:
                        hemisphere->lever = getFloatByAttributeName(node, "lever");
                        hemisphere->theta0Rad =
                            simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0"));
                        if (node->first_attribute("limitLo"))
                        {
                            hemisphere->limitLo = getFloatByAttributeName(node, "limitLo");
                        }
                        if (node->first_attribute("limitHi"))
                        {
                            hemisphere->limitHi = getFloatByAttributeName(node, "limitHi");
                        }
                        break;
                    case RobotNodeHemisphere::Role::SECOND:
                        break;
                }
            }
            else if (nodeName == "four_bar")
            {
                fourBarXmlInfo.emplace();

                std::string roleString = processStringAttribute("role", node, true);
                roleString = simox::alg::to_lower(roleString);
                try
                {
                    fourBarXmlInfo->role = RobotNodeFourBar::RoleFromString(roleString);
                }
                catch (const std::out_of_range& e)
                {
                    THROW_VR_EXCEPTION("Invalid role in four_bar joint: " << e.what())
                }

                const rapidxml::xml_node<>* dimensionsNode =
                    node->first_node("dimensions", 0, false);

                if (fourBarXmlInfo->role == RobotNodeFourBar::Role::ACTIVE)
                {
                    if (dimensionsNode == nullptr)
                    {
                        THROW_VR_EXCEPTION("Missing <dimensions> node for four_bar joint.");
                    }

                    fourBarXmlInfo->dimensions = four_bar::Joint::Dimensions{
                        .shank = getFloatByAttributeName(dimensionsNode, "shank"),
                        .p1 = getFloatByAttributeName(dimensionsNode, "p1"),
                        .p2 = getFloatByAttributeName(dimensionsNode, "p2"),
                        .p3 = getFloatByAttributeName(dimensionsNode, "p3")};
                }
            }
            else
            {
                THROW_VR_EXCEPTION("XML definition <"
                                   << nodeName << "> not supported in <Joint> tag of RobotNode <"
                                   << robotNodeName << ">." << endl);
            }

            node = node->next_sibling();
        }

        /*
        if (dhXMLNode)
        {
            // check for wrongly defined nodes
            THROW_VR_EXCEPTION_IF((prejointTransformNode || tmpXMLNodeAxis || tmpXMLNodeTranslation || postjointTransformNode), "DH specification can not be used together with Axis, TranslationDirection, PreJointTransform or PostJointTransform definitions in <Joint> tag of node " << robotNodeName << endl);
            processDHNode(dhXMLNode,dh);

        } else
        {
            dh.isSet = false;
            processTransformNode(prejointTransformNode, robotNodeName, preJointTransform);
            processTransformNode(postjointTransformNode, robotNodeName, postJointTransform);
            */
        if (jointType == "revolute")
        {
            if (scaleVisu)
            {
                VR_WARNING << "Ignoring ScaleVisualization in Revolute joint." << std::endl;
                scaleVisu = false;
            }

            if (tmpXMLNodeAxis)
            {
                axis[0] = getFloatByAttributeName(tmpXMLNodeAxis, "x");
                axis[1] = getFloatByAttributeName(tmpXMLNodeAxis, "y");
                axis[2] = getFloatByAttributeName(tmpXMLNodeAxis, "z");
            }
            else
            {
                // silently setting axis to (0,0,1)
                //VR_WARNING << "Joint '" << robotNodeName << "' without 'axis' tag. Setting rotation axis to (0,0,1)." << std::endl;
                axis << 0, 0, 1.0f;
                //THROW_VR_EXCEPTION("joint '" << robotNodeName << "' wrongly defined, expecting 'axis' tag." << endl);
            }
        }
        else if (jointType == "prismatic")
        {
            if (tmpXMLNodeTranslation)
            {
                translationDir[0] = getFloatByAttributeName(tmpXMLNodeTranslation, "x");
                translationDir[1] = getFloatByAttributeName(tmpXMLNodeTranslation, "y");
                translationDir[2] = getFloatByAttributeName(tmpXMLNodeTranslation, "z");
            }
            else
            {
                THROW_VR_EXCEPTION("Prismatic joint '"
                                   << robotNodeName
                                   << "' wrongly defined, expecting 'TranslationDirection' tag."
                                   << endl);
            }

            if (scaleVisu)
            {
                THROW_VR_EXCEPTION_IF(scaleVisuFactor.norm() == 0.0f, "Zero scale factor");
            }
        }
        else if (jointType == "hemisphere")
        {
        }

        //}

        RobotNodePtr robotNode;
        RobotNodeFactoryPtr robotNodeFactory = RobotNodeFactory::fromName(jointType, nullptr);
        if (robotNodeFactory)
        {
            /*if (dh.isSet)
            {
                robotNode = robotNodeFactory->createRobotNodeDH(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, dh, physics, rntype);
            } else
            {*/
            // create nodes that are not defined via DH parameters
            robotNode = robotNodeFactory->createRobotNode(robot,
                                                          robotNodeName,
                                                          visualizationNode,
                                                          collisionModel,
                                                          jointLimitLow,
                                                          jointLimitHigh,
                                                          jointOffset,
                                                          preJointTransform,
                                                          axis,
                                                          translationDir,
                                                          physics,
                                                          rntype);
            //}
        }
        else
        {
            THROW_VR_EXCEPTION("RobotNode of type " << jointType << " nyi..." << endl);
        }

        robotNode->setMaxVelocity(maxVelocity);
        robotNode->setMaxAcceleration(maxAcceleration);
        robotNode->setMaxTorque(maxTorque);
        robotNode->setLimitless(limitless);

        robotNode->jointValue = initialvalue;

        if (robotNode->isJoint())
        {
            if (robotNode->jointValue < robotNode->jointLimitLo)
            {
                robotNode->jointValue = robotNode->jointLimitLo;
            }
            else if (robotNode->jointValue > robotNode->jointLimitHi)
            {
                robotNode->jointValue = robotNode->jointLimitHi;
            }
        }
        if (robotNode->isHemisphereJoint() and hemisphere.has_value())
        {
            RobotNodeHemispherePtr node = std::dynamic_pointer_cast<RobotNodeHemisphere>(robotNode);
            node->setXmlInfo(hemisphere.value());
        }

        if (robotNode->isFourBarJoint() and fourBarXmlInfo.has_value())
        {
            auto node = std::dynamic_pointer_cast<RobotNodeFourBar>(robotNode);
            node->setXmlInfo(fourBarXmlInfo.value());
        }

        if (scaleVisu)
        {
            RobotNodePrismaticPtr rnPM = std::dynamic_pointer_cast<RobotNodePrismatic>(robotNode);
            if (rnPM)
            {
                rnPM->setVisuScaleFactor(scaleVisuFactor);
            }
        }

        VR_ASSERT(propagateJVName.size() == propagateJVFactor.size());

        for (size_t i = 0; i < propagateJVName.size(); i++)
        {
            robotNode->propagateJointValue(propagateJVName[i], propagateJVFactor[i]);
        }

        return robotNode;
    }

    RobotNodePtr
    RobotIO::processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
                              RobotPtr robo,
                              const std::string& basePath,
                              int& robotNodeCounter,
                              std::vector<std::string>& childrenNames,
                              std::vector<ChildFromRobotDef>& childrenFromRobot,
                              RobotDescription loadMode,
                              RobotNode::RobotNodeType rntype)
    {
        childrenFromRobot.clear();
        THROW_VR_EXCEPTION_IF(!robotNodeXMLNode, "NULL data in processRobotNode");

        // get name
        std::string robotNodeName = processNameAttribute(robotNodeXMLNode);

        if (robotNodeName.empty())
        {
            std::stringstream ss;
            ss << robo->getType() << "_Node_" << robotNodeCounter;
            robotNodeName = ss.str();
            robotNodeCounter++;
            VR_WARNING << "RobotNode definition expects attribute 'name'. Setting name to "
                       << robotNodeName << std::endl;
        }


        // visu data
        bool visuProcessed = false;
        //bool enableVisu = true;
        bool useAsColModel;

        // collision information
        bool colProcessed = false;

        VisualizationNodePtr visualizationNode = nullptr;
        CollisionModelPtr collisionModel = nullptr;
        RobotNodePtr robotNode;
        std::string visualizationModelXML;
        std::string collisionModelXML;
        SceneObject::Physics physics;
        bool physicsDefined = false;
        Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity();
        SceneObject::PrimitiveApproximation primitiveApproximation;

        rapidxml::xml_node<>* node = robotNodeXMLNode->first_node();
        rapidxml::xml_node<>* jointNodeXML = nullptr;

        std::vector<rapidxml::xml_node<>*> sensorTags;
        std::vector<GraspSetPtr> graspSets;

        while (node)
        {
            std::string nodeName = getLowerCase(node->name());

            if (nodeName == "visualization")
            {
                if (loadMode == eFull || loadMode == eFullVisAsCol)
                {
                    THROW_VR_EXCEPTION_IF(visuProcessed,
                                          "Two visualization tags defined in RobotNode '"
                                              << robotNodeName << "'." << endl);
                    visualizationNode =
                        processVisualizationTag(node, robotNodeName, basePath, useAsColModel);
                    visuProcessed = true;

                    if (useAsColModel && visualizationNode)
                    {
                        THROW_VR_EXCEPTION_IF(colProcessed,
                                              "Two collision tags defined in RobotNode '"
                                                  << robotNodeName << "'." << endl);
                        std::string colModelName = robotNodeName;
                        colModelName += "_VISU_ColModel";
                        // clone model
                        VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
                        // todo: ID?
                        collisionModel.reset(new CollisionModel(
                            visualizationNodeClone, colModelName, CollisionCheckerPtr()));
                        colProcessed = true;
                    }
                }
                else if (loadMode == eCollisionModel)
                {
                    VisualizationNodePtr visualizationNodeCM =
                        checkUseAsColModel(node, robotNodeName, basePath);

                    if (visualizationNodeCM)
                    {
                        useAsColModel = true;
                        THROW_VR_EXCEPTION_IF(colProcessed,
                                              "Two collision tags defined in RobotNode '"
                                                  << robotNodeName << "'." << endl);
                        std::string colModelName = robotNodeName;
                        colModelName += "_VISU_ColModel";
                        // todo: ID?
                        collisionModel.reset(new CollisionModel(
                            visualizationNodeCM, colModelName, CollisionCheckerPtr()));
                        colProcessed = true;
                    }
                }
                else if (loadMode == eStructureStore)
                {
                    std::stringstream ss;
                    ss << *node;
                    visualizationModelXML = ss.str();
                    //rapidxml::print(std::back_inserter(robotNode->visualizationModelXML), node, rapidxml::print_no_indenting);
                }
            }
            else if (nodeName == "collisionmodel")
            {
                if (loadMode == eFull || loadMode == eCollisionModel || loadMode == eFullVisAsCol)
                {
                    THROW_VR_EXCEPTION_IF(colProcessed,
                                          "Two collision tags defined in RobotNode '"
                                              << robotNodeName << "'." << endl);
                    collisionModel = processCollisionTag(node, robotNodeName, basePath);
                    colProcessed = true;
                }
                else if (loadMode == eStructureStore)
                {
                    std::stringstream ss;
                    ss << *node;
                    collisionModelXML = ss.str();
                }
            }
            else if (nodeName == "primitivemodel")
            {
                processPrimitiveModelTag(primitiveApproximation, node);
            }
            else if (nodeName == "child")
            {
                processChildNode(node, childrenNames);
            }
            else if (nodeName == "childfromrobot")
            {
                processChildFromRobotNode(node, robotNodeName, childrenFromRobot);
            }
            else if (nodeName == "joint")
            {
                THROW_VR_EXCEPTION_IF(jointNodeXML,
                                      "Two joint tags defined in RobotNode '" << robotNodeName
                                                                              << "'." << endl);
                jointNodeXML = node;
            }
            else if (nodeName == "physics")
            {
                THROW_VR_EXCEPTION_IF(physicsDefined,
                                      "Two physics tags defined in RobotNode '" << robotNodeName
                                                                                << "'." << endl);
                processPhysicsTag(node, robotNodeName, physics);
                physicsDefined = true;
            }
            else if (nodeName == "transform")
            {
                processTransformNode(robotNodeXMLNode, robotNodeName, transformMatrix);
            }
            else if (nodeName == "sensor")
            {
                sensorTags.push_back(node);
            }
            else if (nodeName == "graspset")
            {
                GraspSetPtr gs = processGraspSet(node, robotNodeName);
                THROW_VR_EXCEPTION_IF(!gs,
                                      "Invalid grasp set in '" << robotNodeName << "'." << endl);
                graspSets.push_back(gs);
            }
            else
            {
                THROW_VR_EXCEPTION("XML definition <"
                                   << nodeName << "> not supported in RobotNode <" << robotNodeName
                                   << ">." << endl);
            }

            node = node->next_sibling();
        }

        if (!colProcessed && visualizationNode && loadMode == eFullVisAsCol)
        {
            // Use collision model as visu model if not found
            std::string colModelName = robotNodeName;
            colModelName += "_VISU_ColModel";
            // clone model
            VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
            // todo: ID?
            collisionModel.reset(
                new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
        }

        //create joint from xml data
        robotNode = processJointNode(jointNodeXML,
                                     robotNodeName,
                                     robo,
                                     visualizationNode,
                                     collisionModel,
                                     physics,
                                     rntype,
                                     transformMatrix);
        robotNode->basePath = basePath;
        robotNode->visualizationModelXML = visualizationModelXML;
        robotNode->collisionModelXML = collisionModelXML;
        robotNode->setPrimitiveApproximation(primitiveApproximation);

        // process sensors
        for (auto& sensorTag : sensorTags)
        {
            processSensor(robotNode, sensorTag, loadMode, basePath);
        }

        for (const auto& graspSet : graspSets)
        {
            robotNode->addGraspSet(graspSet);
        }


        return robotNode;
    }

    VisualizationNodePtr
    RobotIO::checkUseAsColModel(rapidxml::xml_node<>* visuXMLNode,
                                const std::string& /*robotNodeName*/,
                                const std::string& basePath)
    {
        bool enableVisu = true;
        //bool coordAxis = false;
        //float coordAxisFactor = 1.0f;
        //std::string coordAxisText = "";
        //std::string visuCoordType = "";
        std::string visuFileType = "";
        std::string visuFile = "";
        rapidxml::xml_attribute<>* attr;
        VisualizationNodePtr visualizationNode;

        if (!visuXMLNode)
        {
            return visualizationNode;
        }

        attr = visuXMLNode->first_attribute("enable", 0, false);

        if (attr)
        {
            enableVisu = isTrue(attr->value());
        }

        if (enableVisu)
        {
            rapidxml::xml_node<>* visuFileXMLNode = visuXMLNode->first_node("file", 0, false);

            if (visuFileXMLNode)
            {
                attr = visuFileXMLNode->first_attribute("type", 0, false);
                THROW_VR_EXCEPTION_IF(!attr,
                                      "Missing 'type' attribute in <Visualization> tag." << endl);
                visuFileType = attr->value();
                getLowerCase(visuFileType);
                visuFile = processFileNode(visuFileXMLNode, basePath);
            }

            rapidxml::xml_node<>* useColModel =
                visuXMLNode->first_node("useascollisionmodel", 0, false);

            if (useColModel && visuFile != "")
            {
                VisualizationFactoryPtr visualizationFactory =
                    VisualizationFactory::fromName(visuFileType, nullptr);

                if (visualizationFactory)
                {
                    visualizationNode = visualizationFactory->getVisualizationFromFile(visuFile);
                }
                else
                {
                    VR_WARNING << "VisualizationFactory of type '" << visuFileType
                               << "' not present. Ignoring Visualization data." << std::endl;
                }
            }
        }

        return visualizationNode;
    }

    RobotPtr
    RobotIO::processRobot(rapidxml::xml_node<char>* robotXMLNode,
                          const std::string& basePath,
                          RobotDescription loadMode)
    {
        THROW_VR_EXCEPTION_IF(!robotXMLNode,
                              "No <Robot> tag in XML definition! base path = " << basePath);

        // process Attributes
        std::string robotRoot;
        RobotPtr robo;
        robo = processRobotAttributes(robotXMLNode, robotRoot);

        // process xml nodes