Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <SimoxUtility/math/regression/linear.hpp>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
 
#include "SensorValues.h"
 
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
 
namespace armarx::armem::server::robot_state::proprioception
{
 
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        Base(memoryToIceAdapter,
             armem::robot_state::proprioceptionSegmentID.coreSegmentName,
             arondto::Proprioception::ToAronType(),
             1024)
    {
    }
 
    Segment::~Segment() = default;
 
    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(RobotUnitInterfacePrx robotUnitPrx)
    {
        this->robotUnit = robotUnitPrx;
 
        std::string providerSegmentName = "Robot";
        
        KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
        if (kinematicUnit)
        {
            providerSegmentName = kinematicUnit->getRobotName();
        }
        else
        {
            ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
                           << "\n Falling back to provider segment name '" << providerSegmentName << "'.";
        }
        this->robotUnitProviderID = segmentPtr->id().withProviderSegmentName(providerSegmentName);
    }
 
 
 
    SensorValuesMap Segment::getSensorValuesLocking(
        const armem::Time& timestamp,
        DebugObserverHelper* debugObserver) const
    {
        return doLocked([this, &timestamp, &debugObserver]()
        {
            return getSensorValues(timestamp, debugObserver);
        });
    }
 
 
    static
    aron::data::DictPtr
    getDictElement(const aron::data::Dict& dict, const std::string& key)
    {
        if (dict.hasElement(key))
        {
            return aron::data::Dict::DynamicCastAndCheck(dict.getElement(key));
        }
        return nullptr;
    }
 
 
    SensorValuesMap
    Segment::getSensorValues(
        const armem::Time& timestamp,
        DebugObserverHelper* debugObserver) const
    {
        namespace adn = aron::data;
        ARMARX_CHECK_NOT_NULL(segmentPtr);
 
        SensorValuesMap sensorValues;
        int i = 0;
 
        Duration tFindData = Duration::MilliSeconds(0), tReadSensorValues = Duration::MilliSeconds(0);
        TIMING_START(tProcessEntities)
        segmentPtr->forEachEntity([&](const wm::Entity & entity)
        {
            adn::DictPtr data;
            {
                TIMING_START(_tFindData)
 
                const wm::EntitySnapshot* snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
                if (not snapshot)
                {
                    // Got no snapshot <= timestamp. Take latest instead (if present).
                    snapshot = entity.findLatestSnapshot();
                }
                if (snapshot)
                {
                    data = snapshot->findInstanceData();
                }
 
                TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG);
                tFindData += Duration::MicroSeconds(_tFindData.toMicroSeconds());
            }
            if (data)
            {
                TIMING_START(_tReadSensorValues)
 
                sensorValues.emplace(entity.id().providerSegmentName, readSensorValues(*data));
 
                TIMING_END_COMMENT_STREAM(_tReadSensorValues, "tReadSensorValues " + std::to_string(i), ARMARX_DEBUG)
                tReadSensorValues += Duration::MicroSeconds(_tReadSensorValues.toMicroSeconds());
            }
            ++i;
        });
        TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG)
 
        if (debugObserver)
        {
            debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)", tFindData.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadSensorValues (ms)", tReadSensorValues.toMilliSecondsDouble());
        }
 
        return sensorValues;
    }
 
 
    const armem::MemoryID& Segment::getRobotUnitProviderID() const
    {
        return robotUnitProviderID;
    }
 
    Eigen::VectorXd readJointData(const wm::EntityInstanceData& data)
    {
        namespace adn = aron::data;
 
        std::vector<double> values;
 
        auto addData =
            [&](adn::DictPtr dict) // NOLINT
        {
            for (const auto& [name, value] : dict->getElements())
            {
                values.push_back(
                    static_cast<double>(adn::Float::DynamicCastAndCheck(value)->getValue()));
            }
        };
 
        if (adn::DictPtr joints = getDictElement(data, "joints"))
        {
            if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
            {
                addData(jointsPosition);
            }
            if (adn::DictPtr jointsVelocity = getDictElement(*joints, "velocity"))
            {
                addData(jointsVelocity);
            }
            if (adn::DictPtr jointsTorque = getDictElement(*joints, "torque"))
            {
                addData(jointsTorque);
            }
        }
        Eigen::VectorXd vec =
            Eigen::Map<Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
        return vec;
    }
 
    void
    emplaceJointData(const Eigen::VectorXd& jointData,
                     arondto::Proprioception& dataTemplate)
    {
        Eigen::Index row = 0;
        for (auto& [joint, value] : dataTemplate.joints.position)
        {
            value = static_cast<float>(jointData(row++));
        }
        for (auto& [joint, value] : dataTemplate.joints.velocity)
        {
            value = static_cast<float>(jointData(row++));
        }
        for (auto& [joint, value] : dataTemplate.joints.torque)
        {
            value = static_cast<float>(jointData(row++));
        }
    }
 
    armem::PredictionResult
    Segment::predictLinear(const armem::PredictionRequest& request) const
    {
        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;
        }
 
        const DateTime timeOrigin = DateTime::Now();
        const armarx::Duration timeWindow = Duration::SecondsDouble(properties.predictionTimeWindow);
        SnapshotRangeInfo<Eigen::VectorXd, aron::data::DictPtr> info;
 
        doLocked(
            // Default capture because the number of variables was getting out of hand
            [&, this]()
            {
                info = getSnapshotsInRange<server::wm::CoreSegment,
                                          Eigen::VectorXd,
                                          aron::data::DictPtr>(
                    segmentPtr,
                    request.snapshotID,
                    timeOrigin - timeWindow,
                    timeOrigin,
                    [](const aron::data::DictPtr& data) { return readJointData(*data); },
                    [](const aron::data::DictPtr& data) { return data; });
            });
 
        if (info.success)
        {
            Eigen::VectorXd latestJoints = readJointData(*info.latestValue);
            Eigen::VectorXd prediction(latestJoints.size());
            if (info.timestampsSec.size() <= 1)
            {
                prediction = latestJoints;
            }
            else
            {
                using simox::math::LinearRegression;
                const bool inputOffset = false;
                const LinearRegression model = LinearRegression<Eigen::Dynamic>::Fit(
                    info.timestampsSec, info.values, inputOffset);
                const auto predictionTime = request.snapshotID.timestamp;
                prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
            }
 
            arondto::Proprioception templateData =
                arondto::Proprioception::FromAron(info.latestValue);
            emplaceJointData(prediction, templateData);
            result.success = true;
            result.prediction = templateData.toAron();
        }
        else
        {
            result.success = false;
            result.errorMessage = info.errorMessage;
        }
 
        return result;
    }
 
 
    SensorValues
    Segment::readSensorValues(const wm::EntityInstanceData& data)
    {
        namespace adn = aron::data;
 
        // Just get what we need without casting the whole data.
        SensorValues sensorValues;
        auto checkJVM = [&sensorValues](const std::string& name)
        {
            if (sensorValues.jointValueMap.find(name) == sensorValues.jointValueMap.end())
            {
                sensorValues.jointValueMap[name] = JointValues();
            }
        };
        if (adn::DictPtr joints = getDictElement(data, "joints"))
        {
            if (adn::DictPtr values = getDictElement(*joints, "position"))
            {
                for (const auto& [name, value] : values->getElements())
                {
                    checkJVM(name);
                    sensorValues.jointValueMap[name].position
                        = adn::Float::DynamicCastAndCheck(value)->getValue();
                }
            }
            if (adn::DictPtr values = getDictElement(*joints, "velocity"))
            {
                for (const auto& [name, value] : values->getElements())
                {
                    checkJVM(name);
                    sensorValues.jointValueMap[name].velocity
                        = adn::Float::DynamicCastAndCheck(value)->getValue();
                }
            }
            if (adn::DictPtr values = getDictElement(*joints, "torque"))
            {
                for (const auto& [name, value] : values->getElements())
                {
                    checkJVM(name);
                    sensorValues.jointValueMap[name].velocity
                        = adn::Float::DynamicCastAndCheck(value)->getValue();
                }
            }
        }
        if (adn::DictPtr forceTorqueMap = getDictElement(data, "forceTorque"))
        {
            for (const auto& [name, value] : forceTorqueMap->getElements())
            {
                if (adn::DictPtr forceTorqueValues = aron::data::Dict::DynamicCastAndCheck(value))
                {
                    const Eigen::Vector3f torque
                        = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
                            adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("torque")));
 
                    const Eigen::Vector3f force
                        = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
                            adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("force")));
 
                    sensorValues.forceTorqueValuesMap[name] = ForceTorqueValues
                    {
                        .force = force,
                        .torque = torque
                    };
                }
            }
        }
        return sensorValues;
    }
 
} // namespace armarx::armem::server::robot_state::proprioception