The WorkingMemory component

Introduction

The working memory holds data that is needed for online processing (e.g. localization results and object positions). It can be filled from memory snapshots and store the current data to a snapshot instance (stored in the LongTerm Memory).

The working memory is a SegmentedMemory, which means that the data is organized in segments. By default there are the following segments available:

  • ObjectInstances: This segment holds all object instances that the robot currently is aware of - e.g. objects that have been localized By default the following two fusion methods are instanciated for the ObjectInstances segment:
  • ObjectClasses: All object classes that are currently of interest (e.g. an object localizer is currently searching for them) are stored here.
  • ObjectRelations: Semantic relations between objects (s. memoryx::Relation), e.g. "Cup" isOn "Table" with probability 0.7.

By default, an instance of ObjectLocalizationMemoryUpdater is created and registered. This updater automatically initiates (re-)localization of objects that have been requested, integrates new localization results with former ones and applies the objects' motion models to predict their pose between the localizations.

Configuration

Following properties are supported:

  • UseKalmanFilter (true): Switch fusion with Kalman Filter on/off
  • MatchThreshold (0): Threshold value of spatial proximity between observations to consider them belonging to the same instance (0..1)
  • MatchByClassName (true): Check for class equality when searching for corresponding instance
  • PublishUpdates (true): whether scene updates (ObjectCreated/Updated/Removed) should be published on IceStrom topic
  • UpdatesTopicName (WorkingMemoryUpdates): IceStorm topic name for scene updates
  • UsePriorMemory (true): whether PriorKnowledge should be used
  • PriorMemoryName (PriorKnowledge): PriorKnowledge component name
  • UseLongtermMemory (true): whether longterm memory should be used
  • LongtermMemoryName (LongtermMemory): LongtermMemory component name

See List of Application & Component Properties for an overview of properties.

Quick tutorial

As usual, you need to acquire a proxy first:

#include <MemoryX/interface/components/WorkingMemoryInterface.h>
memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");

Working with object instances

First, get a standard memory segment for object instances:

ObjectInstanceMemorySegmentBasePrx objectInstancesSeg = memoryPrx->getObjectInstancesSegment();

Create an instance with some attributes:

ObjectInstancePtr cup = new ObjectInstance("cup1");
// set position - "standard" attribute
Eigen::Vector3f vec(-5., 2.5, 1.);
FramedPositionBasePtr curPos = new FramedPositionPtr(vec, "");
cup->setPosition(curPos);
// set orientation - "standard" attribute
Eigen::Quaternionf quat(0., 0., 0., 1.);
FramedOrientationBasePtr orient = new FramedOrientation(quat.toRotationMatrix(), "");
cup->setOrientation(orient);
// set "custom" attribute, third parameter is std. deviation
cup->putAttribute("Weight", 0.3f, 0.5);

Object instance can have one or more classes assigned, each with corresponding probability:

cup->addClass("Cup", 0.6);
cup->addClass("Mug", 0.4);

Now, you can send object instance to the working memory:

const std::string cupId = objectInstancesSeg->addEntity(cup);
// NOTE: Due to fusion methods, the remote entity may be different from our local copy
// So we query for the newly added entity
EntityBasePtr fusedCupEntry = objectInstancesSeg->getEntityById(cupId);

Also getObjectInstanceByName() is available.

For new objects, unique ID will be automatically generated and returned by addEntity() function. If you want to update the same object instance later, save this ID and use the updateEntity() function.

To iterate through all the object in the scene, use the following code:

