RemoteRobot: add synchronizeLocalCloneToState

We can get the current robot state and save it.
Later, we can sync a local robot to that saved state.

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

@@ -488,16 +488,24 @@ namespace armarx
     bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
     {
         ARMARX_CHECK_EXPRESSION(robotStatePrx);
+
+        RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
+        RobotStateConfig state = robotStatePrx->getRobotStateAtTimestamp(timestamp);
+
+        return synchronizeLocalCloneToState(robot, state);
+    }
+
+    bool RemoteRobot::synchronizeLocalCloneToState(RobotPtr robot, const RobotStateConfig& state)
+    {
         ARMARX_CHECK_EXPRESSION(robot);
 
         RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
-        RobotStateConfig config = robotStatePrx->getRobotStateAtTimestamp(timestamp);
-        if (config.jointMap.empty())
+        if (state.jointMap.empty())
         {
             return false;
         }
 
-        for (NameValueMap::const_iterator it = config.jointMap.begin(); it != config.jointMap.end(); it++)
+        for (NameValueMap::const_iterator it = state.jointMap.begin(); it != state.jointMap.end(); it++)
         {
             // joint values
             const std::string& jointName = it->first;
@@ -510,8 +518,9 @@ namespace armarx
         }
 
         robot->setConfig(c);
-        auto pose = PosePtr::dynamicCast(config.globalPose);
+        auto pose = PosePtr::dynamicCast(state.globalPose);
         robot->setGlobalPose(pose->toEigen());
+
         return true;
     }
 

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

@@ -227,6 +227,8 @@ namespace armarx
          */
         static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp);
 
+        static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot, RobotStateConfig const& state);
+
         // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
 
         //! Clones the structure of this remote robot to a local instance