RobotNameHelper.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotNameHelper.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
// #include <ArmarXCore/core/system/ArmarXDataPath.cpp>
 
#include <algorithm>
 
#include <Eigen/Dense>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
 
namespace armarx
{
    void
    RobotNameHelper::writeRobotInfoNode(const RobotInfoNodePtr& n,
                                        std::ostream& str,
                                        std::size_t indent,
                                        char indentChar)
    {
        const std::string ind(4 * indent, indentChar);
        if (!n)
        {
            str << ind << "nullptr\n";
            return;
        }
        str << ind << n->name << ", profile = " << n->profile << ", value " << n->value << '\n';
        for (const auto& c : n->children)
        {
            writeRobotInfoNode(c, str, indent + 1);
        }
    }
 
    const RobotInfoNodePtr&
    RobotNameHelper::getRobotInfoNodePtr() const
    {
        return robotInfo;
    }
 
    const std::string RobotNameHelper::LocationLeft = "Left";
    const std::string RobotNameHelper::LocationRight = "Right";
 
    RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo,
                                     const StatechartProfilePtr& profile) :
        root(Node(robotInfo)), robotInfo(robotInfo)
    {
        ARMARX_TRACE;
        StatechartProfilePtr p = profile;
        while (p && !p->isRoot())
        {
            profiles.emplace_back(p->getName());
            p = p->getParent();
        }
        profiles.emplace_back(""); // for matching the default/root
    }
 
    std::string
    RobotNameHelper::select(const std::string& path) const
    {
        ARMARX_TRACE;
        Node node = root;
        for (const std::string& p : Split(path, "/"))
        {
            node = node.select(p, profiles);
        }
        if (!node.isValid())
        {
            std::stringstream s;
            s << "RobotNameHelper::select: path " + path + " not found\nrobotInfo:\n";
            writeRobotInfoNode(robotInfo, s);
            throw std::runtime_error(s.str());
        }
        return node.value();
    }
 
    RobotNameHelperPtr
    RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
    {
        return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
    }
 
    RobotNameHelperPtr
    RobotNameHelper::Create(const std::string& robotXmlFilename,
                            const StatechartProfilePtr& profile)
    {
        RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotXmlFilename);
        RapidXmlReaderNode robotNode = reader->getRoot("Robot");
 
        return Create(readRobotInfo(robotNode.first_node("RobotInfo")), profile);
    }
 
    RobotInfoNodePtr
    RobotNameHelper::readRobotInfo(const RapidXmlReaderNode& infoNode)
    {
        const std::string name = infoNode.name();
        const std::string profile = infoNode.attribute_value_or_default("profile", "");
        const std::string value = infoNode.attribute_value_or_default("value", "");
 
        const auto nodes = infoNode.nodes();
 
        std::vector<RobotInfoNodePtr> children;
        children.reserve(nodes.size());
 
        std::transform(nodes.begin(),
                       nodes.end(),
                       std::back_inserter(children),
                       [](const auto& childNode) { return readRobotInfo(childNode); });
 
        return new RobotInfoNode(name, profile, value, children);
    }
 
    Arm
    RobotNameHelper::getArm(const std::string& side) const
    {
        return Arm(shared_from_this(), side);
    }
 
    RobotArm
    RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
    {
        return RobotArm(Arm(shared_from_this(), side), robot);
    }
 
    std::shared_ptr<const RobotNameHelper>
    Arm::getRobotNameHelper() const
    {
        return rnh;
    }
 
    RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo) : robotInfo(robotInfo)
    {
    }
 
    std::string
    RobotNameHelper::Node::value()
    {
        checkValid();
        return robotInfo->value;
    }
 
    RobotNameHelper::Node
    RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
    {
        ARMARX_TRACE;
        if (!isValid())
        {
            return Node(nullptr);
        }
        std::map<std::string, RobotInfoNodePtr> matches;
        for (const RobotInfoNodePtr& child : robotInfo->children)
        {
            if (child->name == name)
            {
                matches[child->profile] = child;
            }
        }
        for (const std::string& p : profiles)
        {
            if (matches.count(p))
            {
                return matches.at(p);
            }
        }
        return Node(nullptr);
    }
 
    bool
    RobotNameHelper::Node::isValid()
    {
        return robotInfo ? true : false;
    }
 
    void
    RobotNameHelper::Node::checkValid()
    {
        if (!isValid())
        {
 
            std::stringstream s;
            s << "RobotNameHelper::Node nullptr exception\nrobotInfo:\n";
            writeRobotInfoNode(robotInfo, s);
            throw std::runtime_error(s.str());
        }
    }
 
    std::string
    Arm::getSide() const
    {
        return side;
    }
 
    std::string
    Arm::getKinematicChain() const
    {
        ARMARX_TRACE;
        return select("KinematicChain");
    }
 
    std::string
    Arm::getTorsoKinematicChain() const
    {
        ARMARX_TRACE;
        return select("TorsoKinematicChain");
    }
 
    std::string
    Arm::getTCP() const
    {
        ARMARX_TRACE;
        return select("TCP");
    }
 
    std::string
    Arm::getForceTorqueSensor() const
    {
        ARMARX_TRACE;
        return select("ForceTorqueSensor");
    }
 
    std::string
    Arm::getForceTorqueSensorFrame() const
    {
        ARMARX_TRACE;
        return select("ForceTorqueSensorFrame");
    }
 
    std::string
    Arm::getEndEffector() const
    {
        ARMARX_TRACE;
        return select("EndEffector");
    }
 
    std::string
    Arm::getMemoryHandName() const
    {
        ARMARX_TRACE;
        return select("MemoryHandName");
    }
 
    std::string
    Arm::getHandControllerName() const
    {
        ARMARX_TRACE;
        return select("HandControllerName");
    }
 
    std::string
    Arm::getHandUnitName() const
    {
        ARMARX_TRACE;
        return select("HandUnitName");
    }
 
    std::string
    Arm::getHandRootNode() const
    {
        ARMARX_TRACE;
        return select("HandRootNode");
    }
 
    std::string
    Arm::getHandModelPath() const
    {
        ARMARX_TRACE;
        return select("HandModelPath");
    }
 
    std::string
    Arm::getAbsoluteHandModelPath() const
    {
        ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
        auto path = getHandModelPath();
        return ArmarXDataPath::getAbsolutePath(path, path) ? path : "";
    }
 
    std::string
    Arm::getHandModelPackage() const
    {
        ARMARX_TRACE;
        return select("HandModelPackage");
    }
 
    std::string
    Arm::getPalmCollisionModel() const
    {
        ARMARX_TRACE;
        return select("PalmCollisionModel");
    }
 
    std::string
    Arm::getFullHandCollisionModel() const
    {
        ARMARX_TRACE;
        return select("FullHandCollisionModel");
    }
 
    RobotArm
    Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
    {
        ARMARX_TRACE;
        return RobotArm(*this, robot);
    }
 
    Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side) :
        rnh(rnh), side(side)
    {
    }
 
    std::string
    Arm::select(const std::string& path) const
    {
        ARMARX_TRACE;
        return rnh->select(side + "Arm/" + path);
    }
 
    std::string
    RobotArm::getSide() const
    {
        ARMARX_TRACE;
        return arm.getSide();
    }
 
    VirtualRobot::RobotNodeSetPtr
    RobotArm::getKinematicChain() const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        return robot->getRobotNodeSet(arm.getKinematicChain());
    }
 
    VirtualRobot::RobotNodeSetPtr
    RobotArm::getTorsoKinematicChain() const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
    }
 
    VirtualRobot::RobotNodePtr
    RobotArm::getTCP() const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        return robot->getRobotNode(arm.getTCP());
    }
 
    VirtualRobot::RobotNodePtr
    RobotArm::getPalmCollisionModel() const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        return robot->getRobotNode(arm.getPalmCollisionModel());
    }
 
    VirtualRobot::TriMeshModelPtr
    RobotArm::getFullHandCollisionModel() const
    {
        ARMARX_TRACE;
        std::string abs;
        ARMARX_CHECK_EXPRESSION(
            ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
        return VirtualRobot::TriMeshModel::FromFile(abs);
    }
 
    VirtualRobot::RobotNodePtr
    RobotArm::getHandRootNode() const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        return robot->getRobotNode(arm.getHandRootNode());
    }
 
    Eigen::Matrix4f
    RobotArm::getTcp2HandRootTransform() const
    {
        ARMARX_TRACE;
        const auto tcp = getTCP();
        ARMARX_CHECK_NOT_NULL(tcp);
        const auto hand = getHandRootNode();
        ARMARX_CHECK_NOT_NULL(hand);
        return tcp->getPoseInRootFrame().inverse() * hand->getPoseInRootFrame();
    }
 
    const VirtualRobot::RobotPtr&
    RobotArm::getRobot() const
    {
        return robot;
    }
 
    const Arm&
    RobotArm::getArm() const
    {
        return arm;
    }
 
    RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) : arm(arm), robot(robot)
    {
        ARMARX_CHECK_NOT_NULL(robot);
    }
 
    const std::vector<std::string>&
    RobotNameHelper::getProfiles() const
    {
        return profiles;
    }
} // namespace armarx