device.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 
namespace armarx::control::common
{
 
    struct SensorDevicesForNJointTorqueController
    {
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
 
        void
        updateJointValues(Eigen::VectorXf& qpos, Eigen::VectorXf& qvel, Eigen::VectorXf& qtor)
        {
            for (size_t i = 0; i < positionSensors.size(); ++i)
            {
                qpos(i) = positionSensors[i]->position;
            }
            for (size_t i = 0; i < velocitySensors.size(); ++i)
            {
                qvel(i) = velocitySensors[i]->velocity;
            }
            for (size_t i = 0; i < torqueSensors.size(); ++i)
            {
                qtor(i) = torqueSensors[i]->torque;
            }
        };
 
        void
        getJointAngles(Eigen::VectorXf& qpos)
        {
            for (size_t i = 0; i < positionSensors.size(); ++i)
            {
                if (positionSensors[i])
                    qpos(i) = positionSensors[i]->position;
            }
        };
 
        void
        append(const SensorValueBase* sv, const std::string& jointName)
        {
            ARMARX_CHECK_EXPRESSION(sv);
 
            const auto* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
            const auto* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
            const auto* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (torqueSensor)
                torqueSensors.push_back(torqueSensor);
            if (velocitySensor)
                velocitySensors.push_back(velocitySensor);
            if (positionSensor)
                positionSensors.push_back(positionSensor);
            //            if (torqueSensor == nullptr)
            //            {
            //                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            //            }
            //            if (velocitySensor == nullptr)
            //            {
            //                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            //            }
            //            if (positionSensor == nullptr)
            //            {
            //                ARMARX_WARNING << "No position sensor available for " << jointName;
            //            }
        };
    };
 
} // namespace armarx::control::common