diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp index 03d860c56f2757a1c47de71c9d07eee0e4030c2b..e00ebc3c177ac6012992722de81935ee12013023 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp +++ b/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; } diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h index 03f07a1f4cc4118d3083df4d62d9ae527d9972ba..f9f637f33612c60860dd34990a7a16bb23683528 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h +++ b/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