ConverterTools.cpp

Go to the documentation of this file.

#include "ConverterTools.h"
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/advanced.h>
 
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
 
namespace armarx::armem::server::robot_state
{
 
    std::optional<std::string>
    proprioception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
    {
        for (const auto& prefix : prefixes)
        {
            if (simox::alg::starts_with(key, prefix))
            {
                return prefix;
            }
        }
        return std::nullopt;
    }
}
 
 
namespace armarx::armem::server::robot_state::proprioception
{
    ConverterTools::ConverterTools()
    {
        {
            vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
            {
                v.x() = f;
            };
            vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
            {
                v.y() = f;
            };
            vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
            {
                v.z() = f;
            };
            vector3fSetters["x"] = vector3fSetters["X"];
            vector3fSetters["y"] = vector3fSetters["Y"];
            vector3fSetters["z"] = vector3fSetters["Z"];
            vector3fSetters["Rotation"] = vector3fSetters["Z"];
        }
        {
            platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
            {
                return &p.acceleration;
            };
            platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
            {
                return &p.relativePosition;
            };
            platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
            {
                return &p.velocity;
            };
            platformIgnored.insert("absolutePosition");
        }
        {
            ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
            {
                return &ft.gravityCompensationForce;
            };
            ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
            {
                return &ft.gravityCompensationTorque;
            };
            ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
            {
                return &ft.force;
            };
            ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
            {
                return &ft.torque;
            };
        }
        {
            jointGetters["acceleration"] = [](prop::arondto::Joints & j)
            {
                return &j.acceleration;
            };
            jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
            {
                return &j.gravityTorque;
            };
            jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
            {
                return &j.inertiaTorque;
            };
            jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
            {
                return &j.inverseDynamicsTorque;
            };
            jointGetters["position"] = [](prop::arondto::Joints & j)
            {
                return &j.position;
            };
            jointGetters["torque"] = [](prop::arondto::Joints & j)
            {
                return &j.torque;
            };
            jointGetters["velocity"] = [](prop::arondto::Joints & j)
            {
                return &j.velocity;
            };
        }
        {
 
#define ADD_SCALAR_SETTER(container, name, type) \
    container [ #name ] = []( \
                              prop::arondto::Joints & dto, \
                              const std::vector<std::string>& split, \
                              const ConverterValue & value) \
    { \
        dto. name [split.at(1)] = getValueAs< type >(value); \
    }
 
            ADD_SCALAR_SETTER(jointSetters, position, float);
            ADD_SCALAR_SETTER(jointSetters, velocity, float);
            ADD_SCALAR_SETTER(jointSetters, acceleration, float);
 
            ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
            ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
 
            ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
            ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
            ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
 
            ADD_SCALAR_SETTER(jointSetters, torque, float);
            ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
            ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
            ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
            ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
            ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
 
            ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
            ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
            ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
 
            // "temperature" handled below
            // ADD_SCALAR_SETTER(jointSetters, temperature, float);
 
            ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
            ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
 
            ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
            ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
 
            ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
            ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
            ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
            ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
 
 
#define ADD_VECTOR3_SETTER(container, name, type) \
    container [ #name ] = [this]( \
                                  prop::arondto::Joints & dto, \
                                  const std::vector<std::string>& split, \
                                  const ConverterValue & value) \
    { \
        auto& vec = dto. name [split.at(1)]; \
        auto& setter = this->vector3fSetters.at(split.at(3)); \
        setter(vec, getValueAs< type >(value)); \
    }
 
            ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
            ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
 
            // ADD_GETTER(jointVectorGetters, orientation, float);
        }
    }
 
}