diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp index 7cb4951b41b79fa56e17186d85848ff721b77b13..7c293b74840d6f55ec21f0cf1fabe253468441f9 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp +++ b/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 } } + } diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h index 64bca72366ea03d0c8f6399d93352f1433f63888..c87eaf1602e23b3597aacf5f37f59af32b76c6f5 100644 --- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h +++ b/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