Motion Models

Table of Contents

• Introduction
• Creating your own motion model
• Changing the motion model of an object

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:

   #include "AbstractMotionModel.h"
    
   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>
   friend class armarx::GenericFactory;
   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>
#include <MemoryX/libraries/motionmodels/MotionModelAttachedToOtherObject.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);