From 34b11f258f0a3efc3205f71bf09ca19ca682277c Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Thu, 3 Mar 2016 15:55:30 +0100
Subject: [PATCH] robot state doc

---
 etc/doxygen/pages/HowTos.dox      |   3 +-
 etc/doxygen/pages/Overview.dox    |   4 +-
 etc/doxygen/pages/RemoteRobot.dox | 102 +++++++++++++++++++++++++++++-
 3 files changed, 105 insertions(+), 4 deletions(-)

diff --git a/etc/doxygen/pages/HowTos.dox b/etc/doxygen/pages/HowTos.dox
index 8fdef745b..aed242e5d 100644
--- a/etc/doxygen/pages/HowTos.dox
+++ b/etc/doxygen/pages/HowTos.dox
@@ -5,7 +5,7 @@
 
 
 
-\section RobotAPI-HowTos-Eigen-From-Pose How to convert a pose variants to an Eigen matrices
+\section RobotAPI-HowTos-Eigen-From-Pose How to convert a pose variant to an Eigen matrix
 
 Use the following code if you want to convert from an armarx::Pose, an armarx::FramedPose or an armarx::LinkedPose.
 Refer to \ref RobotAPI-FramedPose for further information.
@@ -98,6 +98,7 @@ Start it with the following command:
 The RobotStateComponent provides several methods for accessing the current configuration of the robot and for getting a snapshot of the current state which is compatible with
 models of the Simox/VirtualRobot framework. With these models the whole functionality of Simox (https://gitlab.com/Simox/simox) can be used, e.g. IK solving, collision detection or motion and grasp planning.
 
+See also \ref RobotAPI-RemoteRobot-Overview.
 \par Creating a synchronized model (RemoteRobot)
 
 A RemoteRobot is a synchronized robot data structure which always represents the current state of the robot.
diff --git a/etc/doxygen/pages/Overview.dox b/etc/doxygen/pages/Overview.dox
index 5d192fd31..614040b1c 100644
--- a/etc/doxygen/pages/Overview.dox
+++ b/etc/doxygen/pages/Overview.dox
@@ -35,10 +35,10 @@ Datatypes are available for positions, orientations, directions and poses.
 \par Remote Robot State
 The ArmarX Robot State is the global pose and joint configuration of a robot and is used for coordinate frame transformations.
 These Transformations between coordinate frames are essential everyday operations in a robotics framework.
-ArmarX offers several ways to cope with the different requirements of coordinate transformations.
+ArmarX offers several ways to cope with the different requirements of coordinate transformations in a distributed system like ArmarX.
 
 \par
-\link RobotAPI-RemoteRobot-Overview Read more \endlink
+\subpage RobotAPI-RemoteRobot-Overview "Read more"
 
 \par Interfaces
 RobotAPI provides robot-independent interface definitions that are implemented and used by components in other ArmarX packages.
diff --git a/etc/doxygen/pages/RemoteRobot.dox b/etc/doxygen/pages/RemoteRobot.dox
index a49038c74..d94a1b568 100644
--- a/etc/doxygen/pages/RemoteRobot.dox
+++ b/etc/doxygen/pages/RemoteRobot.dox
@@ -1,7 +1,107 @@
 /**
 \page RobotAPI-RemoteRobot-Overview Remote Robot State
+\tableofcontents
+The ArmarX Robot State is the global pose and joint configuration of a robot and is used for coordinate frame transformations.
+These Transformations between coordinate frames are essential everyday operations in a robotics framework.
+ArmarX offers several ways to cope with the different requirements of coordinate transformations in a distributed system like ArmarX.
 
 Due to the distributed and asynchronous nature of ArmarX these transformations need to be considered with caution.
-The sensor data is measured asynchronously and usually in a different process that the robot state itself.
+The sensor data is measured asynchronously and usually in a different process than the robot state itself.
+Thus, inaccuracies can appear if coordinate transformations are performed without considering the asynchronous nature of ArmarX.
 
+But let's start simple.
+There are 2 options to do coordinate transformations:
+- Transform with a \ref armarx::RemoteRobot "RemoteRobot"
+- Transform with a local robot
+
+Both options use the same interface, just different implementations. Thus, it is easy to interchange them afterwards if the requirements have changed.
+
+<i>But when to use which option?</i>
+
+<b>RemoteRobot:</b><br/>
+The RemoteRobot is a network object. So, each call means a network hop, which introduces a delay on a local ethernet loop of ~0.5ms.
+This option should be used if performance is not of interest, the boilerplate-code should be minimized and the robot is not moving currently.
+
+<b>Local robot:</b><br/>
+If you do several coordinate transformation in one control loop, you should always use a local robot, which is synced to the remote robot at the beginning of the calculation.
+This means you need to setup a local robot clone in your class once and then synchronize it in each cycle with the \ref armarx::RobotStateComponent "RobotStateComponent".
+Sync'ing takes about 1ms and the costs of calls afterwards to the local robot are neglectable.
+
+We will explain in the next sections how to use them.
+
+
+\section RobotAPI-RemoteRobot-Simple-Coordinate-Transformation Simple coordinate transformations with a RemoteRobot object
+If you want to do non-time-critical coordinate transformation, you can use the \ref armarx::RemoteRobot "RemoteRobot" object.
+The basis for this object is a SharedRobotInterfacePrx and can be retrieved from the \ref armarx::RobotStateComponent "RobotStateComponent" with the function \ref armarx::RobotStateComponent::getSynchronizedRobot "getSynchronizedRobot()".
+Then you can just create a new RemoteRobot with (see \ref RobotAPI-HowTos-RemoteRobot-Access "here" for how to access the RobotStateComponent):
+\code
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+SharedRobotInterfacePrx sharedRobot = robotStateComponentPrx->getSynchronizedRobot();
+RemoteRobotPtr remoteRobot(new RemoteRobot(sharedRobot));
+\endcode
+This RemoteRobot implements the Robot interface of Simox' VirtualRobot and can be used in the same way to query the structure and current state of the robot:
+\code
+Eigen::Matrix4f rootPose = remoteRobot->getRootNode()->getGlobalPose();
+\endcode
+
+To use this RemoteRobot to transform an object pose from the cameras to the robots root coordinate system:
+\code
+// Object position 1m away from Cameras of robot Armar3
+FramedPositionPtr position = new FramedPosition(1000,0,0, "Cameras", "Armar3");
+position->toRootFrame(remoteRobot); // if robot was in zero position, now something like 0,1222,1667, "Armar3_Base", "Armar3"
+\endcode
+
+\section RobotAPI-RemoteRobot-Coordinate-Transformation-With-Local-Robot High performance coordinate transformations with a local robot object
+If you need high performance coordinate transformations, you should use a local clone of the remote robot.
+You can create a local clone really easy like this:
+\code
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+VirtualRobot::RobotPtr localRobot(RemoteRobot::createLocalClone(robotStateComponentPrx));
+\endcode
+Now, you have a local robot clone with the robot state from the timestamp where the clone method was called.
+This offers only basic functionality of the RobotPtr like joint names, joint values, robot nodes, robot node sets, robot node poses and transformations.
+If you need all functionality, pass the filepath to the clone method (this path can be retrieved from the RobotStateComponent if both processes are on the same filesystem or have the same filesystem structure).
+This clone call should be done once, since it can take a moment.
+In each cycle of a control loop, the local robot should just be synced to retrieve the current joint values:
+\code
+RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
+\endcode
+Now, the localRobot is updated to the most current robot state. The sync'ing should take around 1ms.
+
+This local robot behaves the same way as the RemoteRobot for coordinate transformations (see above). The difference is that all calculations are done locally and thus much faster.
+
+\section RobotAPI-RemoteRobot-Synching-To-Timestamp Coordinate transformations with a past timestamp
+Since ArmarX is inherently asynchronous, doing coordinate transformations on the most current robot state can introduce errors.
+
+For example the visual object localization of an object can be done based on a stereo image pair.
+This image pair war taken at a specific timestamp.
+From this image pair the 6D object pose is calculated.
+Now, this pose is relative to the camera position.
+But to use it further it usually needs to be transformed in global coordinates or the robot's root coordinate system.
+Though, the image pair was taken at a specific timestamp, which differs from the timestamp after the object recognition algorithm finished.
+If the object pose is transformed to global coordinates now the robot state might already be different because the robot was moving.
+
+To this end, ArmarX offers the possibility to query the robotstate at an arbitrary point in time within the range of the buffer (default: 100s).
+Since the timestamp of the picture was most probably not the same as when the robot state was updated, queries are interpolated linearly.
+
+So let's get down to business how to use the robot state history:
+\code
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+VirtualRobot::RobotPtr localRobot(RemoteRobot::createLocalClone(robotStateComponentPrx));
+while(true)
+{
+    Ice::Long timestamp = // timestamp of camera capture
+    bool success = RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponentPrx, timestamp);
+    // use localRobot
+}
+\endcode
+As you can see, it is almost the same as before, we just need the timestamp to which we want to sync our local robot.
+If the timestamp is not valid, the sync function returns false.
+Now, we can transform the object pose in the most accurate way possible.
 */
-- 
GitLab