EntityIdList ids = memoryPrx->getAllEntityIds();
for(EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
{
const EntityBasePtr objInstance = objectInstancesSeg->getEntityById(*it);
// process objInstance
}

Finally, to remove an object from memory, call

objectInstancesSeg->removeEntity(cupId);

Localizing objects with the WorkingMemory

When the pose of an object (or a robot hand) is needed, the localization is done by the WorkingMemory. To this end, the memory needs to be advised to localize the object, which we do via the ObjectMemoryObserver.

memoryx::ObjectMemoryObserverInterfacePrx objectMemoryObserverProxy;
objectMemoryObserverProxy = getProxy<memoryx::ObjectMemoryObserverInterfacePrx>("ObjectMemoryObserver");
int cycleTime = 100; // in ms
std::string objectName = "nesquik";
ChannelRefBasePtr objectMemoryChannel = objectMemoryObserverProxy->requestObjectClassRepeated(objectName, cycleTime);

This code causes the memory to retrieve the desired object from the Prior Knowledge, contact the appropriate localizer, and automatically localize the object every 100 ms.

If the object has been localized at least once, its pose is available. It can be obtained from the ObjectMemoryObserver using the ChannelRef we got when we requested the object:

memoryx::ChannelRefBaseSequence instances = objectMemoryObserverProxy->getObjectInstances(objectMemoryChannel);
if (instances.size() == 0)
{
// the object has not been localized yet
}
else if (instances.size() == 1)
{
FramedPositionPtr position = ChannelRefPtr::dynamicCast(instances.front())->get<FramedPosition>("position");
FramedOrientationPtr orientation = ChannelRefPtr::dynamicCast(instances.front())->get<FramedOrientation>("orientation");
}
else
{
// several instances of the object have been found - iterate through the list
}
Simplified sequence diagram for the object localization with Working Memory and ObjectMemoryUpdater

Motion models

Every object instance that is being localized and held in the working memory is associated with a motion model. The default motion model for each object is stored in the PriorKnowledge and can be set via the PriorKnowledgeEditor gui. For most objects, the static model is used which assumes that the object just stays where it is. The robot hands have their own motion model which updates their pose according to the kinematic model while they are not being re-localized.

Another motion model that may be usefull sometimes is the MotionModelAttachedToOtherObject. This model can be used to attach the object to another object, e g. a robot hand when it has been grasped. It's predicted pose is then update by keeping it constant with relation to the object that it has been attached to.

To change the motion model e.g. to the one for attaching the object to a hand, do the following:

#include <MemoryX/interface/components/WorkingMemoryInterface.h>
memoryx::WorkingMemoryInterfacePrx workingMemoryProxy = getProxy<memoryx::WorkingMemoryInterfacePrx>("WorkingMemory");
RobotStateComponentInterfacePrx robotStateComponentProxy = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
std::string objectId = "nesquik1";
std::string handName = "hand_right_armar3a";
ChannelRefPtr handMemoryChannel = objectMemoryObserverProxy->requestObjectClassRepeated(handName, 100);
memoryx::MotionModelAttachedToOtherObjectPtr newMotionModel = new memoryx::MotionModelAttachedToOtherObject(robotStateComponentProxy, handMemoryChannel);
workingMemoryProxy->getObjectInstancesSegment()->setNewMotionModel(objectId, newMotionModel);

Working with object relations

MemoryX supports binary, probabilistic relations between object instances. Please note, that all relations are unidirectional, i.e. not symmetric. Symmetry can be modeled explicitly by adding two relations, one per direction. E.g. "Cup is near the Plate" would be represented as isNear(Cup, Plate) and isNear(Plate, Cup).

The API for working with relations is pretty similar to objects API.

Get a relations segment proxy:

RelationsMemorySegmentBasePrx relationsSeg = memoryPrx->getRelationsSegment();

Creating a relation:

ObjectInstancePtr cup, table;
...
RelationBasePtr relIsOn = new Relation("isOn", cup->getId(), table->getId(), 0.7);

Send a relation to the memory:

const std::string relId = relationsSeg->putEntity(relIsOn);

Get all relations in memory:

EntityIdList ids = relationsSeg->getAllEntityIds();
for(EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
{
const RelationBasePtr rel = relationsSeg->getRelationById(*it);
// process objInstance
}

Get relations of a specific type:

RelationList rels = relationsSeg->getRelationsByName("isOn");
for (RelationList::const_iterator it = rels.begin(); it != rels.end(); ++it)
{
const RelationBasePtr rel = *it;
// process relation
}

Get relations having an object Cup on either side:

RelationList rels = relationsSeg->getRelationsByObject(cup->getId());

Furthermore, getRelationsByFromObject() and getRelationsByToObject() allow to query for relations having an object on the specific side only.

Remove a specific relation from memory:

memoryPrx->removeEntity(relId);

Memory snapshots

An exemplary application that uses the snapshot mechanism can be found in the applications/WorkingMemoryExample folder of the MemoryX sources.

The following code snippet shows how to load a snapshot from the LongtermMemory.

void WorkingMemoryExample::onConnectComponent()
{
// ...
memoryx::WorkingMemoryInterfacePrx memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
// get snapshot name from property definition
const std::string snapshotName = getProperty<std::string>("SnapshotName").getValue();
if (!snapshotName.empty())
{
// load snapshot
longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
ARMARX_INFO << "Snapshot succcessfully loaded: " << snapshotName << flush;
}
else
ARMARX_ERROR << "SnapshotName parameter must be specified!" << flush;
// ...
}

Subscribing for scene updates

Scene updates are published on an IceStorm topic, default name is "WorkingMemoryUpdates". To process them, implement WorkingMemoryObserverInterface and subscribe to the topic as usual.

NOTE: The subscription to the topic will be established after onConnectComponent().

void WorkingMemoryExample::onInitComponent()
{
// ...
usingTopic("WorkingMemoryUpdates");
// ...
}
...
void WorkingMemoryExample::reportObjectCreated(const memoryx::EntityBasePtr& object, const Ice::Current&)
{
// process object
}
armarx::ChannelRefPtr
IceInternal::Handle< ChannelRef > ChannelRefPtr
Definition: ChannelRef.h:41
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:42
armarx::FramedPositionPtr
IceInternal::Handle< FramedPosition > FramedPositionPtr
Definition: FramedPose.h:134
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
memoryx::MotionModelAttachedToOtherObject
Definition: MotionModelAttachedToOtherObject.h:34
MotionModelAttachedToOtherObject.h
armarx::FramedOrientationPtr
IceInternal::Handle< FramedOrientation > FramedOrientationPtr
Definition: FramedPose.h:191
ObjectInstance.h
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::Quaternion< float, 0 >
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39