WipingImpController.cpp

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/>.
 *
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "WipingImpController.h"
 
#include <map>
#include <mutex>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid this
 
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointWipingImpMPController>
        registrationControllerNJointWipingImpMPController("NJointWipingImpMPController");
 
    NJointWipingImpMPController::NJointWipingImpMPController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceImpedanceController{robotUnit, config, robot}
    {
        ARMARX_IMPORTANT << getClassName() + " construction done";
    }
 
    std::string
    NJointWipingImpMPController::getClassName(const Ice::Current&) const
    {
        return "NJointWipingImpMPController";
    }
 
    void
    NJointWipingImpMPController::updateMPConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                                const Ice::Current& iceCurrent)
    {
        if (resetMPsV1(dto, controllableNodeSets, nullptr))
        {
            for (auto& pair : limb)
            {
                pair.second->rtFirstRun.store(true);
            }
        }
    }
 
    void
    NJointWipingImpMPController::updateAdaptiveConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                                      const Ice::Current& iceCurrent)
    {
        adaptionCfg = arondto::WipingAdaptionConfigs::FromAron(dto);
        for (auto& pair : limb)
        {
            auto search = adaptionCfg.cfg.find(pair.first);
            if (search == adaptionCfg.cfg.end())
            {
                ARMARX_ERROR << "Arm " << pair.first << " is not included in the adaption config";
            }
            // ARMARX_CHECK_NOT_EQUAL(search, adaptionCfg.cfg.end());
        }
        adaptionReady.store(true);
    }
 
    void
    NJointWipingImpMPController::rtPreActivateController()
    {
        NJointTaskspaceImpedanceController::rtPreActivateController();
        // for (auto& pair: limb)
        // {
        //     adaptionCfg.cfg[pair.first] = arondto::WipingAdaptionConfigs();
        // }
        for (auto& pair : limb)
        {
            adaptionStatus[pair.first] = AdaptionStatus();
            adaptionStatus[pair.first].lastPosition =
                common::getPos(pair.second->rtStatus.currentPose).head<2>();
        }
    }
 
    void
    NJointWipingImpMPController::additionalTask()
    {
        if (not adaptionReady.load())
        {
            return;
        }
        const std::lock_guard<std::recursive_mutex> lock(mtx_mps);
        auto [rtTargetSafe, ftSafe] = additionalTaskUpdateStatus();
 
        // when ft guard is enabled and rt is not safe due to ft trigger, then we stop the corresponding mp
        std::map<std::string, bool> flagMPToStop;
        // double deltaT = 0.0;
        for (auto& pair : limb)
        {
            flagMPToStop[pair.first] = not limb.at(pair.first)->controller.ftsensor.ftSafe.load();
            pair.second->nonRTDeltaT =
                pair.second->rtStatusInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
            // deltaT = pair.second->nonRTDeltaT;
            pair.second->nonRTAccumulateTime = pair.second->rtStatusInNonRT.accumulateTime;
        }
        if (hands)
        {
            for (auto& pair : hands->hands)
            {
                pair.second->nonRTDeltaT =
                    pair.second->rtsInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
                pair.second->nonRTAccumulateTime = pair.second->rtsInNonRT.accumulateTime;
            }
        }
 
        std::map<std::string, bool> mpRunning;
        for (auto& mp : mps)
        {
            const auto mpNodeSet = mp.second.mp->getNodeSetName();
            mpRunning[mp.second.mp->getMPName()] = mp.second.mp->isRunning();
 
            auto search = flagMPToStop.find(mpNodeSet);
            if (search != flagMPToStop.end() and flagMPToStop.at(mpNodeSet) and
                mp.second.mp->isRunning() and
                (limb.at(mpNodeSet)->controller.ftsensor.isForceGuardEnabled() or
                 limb.at(mpNodeSet)->controller.ftsensor.isTorqueGuardEnabled()))
            {
                mp.second.mp->stop();
                ARMARX_INFO << "Due to force/torque safe guard, MP " << mp.second.mp->getMPName()
                            << " is stopped at " << mp.second.mp->getCanonicalValue();
            }
 
            if (mp.second.mp->isFinished())
            {
                continue;
            }
            if (mp.second.mp->getRole() == "taskspace")
            {
                auto& arm = limb.at(mp.second.mp->getNodeSetName());
                if (mp.second.mp->isFirstRun())
                {
                    ARMARX_INFO << "checking TSMP initial status ...";
                    /// Here we check if the TSMP start pose is too far away from the current pose, if so, we would
                    /// prefer to reset the mp start from the current pose.
                    const std::vector<double> mpStart = mp.second.mp->getStartVec();
                    const auto mpStartPose = common::dVecToMat4(mpStart);
                    if (mp.second.mp->getStartFromPrevTarget() or
                        common::detectAndReportPoseDeviationWarning(
                            arm->rtStatusInNonRT.currentPose,
                            mpStartPose,
                            "current pose",
                            "TSMP initial start pose",
                            arm->nonRtConfig.safeDistanceMMToGoal,
                            arm->nonRtConfig.safeRotAngleDegreeToGoal,
                            mp.second.mp->getMPName() + "mp_set_target"))
                    {
                        if (mp.second.mp->getStartFromPrevTarget())
                        {
                            ARMARX_INFO << "User requested to start the current TS MP from the "
                                           "previous target pose";
                        }
                        else
                        {
                            ARMARX_INFO
                                << "deviation from current pose too large, reset MP start pose";
                        }
                        mp.second.mp->validateInitialState(
                            common::mat4ToDVec(arm->rtStatusInNonRT.desiredPose));
                    }
                    ARMARX_INFO << "done";
                }
                mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(mp.second.input);
                in->pose = arm->rtStatusInNonRT.currentPose;
                in->vel = arm->rtStatusInNonRT.currentTwist;
                // in->deltaT = arm->rtStatusInNonRT.deltaT;
                in->deltaT = arm->nonRTDeltaT;
            }
            else if (mp.second.mp->getRole() == "nullspace")
            {
                auto& arm = limb.at(mp.second.mp->getNodeSetName());
                mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(mp.second.input);
                ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF,
                                   arm->rtStatusInNonRT.jointPosition.size());
                in->angleRadian = arm->rtStatusInNonRT.jointPosition;
                in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
                // in->deltaT = arm->rtStatusInNonRT.deltaT;
                in->deltaT = arm->nonRTDeltaT;
            }
            else if (mp.second.mp->getRole() == "hand")
            {
                auto& hand = hands->hands.at(mp.second.mp->getNodeSetName());
                mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(mp.second.input);
                ARMARX_ERROR
                    << "Fix the following line, see new controller implement in template.h";
                //                in->angleRadian = hand->rtsInNonRT.jointPosition.value();
                hand->nonRTDeltaT = hand->bufferRTToNonRT.getReadBuffer().accumulateTime -
                                    hand->nonRTAccumulateTime;
                hand->nonRTAccumulateTime = hand->bufferRTToNonRT.getReadBuffer().accumulateTime;
                // in->deltaT = hand->bufferRTToNonRT.getReadBuffer().deltaT;
                in->deltaT = hand->nonRTDeltaT;
            }
        }
        if (not rtTargetSafe)
        {
            handleRTNotSafeInNonRT();
 
            // return;
            // Make sure that you do not call additionalTaskSetTarget before return;
            // since we overwrite the arm->nonRtConfig in additionalTaskUpdateStatus() with user config, which does not
            // align with the up-to-date mp status. userConfig is only updated to align with MP status everytime a MP
            // finishes execution. Only then, user can use this controller as if no mp is there.
        }
        ARMARX_INFO << VAROUT(rtTargetSafe) << deactivateSpam(0.5);
        runMPs(rtTargetSafe);
 
        std::lock_guard<std::recursive_mutex> adaptLock(adaptionMtx);
        /// set mp target to nonRT config data structure
        for (auto& mp : mps)
        {
            if (not mpRunning.at(mp.second.mp->getMPName()))
            {
                continue;
            }
            for (auto& ftGuard : mpConfig.ftGuard)
            {
                if (ftGuard.mpName == mp.second.mp->getMPName())
                {
                    bool const forceGuard =
                        (ftGuard.force.has_value() and
                         ftGuard.force.value() >= mp.second.mp->getCanonicalValue());
                    bool const torqueGuard =
                        (ftGuard.torque.has_value() and
                         ftGuard.torque.value() >= mp.second.mp->getCanonicalValue());
 
                    auto& arm = limb.at(mp.second.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.second.mp->getCanonicalValue();
                    }
 
                    arm->nonRtConfig.ftConfig.enableSafeGuardForce = forceGuard;
                    arm->nonRtConfig.ftConfig.enableSafeGuardTorque = torqueGuard;
                    arm->controller.ftsensor.enableSafeGuard(resetForce, resetTorque);
                }
            }
 
            if (mp.second.mp->getRole() == "taskspace")
            {
                auto& nodeSetName = mp.second.mp->getNodeSetName();
                auto& cfg = adaptionCfg.cfg.at(nodeSetName);
                auto& s = adaptionStatus.at(nodeSetName);
                auto& arm = limb.at(nodeSetName);
                mp::TSMPOutputPtr out = std::dynamic_pointer_cast<mp::TSMPOutput>(mp.second.output);
 
                /// Note that here we only report warning but still set target to RT, we rely on MP phase stop and
                /// RT safety mechanism to stop or pause the action if needed
                bool tooFar = common::detectAndReportPoseDeviationWarning(
                    arm->rtStatusInNonRT.currentPose,
                    arm->nonRtConfig.desiredPose,
                    "current pose",
                    "desiredPose for RT",
                    arm->nonRtConfig.safeDistanceMMToGoal,
                    arm->nonRtConfig.safeRotAngleDegreeToGoal,
                    mp.second.mp->getMPName() + "mp_set_target");
                arm->nonRtConfig.desiredPose = out->pose;
                arm->nonRtConfig.desiredTwist = out->vel;
 
                /// Note: use force to generate acceleration target and aggregate to the target velocity
                float desiredVelZ =
                    cfg.kpf * (cfg.targetForce - arm->rtStatusInNonRT.currentForceTorque(2));
                arm->nonRtConfig.desiredTwist(2) -= desiredVelZ;
                arm->nonRtConfig.desiredTwist.tail<3>().setZero();
 
                // float dT = static_cast<float>(arm->rtStatusInNonRT.deltaT);
                float dT = static_cast<float>(arm->nonRTDeltaT);
 
                /// check phase stop
                auto& adaptKImp = arm->nonRtConfig.kpImpedance;
                if (not cfg.originalKpImp.has_value())
                {
                    cfg.originalKpImp = adaptKImp;
                }
                Eigen::Vector6f originalKpImp = cfg.originalKpImp.value();
                if (tooFar)
                {
                    Eigen::Vector2f currentForceXY =
                        arm->rtStatusInNonRT.currentForceTorque.head<2>();
                    Eigen::Vector2f dragForce =
                        currentForceXY - cfg.deadZoneForce * currentForceXY / currentForceXY.norm();
                    adaptKImp.head<2>() =
                        (adaptKImp.head<2>().array() - dT * cfg.adaptForceCoeff * dragForce.norm())
                            .max(0.0f);
                    // lastDiff = diff;
                }
                else
                {
                    adaptKImp.head<2>() = (adaptKImp.head<2>().array() + dT * cfg.adaptCoeff)
                                              .min(originalKpImp.head<2>().array());
                }
                adaptKImp(2) = originalKpImp(2);
 
                /// Note: integrate the target pose
                arm->nonRtConfig.desiredPose.block<3, 1>(0, 3) +=
                    dT * arm->nonRtConfig.desiredTwist.head<3>();
 
                /// Note: checking the duration that arm is tooFar to indicate a new location for intended wiping
                if (tooFar)
                {
                    Eigen::Vector2f currentXY = arm->rtStatusInNonRT.currentPose.block<2, 1>(0, 3);
                    if ((s.lastPosition - currentXY).norm() < cfg.changePositionTolerance)
                    {
                        s.changeTimer += dT;
                    }
                    else
                    {
                        s.lastPosition = currentXY;
                        s.changeTimer = 0;
                    }
 
                    if (s.changeTimer > cfg.changeTimerThreshold)
                    {
                        arm->nonRtConfig.desiredPose.block<2, 1>(0, 3) = currentXY;
                        // isPhaseStop = false;  /// Check if desiredPose above in report deviation is correct
                        s.changeTimer = 0;
                    }
                }
                else
                {
                    s.changeTimer = 0;
                }
            }
            else if (mp.second.mp->getRole() == "nullspace")
            {
                auto& arm = limb.at(mp.second.mp->getNodeSetName());
                mp::JSMPOutputPtr out = std::dynamic_pointer_cast<mp::JSMPOutput>(mp.second.output);
                ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF, out->angleRadian.size());
                arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
            }
            else if (mp.second.mp->getRole() == "hand")
            {
                mp::JSMPOutputPtr out = std::dynamic_pointer_cast<mp::JSMPOutput>(mp.second.output);
                auto& hand = hands->hands.at(mp.second.mp->getNodeSetName());
                mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(mp.second.input);
                ARMARX_CHECK_EQUAL(hand->targetNonRT.jointPosition.value().size(),
                                   out->angleRadian.size());
                hand->targetNonRT.jointPosition.value() = out->angleRadian;
            }
            if (mpRunning.at(mp.second.mp->getMPName()) and mp.second.mp->isFinished())
            {
                ARMARX_INFO << "reset buffer for nonRtConfig of " << mp.second.mp->getMPName();
                for (auto& pair : limb)
                {
                    auto& arm = pair.second;
                    arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
                }
                for (auto& pair : limb)
                {
                    userConfig.limbs.at(pair.first) = pair.second->nonRtConfig;
                }
                if (hands)
                {
                    hands->reinitBuffer(userConfig.hands);
                }
            }
        }
        additionalTaskSetTarget();
    }
} // namespace armarx::control::njoint_mp_controller::task_space