added sync to timestamp function to remoterobot

source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp

@@ -155,6 +155,11 @@ namespace armarx
         return p->toEigen(); // convert to eigen first
     }
 
+    SharedRobotInterfacePrx RemoteRobot::getSharedRobot() const
+    {
+        return this->_robot;
+    }
+
     string RemoteRobot::getName()
     {
         return _robot->getName();
@@ -397,6 +402,40 @@ namespace armarx
         return true;
     }
 
+    bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
+    {
+        if (!robotStatePrx || !robot)
+        {
+            ARMARX_ERROR_S << "NULL data. Aborting..." << endl;
+            return false;
+        }
+
+        RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
+        RobotStateConfig config = robotStatePrx->getRobotStateAtTimestamp(timestamp);
+        if (config.jointMap.empty())
+        {
+            return false;
+        }
+
+        for (NameValueMap::const_iterator it = config.jointMap.begin(); it != config.jointMap.end(); it++)
+        {
+            // joint values
+            const std::string& jointName = it->first;
+            float jointAngle = it->second;
+
+            if (!c->setConfig(jointName, jointAngle))
+            {
+                ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+            }
+        }
+
+        robot->setConfig(c);
+        auto pose = PosePtr::dynamicCast(config.globalPose);
+        robot->setGlobalPose(pose->toEigen());
+        return true;
+    }
+
+
 
     // Private (unused methods)
 
@@ -426,4 +465,5 @@ namespace armarx
         }
     }
 
+
 }

source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h

@@ -171,10 +171,7 @@ namespace armarx
 
 
         /// Use this method to share the robot instance over Ice.
-        inline SharedRobotInterfacePrx getSharedRobot()
-        {
-            return this->_robot;
-        }
+        SharedRobotInterfacePrx getSharedRobot() const;
 
         std::string getName();
 
@@ -192,11 +189,21 @@ namespace armarx
         /*!
                 Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
                 The local clone must have the identical structure as the remote robot model, otherwise an error will be reported.
+                This is the fastest way to get update a local robot.
               */
         static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx);
 
         static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx);
 
+        /**
+         * @brief Synchronizes a local robot to a robot state at timestamp.
+         * @param robot Local robot that should be updated
+         * @param robotStatePrx Proxy to the RobotStateComponent
+         * @param timestamp Timestamp to which the local robot should be sychronized. Must be in range of the state history of the RobotStateComponent.
+         * @return True if successfully synced.
+         */
+        static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp);
+
         // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
 
         //! Clones the structure of this remote robot to a local instance