Template.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ...
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <ArmarXCore/core/time/Clock.h>
 
#include <armarx/control/common/mp/JSMP.h>
#include <armarx/control/common/mp/MPPool.h>
#include <armarx/control/common/mp/TSMP.h>
#include <armarx/control/common/utils.h>
#include <armarx/control/njoint_controller/task_space/Base.h>
#include <armarx/control/njoint_mp_controller/task_space/ControllerInterface.h>
 
namespace armarx::control::njoint_mp_controller::task_space
{
    //    using namespace armarx::control::njoint_controller::task_space;
    //    using namespace armarx::control::common;
    namespace common = armarx::control::common;
 
    template <typename TSControllerType, typename MPInterfaceType>
    class TSMPController :
        public TSControllerType,
        public common::mp::MPPool,
        virtual public MPInterfaceType
    {
    public:
        using MPListConfig = common::mp::arondto::MPListConfig;
 
        TSMPController(const RobotUnitPtr& robotUnit,
                       const NJointControllerConfigPtr& config,
                       const VirtualRobot::RobotPtr& /*robot*/);
 
        void updateMPConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                            const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
        void additionalTask() override;
        void onPublish(const SensorAndControl& /*sac*/,
                       const DebugDrawerInterfacePrx& /*interface*/,
                       const DebugObserverInterfacePrx& /*debugObs*/) override;
 
        void rtPreActivateController() override;
 
        struct MPDebugData
        {
            float canonicalValue;
            std::map<std::string, Eigen::Matrix4f> desiredPose;
        };
 
        TripleBuffer<MPDebugData> bufferMPToOnPublish;
 
    private:
        std::map<common::mp::arondto::MPGroup, bool> groupDeviationFlag_;
        void updateNonRTTiming();
        std::map<std::string, bool> manageMPStopsAndGuards();
        void updateMPInputs(common::mp::MPInputOutput& mpData);
        void processGroupSafety(common::mp::MPInputOutput& mpData, MPDebugData& buffer);
        void
        processMPOutput(common::mp::MPInputOutput& mpData, MPDebugData& buffer, bool rtTargetSafe);
    };
 
