HandControlBase.cpp

Go to the documentation of this file.

#include "HandControlBase.h"
 
#include <VirtualRobot/RobotNodeSet.h> // IWYU pragma: keep
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_controller::core
{
 
    HandControlBase::HandControlBase(const RobotUnitPtr& robotUnit,
                                     const VirtualRobot::RobotPtr& rtRobot,
                                     const HandConfigDict& cfg)
    {
        for (const auto& pair : cfg)
        {
            auto handNodeSet = rtRobot->getRobotNodeSet(pair.first);
            hands.emplace(pair.first,
                          std::make_unique<HandData>(robotUnit, handNodeSet, pair.second));
        }
    }
 
    void
    HandControlBase::nonRTSetTarget()
    {
        for (auto& pair : hands)
        {
            pair.second->setTarget();
        }
    }
 
    void
    HandControlBase::nonRTUpdateStatus()
    {
        for (auto& [name, hand] : hands)
        {
            hand->targetNonRT = hand->bufferUserToNonRT.getUpToDateReadBuffer();
            hand->rtsInNonRT = hand->bufferRTToNonRT.getUpToDateReadBuffer();
        }
    }
 
    void
    HandControlBase::updateRTStatus(double deltaT)
    {
        for (auto& [_, hand] : hands)
        {
            hand->rts.deltaT = deltaT;
            hand->rts.accumulateTime += deltaT;
            hand->bufferRTToNonRT.getWriteBuffer() = hand->rts;
            hand->bufferRTToNonRT.commitWrite();
        }
    }
 
    void
    HandControlBase::updateConfig(const HandConfigDict& cfg)
    {
        for (const auto& [name, hand_cfg] : cfg)
        {
            if (hand_cfg.jointPosition.has_value())
            {
                auto& target = hands.at(name)->bufferUserToNonRT.getWriteBuffer();
                target.jointPosition = hand_cfg.jointPosition;
                hands.at(name)->bufferUserToNonRT.commitWrite();
            }
        }
    }
 
    void
    HandControlBase::getConfig(HandConfigDict& cfgToUpdate)
    {
        for (auto& [name, hand] : hands)
        {
            cfgToUpdate.at(name) = hand->bufferConfigNonRTToUser.getUpToDateReadBuffer();
        }
    }
 
    void
    HandControlBase::reinitBuffer(HandConfigDict& cfgToUpdate)
    {
        for (auto& [name, hand] : hands)
        {
            cfgToUpdate.at(name) = hand->targetNonRT;
            hand->bufferUserToNonRT.reinitAllBuffers(hand->targetNonRT);
            hand->bufferConfigNonRTToOnPublish.reinitAllBuffers(hand->targetNonRT);
        }
    }
 
    void
    HandControlBase::limbPublish(HandPtr& hand, const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto rtData = hand->bufferConfigNonRTToOnPublish.getUpToDateReadBuffer();
 
        if (rtData.jointPosition.has_value())
        {
            common::debugEigenVec(datafields, "jointPosition", 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::nonRtActivateController()
    {
        for (auto& pair : hands)
        {
            pair.second->ctrl->activateController();
        }
    }
 
    void
    HandControlBase::rtPreActivate()
    {
        for (auto& pair : hands)
        {
            pair.second->rtPreActivate();
        }
    }
 
    void
    HandControlBase::rtPostDeactivate(HandConfigDict& cfgToUpdate)
    {
        // The controller should not be deactivated
        getConfig(cfgToUpdate);
    }
 
    /// ------------------------------------- HandData ---------------------------------------------
    void
    HandControlBase::HandData::validate()
    {
        ARMARX_CHECK_EQUAL(targetNonRT.jointPosition.value().size(), jointLimMin.size());
        ARMARX_CHECK_EQUAL(targetNonRT.jointPosition.value().size(), jointLimMax.size());
 
        targetNonRT.jointPosition.value() =
            targetNonRT.jointPosition.value().cwiseMax(jointLimMin).cwiseMin(jointLimMax);
    }
 
    void
    HandControlBase::HandData::setTarget()
    {
        if (targetNonRT.jointPosition.has_value())
        {
            validate();
 
            if (not isTargetChanged())
            {
                return;
            }
            for (unsigned int i = 0; i < jointNames.size(); i++)
            {
                desiredJointPositionMap.at(jointNames[i]) = targetNonRT.jointPosition.value()(i);
            }
        }
        ctrl->setTargetsMap(desiredJointPositionMap);
 
        bufferConfigNonRTToOnPublish.getWriteBuffer() = targetNonRT;
        bufferConfigNonRTToOnPublish.commitWrite();
 
        bufferConfigNonRTToUser.getWriteBuffer() = targetNonRT;
        bufferConfigNonRTToUser.commitWrite();
    }
 
    bool
    HandControlBase::HandData::isTargetChanged()
    {
        bool changed =
            not(targetNonRT.jointPosition.value() - prevTargetNonRT.jointPosition.value())
                   .isZero(deltaJointValueThreshold);
        if (changed)
        {
            prevTargetNonRT = targetNonRT;
        }
        return changed;
    }
 
    HandControlBase::HandData::HandData(const RobotUnitPtr& robotUnit,
                                        const VirtualRobot::RobotNodeSetPtr& robotNodeSet,
                                        const HandConfig& cfg) :
        kinematicChainName(robotNodeSet->getName()),
        jointNames(robotNodeSet->getNodeNames()),
        nJoints(robotNodeSet->getSize())
    {
        auto controller = robotUnit->getNJointController(cfg.njointControllerName);
        ARMARX_CHECK_NOT_NULL(controller) << "Hand controller " << cfg.njointControllerName
                                          << " is not available in "
                                             "robot Unit";
        ctrl = Ice::checkedCast<hand::HandShapeControllerInterfacePrx>(controller);
        ARMARX_CHECK_NOT_NULL(ctrl);
 
        targetNonRT = cfg;
        rts.deltaT = 0.0;
        rts.accumulateTime = 0.0;
        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);
 
        for (auto& name : jointNames)
        {
            if (name.find(kinematicChainName) == 0)
            {
                name.erase(0, kinematicChainName.length());
            }
            ARMARX_INFO << "-- " << kinematicChainName << " adding joint: " << name;
            desiredJointPositionMap.emplace(name, 0.0f);
        }
    }
 
    void
    HandControlBase::HandData::rtPreActivate()
    {
        ARMARX_RT_LOGF_INFO("--> rtPreActivate for hand: %s", kinematicChainName.c_str());
 
        rtsInNonRT = rts;
        desiredJointPositionMap = ctrl->getJointValuesMap();
        if (not targetNonRT.jointPosition.has_value())
        {
            targetNonRT.jointPosition = Eigen::VectorXf::Zero(jointNames.size());
        }
 
        for (auto i = 0; i < targetNonRT.jointPosition.value().size(); ++i)
        {
            if (desiredJointPositionMap.count(jointNames[i]) > 0)
            {
                targetNonRT.jointPosition.value()(i) = desiredJointPositionMap.at(jointNames[i]);
            }
            else
            {
                ARMARX_ERROR << "Inconsistent hand joint: " << jointNames[i]
                             << " does not exist in hand shape controller";
            }
        }
        prevTargetNonRT = targetNonRT;
        nonRTAccumulateTime = 0.0;
        nonRTDeltaT = 0.0;
        ARMARX_CHECK_EQUAL(static_cast<size_t>(targetNonRT.jointPosition.value().size()), nJoints);
 
        bufferRTToNonRT.reinitAllBuffers(rts);
        bufferUserToNonRT.reinitAllBuffers(targetNonRT);
        bufferConfigNonRTToOnPublish.reinitAllBuffers(targetNonRT);
        bufferConfigNonRTToUser.reinitAllBuffers(targetNonRT);
    }
 
 
} // namespace armarx::control::njoint_controller::core