adapted to simox changes regarding getGlobalPose

source/RobotAPI/components/robotstate/SharedRobotServants.cpp

@@ -125,10 +125,20 @@ PoseBasePtr SharedRobotNodeServant::getPostJointTransformation(const Current &cu
     return new Pose(_node->getPostJointTransformation());
 }
 */
-PoseBasePtr SharedRobotNodeServant::getGlobalPose(const Current &current) const
+FramedPoseBasePtr SharedRobotNodeServant::getGlobalPose(const Current &current) const
 {
     ReadLockPtr lock = _node->getRobot()->getReadLock();
-    return new Pose(_node->getGlobalPose());
+    return new FramedPose(_node->getGlobalPose(),
+                          GlobalFrame,
+                          "");
+}
+
+FramedPoseBasePtr SharedRobotNodeServant::getPoseInRootFrame(const Current &current) const
+{
+    ReadLockPtr lock = _node->getRobot()->getReadLock();
+    return new FramedPose(_node->getPoseInRootFrame(),
+                          _node->getRobot()->getRootNode()->getName(),
+                          _node->getRobot()->getName());
 }
 
 

source/RobotAPI/components/robotstate/SharedRobotServants.h

@@ -51,27 +51,28 @@ namespace armarx {
         public SharedObjectBase
     {
     public:
-        SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current &current*/);
+        SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::Current()*/);
         ~SharedRobotNodeServant();
 
-        virtual float getJointValue(const Ice::Current &current) const;
-        virtual std::string getName(const Ice::Current &current) const;
+        virtual float getJointValue(const Ice::Current & current = Ice::Current()) const;
+        virtual std::string getName(const Ice::Current & current = Ice::Current()) const;
 
-        virtual PoseBasePtr getLocalTransformation(const Ice::Current &current) const;
-        virtual PoseBasePtr getGlobalPose(const Ice::Current &current) const;
+        virtual PoseBasePtr getLocalTransformation(const Ice::Current & current = Ice::Current()) const;
+        virtual FramedPoseBasePtr getGlobalPose(const Ice::Current & current = Ice::Current()) const;
+        virtual FramedPoseBasePtr getPoseInRootFrame(const Ice::Current &current = Ice::Current()) const;
 
-        virtual JointType getType(const Ice::Current &current) const;
-        virtual Vector3BasePtr getJointTranslationDirection(const Ice::Current &current) const;
-        virtual Vector3BasePtr getJointRotationAxis(const Ice::Current &current) const;
+        virtual JointType getType(const Ice::Current & current = Ice::Current()) const;
+        virtual Vector3BasePtr getJointTranslationDirection(const Ice::Current & current = Ice::Current()) const;
+        virtual Vector3BasePtr getJointRotationAxis(const Ice::Current & current = Ice::Current()) const;
 
-        virtual bool hasChild(const std::string &name, bool recursive, const Ice::Current &current) const;
-        virtual std::string getParent(const Ice::Current &current) const;
-        virtual NameList getChildren(const Ice::Current &current) const;
-        virtual NameList getAllParents(const std::string &name, const Ice::Current &current) const;
+        virtual bool hasChild(const std::string &name, bool recursive, const Ice::Current & current = Ice::Current()) const;
+        virtual std::string getParent(const Ice::Current & current = Ice::Current()) const;
+        virtual NameList getChildren(const Ice::Current & current = Ice::Current()) const;
+        virtual NameList getAllParents(const std::string &name, const Ice::Current & current = Ice::Current()) const;
 
-        virtual float getJointValueOffest(const Ice::Current &current) const;
-        virtual float getJointLimitHigh(const Ice::Current &current) const;
-        virtual float getJointLimitLow(const Ice::Current &current) const;
+        virtual float getJointValueOffest(const Ice::Current & current = Ice::Current()) const;
+        virtual float getJointLimitHigh(const Ice::Current & current = Ice::Current()) const;
+        virtual float getJointLimitLow(const Ice::Current & current = Ice::Current()) const;
 
 
 