    ///// -------------------------------- Implementation --------------------------------
    template <typename TSControllerType, typename MPInterfaceType>
    TSMPController<TSControllerType, MPInterfaceType>::TSMPController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        TSControllerType{robotUnit, config, robot}, common::mp::MPPool{}
    {
        auto data = MPDebugData();
        data.canonicalValue = 1.0;
        bufferMPToOnPublish.reinitAllBuffers(data);
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::rtPreActivateController()
    {
        TSControllerType::rtPreActivateController();
 
        if (not isFinished("all") and (not this->rtGetDesiredPoseSafeStatusOnActivation() or
                                       not this->rtGetDesiredPoseSafeStatus()))
        {
            ARMARX_WARNING << "Your MPs are not terminated before controller de-/re-activation, "
                              "and the controller is not in a safe status regarding desried pose. "
                              "either your MP has an improper desired pose, or "
                              "you interruped the controller with a different controller. "
                              "No worry, this is just a warning. I will terminate all running MPs.";
            requestStop("all");
        }
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::onPublish(
        const SensorAndControl& sac,
        const DebugDrawerInterfacePrx& interface,
        const DebugObserverInterfacePrx& debugObs)
    {
        TSControllerType::onPublish(sac, interface, debugObs);
 
        StringVariantBaseMap datafields;
        auto& data = bufferMPToOnPublish.getUpToDateReadBuffer();
        datafields["canonicalValue"] = new Variant(data.canonicalValue);
 
        auto& desiredPoses = data.desiredPose;
        for (const auto& [name, pose] : desiredPoses)
        {
            common::debugEigenPose(datafields, "mp.desired_pose_" + name, pose);
        }
        debugObs->setDebugChannel("MP", datafields);
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::updateMPConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& /*iceCurrent*/)
    {
        if (not isFinished("all"))
        {
            ARMARX_WARNING
                << "All running MPs will be terminated. This is only a temporal solution, most "
                   "likely your previous mp get into a un-safe status and in your high-level "
                   "application, you are not checking and using these status. A proper way to "
                   "terminate the mps is to get the ctrl proxy and call requestStop('all'). In "
                   "theory, controller should not handle this, but to save your time now, I will "
                   "force to remove the existing mp regardless and create new MPs on your request";
            requestStop("all");
            while (not isFinished("all"))
            {
                armarx::Clock::WaitFor(armarx::Duration::SecondsDouble(0.01));
            }
            ARMARX_INFO << "-- all mps stopped";
        }
        if (resetMPsV1(dto, this->controllableNodeSets, this->coordinator))
        {
            for (auto& [_, arm] : this->limb)
            {
                arm->rtFirstRun.store(true);
            }
        }
        {
            const std::scoped_lock lock(mtx_mps);
            auto& buffer = bufferMPToOnPublish.getWriteBuffer();
            buffer.desiredPose.clear();
            ARMARX_VERBOSE << " -- Debug message: reset MP debug buffer " << mps.size();
            for (const auto& mp : mps)
            {
                if (mp.second.mp->getRole() == "taskspace")
                {
                    buffer.desiredPose.emplace(mp.second.mp->getNodeSetName(),
                                               Eigen::Matrix4f::Identity());
                    ARMARX_VERBOSE << "reset mp debug message: " << mp.second.mp->getNodeSetName()
                                   << " to identity";
                }
            }
            bufferMPToOnPublish.reinitAllBuffers(buffer);
        }
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::additionalTask()
    {
        const std::lock_guard<std::recursive_mutex> lock(mtx_mps);
 
        /// 1. Update Status from RT and user configuration update
        auto [rtTargetSafe, ftSafe] = this->additionalTaskUpdateStatus();
        if (this->coordinator)
        {
            this->coordinator->updateNonRt();
        }
 
        ARMARX_TRACE;
        /// 2. Update Timing calculations
        this->updateNonRTTiming();
 
        /// 3. Check Stop Conditions
        std::map<std::string, bool> mpRunningStatus = this->manageMPStopsAndGuards();
 
        /// 4. Prepare Inputs
        for (auto& mp : mps)
        {
            if (mpRunningStatus[mp.second.mp->getMPName()] && !mp.second.mp->isFinished())
            {
                this->updateMPInputs(mp.second);
            }
        }
 
        /// 5. Handle RT unsafe logging due to descripancy in desired and current pose
        if (!rtTargetSafe)
        {
            this->handleRTNotSafeInNonRT();
        }
 
        /// 6. The MPs will envolve one step forward if it is the first run or rtTargetSafe is true
        runMPs(rtTargetSafe);
 
        /// 7. Set mp target to nonRT config data structure
        auto& buffer = bufferMPToOnPublish.getWriteBuffer();
 
        groupDeviationFlag_.clear();
        for (auto& mpWrapper : mps)
        {
            auto& mpData = mpWrapper.second;
 
            // Only process if the MP is marked as running
            if (mpRunningStatus[mpData.mp->getMPName()])
            {
                this->processGroupSafety(mpData, buffer);
            }
        }
 
        for (auto& mpWrapper : mps)
        {
            auto& mpData = mpWrapper.second;
 
            // Only process if the MP is marked as running
            if (mpRunningStatus[mpData.mp->getMPName()])
            {
                this->processMPOutput(mpData, buffer, rtTargetSafe);
            }
        }
 
        /// 8. Handle coordinator and set/commit non RT targets/buffer
        if (this->coordinator)
        {
            this->coordinator->commitNonRt();
        }
 
        this->additionalTaskSetTarget();
        bufferMPToOnPublish.commitWrite();
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::updateNonRTTiming()
    {
        for (auto& [name, arm] : this->limb)
        {
            arm->nonRTDeltaT = arm->rtStatusInNonRT.accumulateTime - arm->nonRTAccumulateTime;
            arm->nonRTAccumulateTime = arm->rtStatusInNonRT.accumulateTime;
        }
        if (this->hands)
        {
            for (auto& [_, hand] : this->hands->hands)
            {
                hand->nonRTDeltaT = hand->rtsInNonRT.accumulateTime - hand->nonRTAccumulateTime;
                hand->nonRTAccumulateTime = hand->rtsInNonRT.accumulateTime;
            }
        }
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    std::map<std::string, bool>
    TSMPController<TSControllerType, MPInterfaceType>::manageMPStopsAndGuards()
    {
        std::map<std::string, bool> mpRunning;
 
        // Check global FT safety per limb
        std::map<std::string, bool> limbStopFlags;
        for (auto& [name, arm] : this->limb)
        {
            //          limbStopFlags[name] = not arm->controller.isForceTorqueSafe();
            limbStopFlags[name] = not arm->controller.ftsensor.ftSafe.load();
        }
 
        for (auto& mpWrapper : mps)
        {
            auto& mp = mpWrapper.second.mp;
            const auto mpName = mp->getMPName();
            const auto nodeSet = mp->getNodeSetName();
 
            mpRunning[mpName] = mp->isRunning();
 
            /// 1. Check User Stop Request
            if (mp->isStopRequested())
            {
                mp->stop();
                ARMARX_INFO << "user requested to stop mp execution at canonical value: "
                            << mp->getCanonicalValue();
                continue;
            }
 
            /// 2. Check Force/Torque Guards configuration
            bool forceGuardEnabled = false;
            bool torqueGuardEnabled = false;
            for (auto& ftGuard : mpConfig.ftGuard)
            {
                if (ftGuard.mpName == mpName)
                {
                    if (ftGuard.force.has_value() &&
                        ftGuard.force.value() >= mp->getCanonicalValue())
                    {
                        forceGuardEnabled = true;
                    }
                    if (ftGuard.torque.has_value() &&
                        ftGuard.torque.value() >= mp->getCanonicalValue())
                    {
                        torqueGuardEnabled = true;
                    }
                }
            }
 
            /// 3. Stop if Global FT Unsafe OR Specific Guard Triggered
            bool globalFtUnsafe = (limbStopFlags.count(nodeSet) && limbStopFlags.at(nodeSet));
 
            if (globalFtUnsafe && mp->isRunning() && !mp->isFirstRun() &&
                ((forceGuardEnabled &&
                  this->limb.at(nodeSet)->controller.ftsensor.isForceGuardEnabled()) ||
                 (torqueGuardEnabled &&
                  this->limb.at(nodeSet)->controller.ftsensor.isTorqueGuardEnabled())))
            {
                mp->stop();
                ARMARX_INFO << "Due to force/torque safe guard, MP " << mpName << " is stopped at "
                            << mp->getCanonicalValue();
 
                if (this->coordinatorEnabled)
                {
                    for (auto& mpWrapper2 : mps)
                    {
                        auto& mp2 = mpWrapper2.second.mp;
                        ARMARX_INFO << "Stopped MP " << mp2->getMPName()
                                    << " as coordination is enabled";
                    }
                }
                else
                {
                    for (auto& mpWrapper2 : mps)
                    {
                        auto& mp2 = mpWrapper2.second.mp;
                        if (mp->getGroup() == mp2->getGroup())
                        {
                            mp2->stop();
                            ARMARX_INFO << "Stopped MP " << mp2->getMPName() << " part of MP group "
                                        << mp->getGroupName();
                        }
                    }
                }
            }
        }
        return mpRunning;
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::updateMPInputs(
        common::mp::MPInputOutput& mpData)
    {
        auto& mp = mpData.mp;
        const std::string& role = mp->getRole();
        const std::string& nodeSet = mp->getNodeSetName();
 
        ARMARX_TRACE;
        if (role == "taskspace")
        {
            auto& arm = this->limb.at(nodeSet);
 
            // Handle First Run Logic (Deviation Checks)
            if (mp->isFirstRun())
            {
                ARMARX_VERBOSE << "checking TSMP initial status ...";
                const std::vector<double> mpStart = mp->getStartVec();
                const auto mpStartPose = common::dVecToMat4(mpStart);
 
                const bool deviation = common::detectAndReportPoseDeviationWarning(
                    arm->rtStatusInNonRT.currentPose,
                    mpStartPose,
                    "current pose",
                    "initial start pose",
                    arm->nonRtConfig.safeDistanceMMToGoal,
                    arm->nonRtConfig.safeRotAngleDegreeToGoal,
                    "Additional task for task space MPs: " + mp->getMPName() +
                        " detects that the ");
 
                if (mp->getStartFromPrevTarget() or deviation)
                {
                    const bool deviation2 = common::detectAndReportPoseDeviationWarning(
                        arm->rtStatusInNonRT.currentPose,
                        arm->rtStatusInNonRT.desiredPose,
                        "current pose",
                        "desired pose",
                        arm->nonRtConfig.safeDistanceMMToGoal,
                        arm->nonRtConfig.safeRotAngleDegreeToGoal,
                        "Additional task for task space MPs: " + mp->getMPName() +
                            " detects that the ");
 
                    if (mp->getStartFromPrevTarget() and not deviation2)
                    {
                        const common::DVec desiredPoseVec =
                            common::mat4ToDVec(arm->rtStatusInNonRT.desiredPose);
                        ARMARX_INFO << "User requested to start the current TS MP for " << nodeSet
                                    << " with previous target pose:\n"
                                    << common::dVecToString(desiredPoseVec);
                        mp->validateInitialState(desiredPoseVec);
                    }
                    else
                    {
                        const common::DVec currentPoseVec =
                            common::mat4ToDVec(arm->rtStatusInNonRT.currentPose);
                        ARMARX_WARNING
                            << "Deviation from current pose too large, reset TS MP for " << nodeSet
                            << " to current pose:\n" << common::dVecToString(currentPoseVec);
                        mp->validateInitialState(currentPoseVec);
                    }
                }
                ARMARX_VERBOSE << "done";
            }
 
            common::mp::TSMPInputPtr in =
                std::dynamic_pointer_cast<common::mp::TSMPInput>(mpData.input);
            in->pose = arm->rtStatusInNonRT.currentPose;
            in->vel = arm->rtStatusInNonRT.currentTwist;
            in->deltaT = arm->nonRTDeltaT;
            return;
        }
 
        if (role == "nullspace")
        {
            auto& arm = this->limb.at(nodeSet);
            common::mp::JSMPInputPtr in =
                std::dynamic_pointer_cast<common::mp::JSMPInput>(mpData.input);
            ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF,
                               arm->rtStatusInNonRT.jointPosition.size());
            in->angleRadian = arm->rtStatusInNonRT.jointPosition;
            in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
            in->deltaT = arm->nonRTDeltaT;
            return;
        }
 
        if (role == "hand")
        {
            auto& hand = this->hands->hands.at(nodeSet);
            common::mp::JSMPInputPtr in =
                std::dynamic_pointer_cast<common::mp::JSMPInput>(mpData.input);
            if (hand->targetNonRT.jointPosition.has_value())
            {
                in->angleRadian = hand->targetNonRT.jointPosition.value();
                in->deltaT = hand->nonRTDeltaT;
                //                ARMARX_VERBOSE << "__ " << VAROUT(in->angleRadian) << VAROUT(in->deltaT);
            }
            return;
        }
 
        if (role == "object")
        {
            if (not this->coordinator)
            {
                ARMARX_WARNING
                    << "when using the object for MP, the coordinator must be first instantiated";
                return;
            }
            const std::scoped_lock lock_vo_non_rt(this->coordinator->mtx_data_non_rt);
            common::mp::TSMPInputPtr in =
                std::dynamic_pointer_cast<common::mp::TSMPInput>(mpData.input);
            in->pose = this->coordinator->dataNonRt.currentPose;
            in->vel = this->coordinator->dataNonRt.currentVel;
            in->deltaT = this->coordinator->dataNonRt.deltaT;
            return;
        }
 
        ARMARX_WARNING << "Your mp role " << role << " is not supported";
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::processGroupSafety(
        common::mp::MPInputOutput& mpData, // Pass the specific MP data structure
        MPDebugData& buffer)
    {
        auto& mp = mpData.mp;
        /// --------------------------------------------------------------------------------
        /// A. Check/Trigger Force-Torque Safety Guards for this MP
        /// --------------------------------------------------------------------------------
        for (auto& ftGuard : mpConfig.ftGuard)
        {
            if (ftGuard.mpName == mp->getMPName())
            {
                bool const forceGuard =
                    (ftGuard.force.has_value() && ftGuard.force.value() >= mp->getCanonicalValue());
                bool const torqueGuard = (ftGuard.torque.has_value() &&
                                          ftGuard.torque.value() >= mp->getCanonicalValue());
 
                auto& arm = this->limb.at(mp->getNodeSetName());
 
                bool const resetForce =
                    not arm->controller.ftsensor.isForceGuardEnabled() and forceGuard;
                bool const resetTorque =
                    not arm->controller.ftsensor.isTorqueGuardEnabled() and torqueGuard;
                if (resetForce or resetTorque)
                {
                    ARMARX_INFO << "Triggering force torque safety guard for "
                                << arm->kinematicChainName << " at can value "
                                << mp->getCanonicalValue();
                }
 
                arm->nonRtConfig.ftConfig.enableSafeGuardForce = forceGuard;
                arm->nonRtConfig.ftConfig.enableSafeGuardTorque = torqueGuard;
                arm->controller.ftsensor.enableSafeGuard(resetForce, resetTorque);
            }
        }
 
        /// --------------------------------------------------------------------------------
        /// B. Fetch MP desired pose/values and set to the buffer going to RT thread
        /// --------------------------------------------------------------------------------
        const std::string& role = mp->getRole();
        const std::string& nodeSetName = mp->getNodeSetName();
 
        if (role == "taskspace")
        {
            buffer.canonicalValue = mp->getCanonicalValue();
            auto& arm = this->limb.at(nodeSetName);
            auto out = std::dynamic_pointer_cast<common::mp::TSMPOutput>(mpData.output);
 
            bool flag = common::detectAndReportPoseDeviationWarning(
                arm->rtStatusInNonRT.currentPose,
                out->pose,
                "current pose",
                "desired pose",
                arm->nonRtConfig.safeDistanceMMToGoal,
                arm->nonRtConfig.safeRotAngleDegreeToGoal,
                "Additional task for task space MPs: " + mp->getMPName() + " detects that the ");
            groupDeviationFlag_.emplace(mp->getGroup(), flag);
 
            arm->nonRtConfig.desiredPose = out->pose;
            arm->nonRtConfig.desiredTwist = out->vel;
            buffer.desiredPose.at(nodeSetName) = out->pose;
        }
    }
 
    template <typename TSControllerType, typename MPInterfaceType>
    void
    TSMPController<TSControllerType, MPInterfaceType>::processMPOutput(
        common::mp::MPInputOutput& mpData, // Pass the specific MP data structure
        MPDebugData& buffer,
        bool rtTargetSafe)
    {
        auto& mp = mpData.mp;
        bool deviationFlag = true;
        if (groupDeviationFlag_.count(mp->getGroup()))
        {
            deviationFlag = groupDeviationFlag_.at(mp->getGroup());
        }
        const std::string& role = mp->getRole();
        const std::string& nodeSetName = mp->getNodeSetName();
 
        if (role == "object")
        {
            buffer.canonicalValue = mp->getCanonicalValue();
            if (this->coordinator)
            {
                auto out = std::dynamic_pointer_cast<common::mp::TSMPOutput>(mpData.output);
                this->coordinator->cfgInNonRt.desiredPose = out->pose;
            }
        }
        else if (role == "nullspace")
        {
            auto& arm = this->limb.at(nodeSetName);
            auto out = std::dynamic_pointer_cast<common::mp::JSMPOutput>(mpData.output);
            ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF, out->angleRadian.size());
            if (not deviationFlag)
            {
                arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
            }
        }
        else if (role == "hand")
        {
            auto& hand = this->hands->hands.at(nodeSetName);
            auto out = std::dynamic_pointer_cast<common::mp::JSMPOutput>(mpData.output);
            ARMARX_CHECK_EQUAL(hand->targetNonRT.jointPosition.value().size(),
                               out->angleRadian.size());
            if (not deviationFlag)
            {
                hand->targetNonRT.jointPosition.value() = out->angleRadian;
            }
            //            ARMARX_VERBOSE << "__ mp set target " << nodeSetName << ": "
            //                        << VAROUT(mp->getCanonicalValue()) << VAROUT(out->angleRadian);
        }
 
        /// ----------------------------------------------------------------------------------
        /// C. Upon MP stop, reset the nonRTConfig and User buffer, so that controller behaves
        /// Like a normal NJointController without MP.
        /// This is only a temporal solution, it has to be re-considered, e.g.,
        /// - 1) whether to stop the whole group (task space/nullspace/hand) at the same time
        /// - 2) all allow user to stop only partially;
        ///   E.g., In dishwasher skill, you want to trigger the stop at last 10%, but still
        ///   want to execute the rest of the finger motions to close and grasp the handle.
        ///   Otherwise, you will need to add a grasp motion after the skill for 1)
        /// ----------------------------------------------------------------------------------
        if (mp->isFinished())
        {
            ARMARX_VERBOSE << "-- reset buffer for nonRtConfig of " << mp->getMPName();
 
            // Reset all limbs to safe state or existing RT state
            for (auto& [name, arm] : this->limb)
            {
                // If RT was unsafe OR the MP calculated an unsafe deviation
                if (!rtTargetSafe || deviationFlag) // Note: Logic preserved from your update
                {
                    ARMARX_VERBOSE << "-- discard the last MP desired pose, use the existing RT "
                                      "desired pose for "
                                   << arm->kinematicChainName << ": \n"
                                   << arm->rtStatusInNonRT.desiredPose;
                    arm->nonRtConfig.desiredPose = arm->rtStatusInNonRT.desiredPose;
                }
                arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
            }
 
            // Sync user config
            for (auto& [name, arm] : this->limb)
            {
                this->userConfig.limbs.at(name) = arm->nonRtConfig;
            }
 
            // Sync hands
            if (this->hands)
            {
                this->hands->reinitBuffer(this->userConfig.hands);
            }
 
            // Sync coordinator
            if (this->coordinator)
            {
                this->coordinator->resetBufferUserCfgAfterMP();
            }
        }
    }
} // namespace armarx::control::njoint_mp_controller::task_space
 
//#include "Template_impl.h"