AbstractMotionModel.cpp

Go to the documentation of this file.

 
#include "AbstractMotionModel.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
memoryx::AbstractMotionModel::AbstractMotionModel(
    armarx::RobotStateComponentInterfacePrx robotStateProxy)
{
    std::unique_lock lock(motionPredictionLock);
 
    this->robotStateProxy = robotStateProxy;
    poseAtLastLocalization = new armarx::LinkedPose();
    predictedPoseAtStartOfCurrentLocalization = nullptr;
    globalRobotPoseAtLastLocalization = nullptr;
    uncertaintyAtLastLocalization = nullptr;
    uncertaintyAtStartOfCurrentLocalization = nullptr;
    timeOfLastLocalizationStart = new armarx::TimestampVariant(IceUtil::Time::now());
    timeOfLastSuccessfulLocalization = new armarx::TimestampVariant(IceUtil::Time::now());
}
 
armarx::LinkedPoseBasePtr
memoryx::AbstractMotionModel::getPredictedPose(const Ice::Current&)
{
    std::unique_lock lock(motionPredictionLock);
 
    armarx::LinkedPosePtr ret = getPredictedPoseInternal();
    return ret;
}
 
memoryx::MultivariateNormalDistributionBasePtr
memoryx::AbstractMotionModel::getUncertainty(const Ice::Current&)
{
    std::unique_lock lock(motionPredictionLock);
 
    MultivariateNormalDistributionBasePtr ret = getUncertaintyInternal();
    return ret;
}
 
void
memoryx::AbstractMotionModel::setPoseAtLastLocalisation(
    const armarx::LinkedPoseBasePtr& poseAtLastLocalization,
    const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization,
    const memoryx::MultivariateNormalDistributionBasePtr& uncertaintyAtLastLocalization,
    const Ice::Current&)
{
    std::unique_lock lock(motionPredictionLock);
    this->poseAtLastLocalization = poseAtLastLocalization;
    timeOfLastSuccessfulLocalization =
        armarx::TimestampVariantPtr::dynamicCast(timeOfLastLocalizationStart->clone());
 
    if (globalRobotPoseAtLastLocalization)
    {
        this->globalRobotPoseAtLastLocalization = globalRobotPoseAtLastLocalization;
    }
 
    if (uncertaintyAtLastLocalization)
    {
        this->uncertaintyAtLastLocalization = uncertaintyAtLastLocalization;
    }
}
 
void
memoryx::AbstractMotionModel::savePredictedPoseAtStartOfCurrentLocalization(const Ice::Current&)
{
    std::unique_lock lock(motionPredictionLock);
 
    predictedPoseAtStartOfCurrentLocalization = getPredictedPoseInternal();
    uncertaintyAtStartOfCurrentLocalization = getUncertaintyInternal();
    timeOfLastLocalizationStart = new armarx::TimestampVariant(IceUtil::Time::now());
}
 
armarx::LinkedPoseBasePtr
memoryx::AbstractMotionModel::getPredictedPoseAtStartOfCurrentLocalization(const Ice::Current&)
{
    return predictedPoseAtStartOfCurrentLocalization;
}
 
memoryx::MultivariateNormalDistributionBasePtr
memoryx::AbstractMotionModel::getUncertaintyAtStartOfCurrentLocalization(const Ice::Current&)
{
    return uncertaintyAtStartOfCurrentLocalization;
}
 
std::string
memoryx::AbstractMotionModel::getMotionModelName()
{
    return ice_id();
}
 
memoryx::AbstractMotionModel::EMotionModelType
memoryx::AbstractMotionModel::getMotionModelTypeByName(std::string motionModelName)
{
    if (motionModelName.compare("Static") == 0)
    {
        return eMotionModelStaticObject;
    }
    else if (motionModelName.compare("RobotHand") == 0)
    {
        return eMotionModelRobotHand;
    }
    else if (motionModelName.compare("AttachedToOtherObject") == 0)
    {
        return eMotionModelAttachedToOtherObject;
    }
    else if (motionModelName.compare("KBM") == 0)
    {
        return eMotionModelKBM;
    }
    else if (motionModelName.empty())
    {
        ARMARX_WARNING_S << "Motion model name is empty!";
        return eMotionModelStaticObject;
    }
    else
    {
        ARMARX_WARNING_S << "Unknown motion model: " << motionModelName;
        return eMotionModelStaticObject;
    }
}
 
armarx::LinkedPoseBasePtr
memoryx::AbstractMotionModel::getPoseAtLastLocalisation(const Ice::Current&)
{
    std::unique_lock lock(motionPredictionLock);
 
    armarx::LinkedPoseBasePtr ret = poseAtLastLocalization;
    return ret;
}
 
memoryx::AbstractMotionModel::AbstractMotionModel()
{
}