HandControlBase.cpp

Go to the documentation of this file.

#include "HandControlBase.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_controller::core
{
 
    HandControlBase::HandControlBase(const VirtualRobot::RobotPtr& rtRobot,
                                     const HandConfigDict& cfg)
    {
        for (auto& pair : cfg)
        {
            auto handNodeSet = rtRobot->getRobotNodeSet(pair.first);
            hands.emplace(pair.first, std::make_unique<HandData>(handNodeSet, pair.second));
        }
    }
 
    void
    HandControlBase::appendDevice(const std::string& handNodeSet,
                                  const SensorValueBase* sv,
                                  ControlTargetBase* ct,
                                  const std::string& jointName)
    {
        ARMARX_CHECK_EXPRESSION(sv);
        hands.at(handNodeSet)->sensorDevices.append(sv, jointName);
 
        ARMARX_CHECK_EXPRESSION(ct);
        auto* casted_ct = ct->asA<ControlTarget1DoFActuatorPosition>();
        ARMARX_CHECK_EXPRESSION(casted_ct);
        hands.at(handNodeSet)->targets.push_back(casted_ct);
    }
 
    void
    HandControlBase::nonRTSetTarget()
    {
        for (auto& pair : hands)
        {
            auto& hand = pair.second;
            hand->bufferNonRTToRT.getWriteBuffer() = hand->targetNonRT;
            hand->bufferNonRTToRT.commitWrite();
        }
    }
 
    void
    HandControlBase::nonRTUpdateStatus()
    {
        for (auto& pair : hands)
        {
            auto& hand = pair.second;
            hand->targetNonRT = hand->bufferUserToNonRT.getUpToDateReadBuffer();
            hand->rtsInNonRT = hand->bufferRTToNonRT.getUpToDateReadBuffer();
        }
    }
 
    void
    HandControlBase::updateRTStatus(const double deltaT)
    {
        for (auto& pair : hands)
        {
            pair.second->rts.deltaT = deltaT;
            pair.second->rts.accumulateTime += deltaT;
            pair.second->sensorDevices.getJointAngles(pair.second->rts.jointPosition.value());
            pair.second->bufferRTToNonRT.getWriteBuffer() = pair.second->rts;
            pair.second->bufferRTToNonRT.commitWrite();
        }
    }
 
    void
    HandControlBase::setTargets()
    {
        for (auto& pair : hands)
        {
            pair.second->setTarget();
        }
    }
 
    void
    HandControlBase::updateConfig(const HandConfigDict& cfg)
    {
        for (auto& pair : cfg)
        {
            if (pair.second.jointPosition.has_value())
            {
                auto& target = hands.at(pair.first)->bufferUserToNonRT.getWriteBuffer();
                target.jointPosition = pair.second.jointPosition;
                hands.at(pair.first)->bufferUserToNonRT.commitWrite();
            }
        }
    }
 
    void
    HandControlBase::getConfig(HandConfigDict& cfgToUpdate)
    {
        for (auto& pair : hands)
        {
            cfgToUpdate.at(pair.first) = pair.second->bufferNonRTToRT.getUpToDateReadBuffer();
        }
    }
 
    void
    HandControlBase::reinitBuffer(HandConfigDict& cfgToUpdate)
    {
        for (auto& pair : hands)
        {
            cfgToUpdate.at(pair.first) = pair.second->targetNonRT;
            pair.second->bufferNonRTToRT.reinitAllBuffers(pair.second->targetNonRT);
            pair.second->bufferUserToNonRT.reinitAllBuffers(pair.second->targetNonRT);
        }
    }
 
    void
    HandControlBase::limbPublish(HandPtr& hand, const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto rtData = hand->bufferConfigRTToOnPublish.getUpToDateReadBuffer();
 
        if (rtData.jointPosition.has_value())
        {
            common::debugEigenVec(datafields, "kpImpedance", rtData.jointPosition.value());
            debugObs->setDebugChannel("hand_" + hand->kinematicChainName, datafields);
        }
    }
 
    void
    HandControlBase::onPublish(const DebugObserverInterfacePrx& debugObs)
    {
        for (auto& pair : hands)
        {
            limbPublish(pair.second, debugObs);
        }
    }
 
    void
    HandControlBase::rtPreActivate()
    {
        for (auto& pair : hands)
        {
            ARMARX_INFO << "rtPreActivate for hand " << pair.first;
            pair.second->rtPreActivate();
        }
    }
 
    void
    HandControlBase::rtPostDeactivate()
    {
    }
 
    /// ------------------------------------- HandData ---------------------------------------------
    void
    HandControlBase::HandData::validate()
    {
        ARMARX_CHECK_EQUAL(targetRT.jointPosition.value().size(), jointLimMin.size());
        ARMARX_CHECK_EQUAL(targetRT.jointPosition.value().size(), jointLimMax.size());
        targetRT.jointPosition.value() =
            targetRT.jointPosition.value().cwiseMax(jointLimMin).cwiseMin(jointLimMax);
    }
 
    void
    HandControlBase::HandData::setTarget()
    {
        targetRT = bufferNonRTToRT.getUpToDateReadBuffer();
        if (targetRT.jointPosition.has_value())
        {
            validate();
            for (size_t i = 0; i < targets.size(); i++)
            {
                targets[i]->position = targetRT.jointPosition.value()(i);
            }
        }
        bufferConfigRTToOnPublish.getWriteBuffer() = targetRT;
        bufferConfigRTToOnPublish.commitWrite();
    }
 
    HandControlBase::HandData::HandData(const VirtualRobot::RobotNodeSetPtr& robotNodeSet,
                                        const HandConfig& cfg) :
        kinematicChainName(robotNodeSet->getName()),
        jointNames(robotNodeSet->getNodeNames()),
        nJoints(robotNodeSet->getSize()),
        targetRT(cfg),
        targetNonRT(cfg)
    {
        rts.deltaT = 0.0;
        rts.jointPosition = Eigen::VectorXf::Zero(nJoints);
        rtsInNonRT = rts;
        jointLimMin = Eigen::Map<Eigen::VectorXf>(robotNodeSet->getNodeLimitsLo().data(), nJoints);
        jointLimMax = Eigen::Map<Eigen::VectorXf>(robotNodeSet->getNodeLimitsHi().data(), nJoints);
        ARMARX_CHECK_EQUAL(static_cast<size_t>(jointLimMin.size()), nJoints);
        ARMARX_CHECK_EQUAL(static_cast<size_t>(jointLimMax.size()), nJoints);
    }
 
    void
    HandControlBase::HandData::rtPreActivate()
    {
        ARMARX_CHECK_EQUAL(sensorDevices.positionSensors.size(), nJoints)
            << "In your class constructor, did you forget to call HandControlBase::appendDevice( "
               "... )?";
        ARMARX_CHECK_EQUAL(targets.size(), nJoints);
        sensorDevices.getJointAngles(rts.jointPosition.value());
        rts.jointPosition.value() =
            rts.jointPosition.value().cwiseMax(jointLimMin).cwiseMin(jointLimMax);
 
        if (not targetRT.jointPosition.has_value())
        {
            targetRT.jointPosition = rts.jointPosition;
            targetNonRT = targetRT;
        }
        rtsInNonRT = rts;
        nonRTAccumulateTime = 0.0;
        nonRTDeltaT = 0.0;
        ARMARX_CHECK_EQUAL(static_cast<size_t>(targetRT.jointPosition.value().size()), nJoints);
        bufferNonRTToRT.reinitAllBuffers(targetRT);
        bufferRTToNonRT.reinitAllBuffers(rts);
        bufferUserToNonRT.reinitAllBuffers(targetRT);
        bufferRTToUser.reinitAllBuffers(targetRT);
        bufferConfigRTToOnPublish.reinitAllBuffers(targetRT);
        ARMARX_INFO_S << kinematicChainName << " joints " << targetRT.jointPosition.value();
    }
 
 
} // namespace armarx::control::njoint_controller::core