RobotNameHelper: added hand related functions

source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp

@@ -121,6 +121,11 @@ void RobotNameHelper::Node::checkValid()
 }
 
 
+std::string RobotNameHelper::Arm::getSide() const
+{
+    return side;
+}
+
 std::string RobotNameHelper::Arm::getKinematicChain() const
 {
     return select("KinematicChain");
@@ -156,6 +161,21 @@ std::string RobotNameHelper::Arm::getHandControllerName() const
     return select("HandControllerName");
 }
 
+std::string RobotNameHelper::Arm::getHandRootNode() const
+{
+    return select("HandRootNode");
+}
+
+std::string RobotNameHelper::Arm::getHandModelPath() const
+{
+    return select("HandModelPath");
+}
+
+std::string RobotNameHelper::Arm::getHandModelPackage() const
+{
+    return select("HandModelPackage");
+}
+
 RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
 {
     return RobotArm(*this, robot);
@@ -172,6 +192,11 @@ std::string RobotNameHelper::Arm::select(const std::string& path) const
     return rnh->select(side + "Arm/" + path);
 }
 
+std::string RobotNameHelper::RobotArm::getSide() const
+{
+    return arm.getSide();
+}
+
 VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
 {
     return robot->getRobotNodeSet(arm.getKinematicChain());
@@ -187,6 +212,21 @@ VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
     return robot->getRobotNode(arm.getTCP());
 }
 
+VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
+{
+    return robot->getRobotNode(arm.getHandRootNode());
+}
+
+Eigen::Matrix4f RobotNameHelper::RobotArm::getTcp2HandRootTransform() const
+{
+    return getTCP()->getPoseInRootFrame().inverse() * getHandRootNode()->getPoseInRootFrame();
+}
+
+RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const
+{
+    return arm;
+}
+
 RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
     : arm(arm), robot(robot)
 {

source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h

@@ -61,6 +61,7 @@ namespace armarx
             friend class RobotNameHelper;
         public:
 
+            std::string getSide() const;
             std::string getKinematicChain() const;
             std::string getTorsoKinematicChain() const;
             std::string getTCP() const;
@@ -68,6 +69,9 @@ namespace armarx
             std::string getEndEffector() const;
             std::string getMemoryHandName() const;
             std::string getHandControllerName() const;
+            std::string getHandRootNode() const;
+            std::string getHandModelPath() const;
+            std::string getHandModelPackage() const;
             RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
 
         private:
@@ -83,9 +87,13 @@ namespace armarx
             friend class RobotNameHelper;
             friend class Arm;
         public:
+            std::string getSide() const;
             VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
             VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
             VirtualRobot::RobotNodePtr getTCP() const;
+            VirtualRobot::RobotNodePtr getHandRootNode() const;
+            Eigen::Matrix4f getTcp2HandRootTransform() const;
+            Arm getArm() const;
 
         private:
             RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);