Motion Models

Introduction

Every object instance that is being localized and held in the working memory is associated with a motion model. The motion model predicts the pose of the object after it has been localized, i.e. it fills the time gaps between the localizations and updates the object's pose while it is not in the field of view.

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 updated by keeping it constant with relation to the object that it has been attached to.

Creating your own motion model

To create a new kind of motion model, follow these steps.

  1. Create an Ice interface in the file MemoryX/source/MemoryX/interface/workingmemory/MotionModel.ice.
  2. In the file MemoryX/source/MemoryX/libraries/motionmodels/AbstractMotionModel.h, add your new type in the list
    enum EMotionModelType
    {
    ....
    eMotionModelMyNewType
    }
    and in the function
    static EMotionModelType getMotionModelTypeByName(std::string motionModelName)
    add a line
    else if (motionModelName.compare("MyNewType") == 0) return eMotionModelMyNewType;
  3. Create a new .h and .cpp file in the directory MemoryX/source/MemoryX/libraries/motionmodels. Your class has to inherit from AbstractMotionModel and your Ice interface:
    namespace memoryx
    {
    class MotionModelMyNewType :
    virtual public AbstractMotionModel,
    virtual public MotionModelMyNewTypeBase
    {
    public:
    virtual AbstractMotionModel::EMotionModelType getMotionModelType() { return AbstractMotionModel::eMotionModelMyNewType; }
    protected:
    virtual armarx::LinkedPosePtr getPredictedPoseInternal();
    };
    typedef IceInternal::Handle<MotionModelMyNewType> MotionModelMyNewTypePtr;
    }
    You have to implement (at least) the function getPredictedPoseInternal() which returns the predicted pose of the object using your prediction method.
  4. In order to allow for your motion model to be sent via Ice, all member variables have to be declared in the Slice file. Everything you declare in the .h file will be lost when the model is sent to or from an Ice proxy. In your .h file, add these lines in the protected section:
    // for the object factory
    template <class IceBaseClass, class DerivedClass>
    MotionModelMyNewType() { }
  5. In the file MotionModelObjectFactories.h, include your .h file and add your new motion model to the map by adding a line to the function armarx::ObjectFactoryMap getFactories():
    add<MotionModelMyNewTypeBase, MotionModelMyNewType>(map);
  6. Don't forget to add your new files to the CMakeLists.txt.

If your motion model needs to be sendable via Ice, e.g. when you want to exchange it at runtime, you need to follow all steps. If your model will never be changed, it is sufficient to do steps 2, 3 and 6. In case anyone ever tries to send it over Ice then, that will cause an exception at runtime that saying that there is no ObjectFactory.

Changing the motion model of an object

You can change the motion model of an object that is already being localized at runtime. To switch it 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);
armarx::ChannelRefPtr
IceInternal::Handle< ChannelRef > ChannelRefPtr
Definition: ChannelRef.h:41
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
IceInternal::Handle
Definition: forward_declarations.h:8
memoryx::MotionModelAttachedToOtherObject
Definition: MotionModelAttachedToOtherObject.h:34
MotionModelAttachedToOtherObject.h
AbstractMotionModel.h
armarx::GenericFactory
Definition: FactoryCollectionBase.h:51
memoryx::AbstractMotionModel::EMotionModelType
EMotionModelType
Definition: AbstractMotionModel.h:42