Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
// STL
#include <iterator>
 
#include <manif/SE3.h>
 
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
 
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
 
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
 
 
namespace armarx::armem::server::robot_state::localization
{
 
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        Base(memoryToIceAdapter, armem::robot_state::localizationSegmentID.coreSegmentName, arondto::Transform::ToAronType(), 1024)
    {
    }
 
 
    Segment::~Segment()
    {
    }
 
    void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        Base::defineProperties(defs, prefix);
 
        defs->optional(properties.predictionTimeWindow,
                       "prediction.TimeWindow",
                       "Duration of time window into the past to use for predictions"
                       " when requested via the PredictingMemoryInterface (in seconds).");
    }
 
    void Segment::init()
    {
        Base::init();
 
        segmentPtr->addPredictor(armem::PredictionEngine{.engineID = "Linear"},
                [this](const PredictionRequest& request){ return this->predictLinear(request); });
    }
 
 
    void Segment::onConnect()
    {
    }
 
 
    RobotFramePoseMap
    Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
    {
        return this->doLocked([this, &timestamp]()
        {
            return getRobotFramePoses(timestamp);
        });
    }
 
 
    RobotFramePoseMap
    Segment::getRobotFramePoses(const armem::Time& timestamp) const
    {
        using ::armarx::armem::robot_state::localization::TransformHelper;
        using ::armarx::armem::robot_state::localization::TransformQuery;
 
        RobotFramePoseMap frames;
        for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
        {
            TransformQuery query
            {
                .header = {
                    .parentFrame = armarx::GlobalFrame,
                    .frame       = ::armarx::armem::robot_state::constants::robotRootNodeName,
                    .agent       = robotName,
                    .timestamp   = timestamp
                }
            };
 
            const auto result = TransformHelper::lookupTransformChain(*segmentPtr, query);
            if (not result)
            {
                // TODO
                continue;
            }
 
            frames.emplace(robotName, result.transforms);
        }
 
        ARMARX_VERBOSE << deactivateSpam(60)
                       << "Number of known robot pose chains: " << frames.size();
 
        return frames;
    }
 
 
    RobotPoseMap
    Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
    {
        return this->doLocked([this, &timestamp]()
        {
            return getRobotGlobalPoses(timestamp);
        });
    }
 
 
    RobotPoseMap
    Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
    {
        using ::armarx::armem::robot_state::localization::TransformHelper;
        using ::armarx::armem::robot_state::localization::TransformQuery;
 
        RobotPoseMap robotGlobalPoses;
        for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
        {
            TransformQuery query
            {
                .header =
                {
                    .parentFrame = GlobalFrame,
                    .frame       = armarx::armem::robot_state::constants::robotRootNodeName,
                    .agent       = robotName,
                    .timestamp   = timestamp
                }
            };
 
            if (const auto result = TransformHelper::lookupTransform(*segmentPtr, query))
            {
                robotGlobalPoses.emplace(robotName, result.transform.transform);
            }
            else
            {
                // TODO
            }
        }
 
        ARMARX_VERBOSE << deactivateSpam(60)
                       << "Number of known robot poses: " << robotGlobalPoses.size();
 
        return robotGlobalPoses;
    }
 
 
    bool Segment::commitTransform(const armarx::armem::robot_state::localization::Transform& transform)
    {
        Commit commit;
        commit.add(makeUpdate(transform));
 
        const armem::CommitResult result = iceMemory.commit(commit);
        return result.allSuccess();
    }
 
 
    bool Segment::commitTransformLocking(const armarx::armem::robot_state::localization::Transform& transform)
    {
        Commit commit;
        commit.add(makeUpdate(transform));
 
        const armem::CommitResult result = iceMemory.commitLocking(commit);
        return result.allSuccess();
    }
 
 
    EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::localization::Transform& transform) const
    {
        const armem::Time& timestamp = transform.header.timestamp;
        const MemoryID providerID = segmentPtr->id().withProviderSegmentName(transform.header.agent);
 
        EntityUpdate update;
        update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
        update.arrivedTime = update.referencedTime = update.sentTime = timestamp;
 
        arondto::Transform aronTransform;
        toAron(aronTransform, transform);
        update.instancesData = {aronTransform.toAron()};
 
        return update;
    }
 
 
    PredictionResult Segment::predictLinear(const PredictionRequest& request)
    {
        PredictionResult result;
        result.snapshotID = request.snapshotID;
        if (request.predictionSettings.predictionEngineID != "Linear")
        {
            result.success = false;
            result.errorMessage = "Prediction engine " +
                                  request.predictionSettings.predictionEngineID +
                                  " is not supported in Proprioception.";
            return result;
        }
 
        static const int tangentDims = 6;
        using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
 
        const DateTime timeOrigin = DateTime::Now();
        const armarx::Duration timeWindow =
            Duration::SecondsDouble(properties.predictionTimeWindow);
        SnapshotRangeInfo<Vector6d, arondto::Transform> info;
 
        doLocked(
            [&, this]()
            {
                info = getSnapshotsInRange<server::wm::CoreSegment, Vector6d, arondto::Transform>(
                    segmentPtr,
                    request.snapshotID,
                    timeOrigin - timeWindow,
                    timeOrigin,
                    [](const aron::data::DictPtr& data)
                    {
                        Eigen::Matrix4d mat =
                            arondto::Transform::FromAron(data).transform.cast<double>();
                        manif::SE3d se3(simox::math::position(mat),
                                        Eigen::Quaterniond(simox::math::orientation(mat)));
                        return se3.log().coeffs();
                    },
                    [](const aron::data::DictPtr& data)
                    { return arondto::Transform::FromAron(data); });
            });
 
        if (info.success)
        {
            Eigen::Matrix4f prediction;
            if (info.timestampsSec.size() <= 1)
            {
                prediction = info.latestValue.transform;
            }
            else
            {
                using simox::math::LinearRegression;
                const bool inputOffset = false;
                const LinearRegression<tangentDims> model = LinearRegression<tangentDims>::Fit(
                    info.timestampsSec, info.values, inputOffset);
                const auto predictionTime = request.snapshotID.timestamp;
                Vector6d linearPred =
                    model.predict((predictionTime - timeOrigin).toSecondsDouble());
                prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
            }
 
            info.latestValue.transform = prediction;
            result.success = true;
            result.prediction = info.latestValue.toAron();
        }
        else
        {
            result.success = false;
            result.errorMessage = info.errorMessage;
        }
 
        return result;
    }
 
} // namespace armarx::armem::server::robot_state::localization