diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp index 5bf8b9eaa04605ebb19483a147b675a7e1b337d1..9d0551330671517b37dc0872135e302cd268d00b 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp +++ b/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) { diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h index 0f633f431863e9c6064d559f72776487de08734d..9af13effafd0df85f8426a3780d5a4c665ee24f2 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h +++ b/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);