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 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 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 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.

But when to use which option?

RemoteRobot:
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.

Local robot:
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 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.

Simple coordinate transformations with a RemoteRobot object

If you want to do non-time-critical coordinate transformation, you can use the RemoteRobot object. The basis for this object is a SharedRobotInterfacePrx and can be retrieved from the RobotStateComponent with the function getSynchronizedRobot(). Then you can just create a new RemoteRobot with (see here for how to access the RobotStateComponent):

#include <RobotAPI/interface/core/RobotState.h>
SharedRobotInterfacePrx sharedRobot = robotStateComponentPrx->getSynchronizedRobot();
RemoteRobotPtr remoteRobot(new RemoteRobot(sharedRobot));

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:

Eigen::Matrix4f rootPose = remoteRobot->getRootNode()->getGlobalPose();

To use this RemoteRobot to transform an object pose from the cameras to the robots root coordinate system:

// 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"

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:

#include <RobotAPI/interface/core/RobotState.h>
VirtualRobot::RobotPtr localRobot(RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages()));

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 pass in an empty filename (or omit) to the clone method only limited functionality is available (only coordinate transformations). 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:

RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);

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.

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:

#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
}

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.

RemoteRobot.h
armarx::SharedRobotInterfacePrx
::IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface > SharedRobotInterfacePrx
Definition: FramedPose.h:57
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
armarx::FramedPositionPtr
IceInternal::Handle< FramedPosition > FramedPositionPtr
Definition: FramedPose.h:134
timestamp
std::string timestamp()
Definition: CartographerAdapter.cpp:85
armarx::RemoteRobotPtr
std::shared_ptr< RemoteRobot > RemoteRobotPtr
Definition: RemoteRobot.h:263
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
armarx::status::success
@ success