@@ -94,20 +95,20 @@ namespace armarx {
         SharedRobotServant(VirtualRobot::RobotPtr robot);
         ~SharedRobotServant();
 
-        virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string & name, const Ice::Current &current);
-        virtual SharedRobotNodeInterfacePrx getRootNode(const Ice::Current &current);
-        virtual bool hasRobotNode(const std::string & name, const Ice::Current &current);
+        virtual SharedRobotNodeInterfacePrx getRobotNode(const std::string & name, const Ice::Current & current = Ice::Current());
+        virtual SharedRobotNodeInterfacePrx getRootNode(const Ice::Current & current = Ice::Current());
+        virtual bool hasRobotNode(const std::string & name, const Ice::Current & current = Ice::Current());
 
-        virtual NameList getRobotNodes(const Ice::Current &current);
-        virtual RobotNodeSetInfoPtr getRobotNodeSet(const std::string & name, const Ice::Current &current);
-        virtual NameList getRobotNodeSets(const Ice::Current &current);
-        virtual bool hasRobotNodeSet(const std::string & name, const Ice::Current &current);
+        virtual NameList getRobotNodes(const Ice::Current & current = Ice::Current());
+        virtual RobotNodeSetInfoPtr getRobotNodeSet(const std::string & name, const Ice::Current & current = Ice::Current());
+        virtual NameList getRobotNodeSets(const Ice::Current & current = Ice::Current());
+        virtual bool hasRobotNodeSet(const std::string & name, const Ice::Current & current = Ice::Current());
 
-        virtual std::string getName(const Ice::Current &current);
-        virtual std::string getType(const Ice::Current &current);
+        virtual std::string getName(const Ice::Current & current = Ice::Current());
+        virtual std::string getType(const Ice::Current & current = Ice::Current());
 
-        virtual PoseBasePtr getGlobalPose(const Ice::Current &current);
-        NameValueMap getConfig(const Ice::Current &current);
+        virtual PoseBasePtr getGlobalPose(const Ice::Current & current = Ice::Current());
+        NameValueMap getConfig(const Ice::Current & current = Ice::Current());
 
         VirtualRobot::RobotPtr getRobot()
         {

source/RobotAPI/components/units/TCPControlUnit.cpp

@@ -1104,7 +1104,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles,
     {
         RobotNodePtr& node = nodesToReport.at(i);
         const std::string &tcpName  = node->getName();
-        const Eigen::Matrix4f &currentPose = node->getGlobalPose();
+        const Eigen::Matrix4f &currentPose = node->getPoseInRootFrame();
         tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
 
     }
@@ -1130,7 +1130,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
     {
         RobotNodePtr node = nodesToReport.at(i);
         const std::string &tcpName  = node->getName();
-        const Eigen::Matrix4f &currentPose = node->getGlobalPose();
+        const Eigen::Matrix4f &currentPose = node->getPoseInRootFrame();
         tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
 
     }
@@ -1149,7 +1149,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
     {
         RobotNodePtr node = localVelReportRobot->getRobotNode(nodesToReport.at(i)->getName());
         const std::string &tcpName  = node->getName();
-        const Eigen::Matrix4f &currentPose = node->getGlobalPose();
+        const Eigen::Matrix4f &currentPose = node->getPoseInRootFrame();
 
 
         FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);

source/RobotAPI/interface/core/RobotState.ice

@@ -3,7 +3,7 @@
 
 #include <RobotAPI/interface/units/KinematicUnitInterface.ice>
 #include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
 
 module armarx
 {
@@ -56,7 +56,10 @@ module armarx
         PoseBase getLocalTransformation();
         
         ["cpp:const"] idempotent
-        PoseBase getGlobalPose();
+        FramedPoseBase getGlobalPose();
+
+        ["cpp:const"] idempotent
+        FramedPoseBase getPoseInRootFrame();
 
         ["cpp:const"] idempotent
         JointType getType();