WipingMixImpVelColController.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 "WipingMixImpVelColController.h"
 
#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<NJointWipingMixImpVelColMPController>
        registrationControllerNJointWipingMixImpVelColMPController(
            "NJointWipingMixImpVelColMPController");
 
    NJointWipingMixImpVelColMPController::NJointWipingMixImpVelColMPController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceMixedImpedanceVelocityController{robotUnit, config, robot},
        NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController{robotUnit, config, robot}
    {
        ARMARX_IMPORTANT << getClassName() + " construction done";
    }
 
    std::string
    NJointWipingMixImpVelColMPController::getClassName(const Ice::Current&) const
    {
        return "NJointWipingMixImpVelColMPController";
    }
 
    void
    NJointWipingMixImpVelColMPController::updateMPConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        if (resetMPs(dto, controllableNodeSets))
        {
            for (auto& pair : limb)
            {
                pair.second->rtFirstRun.store(true);
            }
        }
    }
 
    void
    NJointWipingMixImpVelColMPController::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
    NJointWipingMixImpVelColMPController::rtPreActivateController()
    {
        NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController::
            rtPreActivateController();
        // for (auto& pair: limb)
        // {
        //     adaptionCfg.cfg[pair.first] = arondto::WipingAdaptionConfigs();
        // }
        for (auto& pair : limb)
        {
            adaptionStatus[pair.first] = AdaptionStatus();
        }
    }
 
    void
    NJointWipingMixImpVelColMPController::additionalTask()
    {
        if (not adaptionReady.load())
        {
            return;
        }
        std::lock_guard<std::recursive_mutex> lock(mtx_mps);
        bool rtSafe = 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;
        for (auto& pair : limb)
        {
            flagMPToStop[pair.first] = limb.at(pair.first)->controller.ftsensor.ftSafe.load();
        }
 
        std::map<std::string, bool> mpRunning;
        for (auto& _mp : mps)
        {
            const auto mpNodeSet = _mp.second.mp->getNodeSetName();
            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();
            }
            mpRunning[_mp.second.mp->getMPName()] = _mp.second.mp->isRunning();
            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 (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"))
                    {
                        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;
            }
            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.numJoints,
                                   arm->rtStatusInNonRT.jointPos.size());
                in->angleRadian = arm->rtStatusInNonRT.jointPos;
                in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
                in->deltaT = arm->rtStatusInNonRT.deltaT;
            }
            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);
                in->angleRadian =
                    hand->bufferRTToNonRT.getUpToDateReadBuffer().jointPosition.value();
                in->deltaT = hand->bufferRTToNonRT.getReadBuffer().deltaT;
            }
        }
        if (not rtSafe)
        {
            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.
        }
        runMPs(rtSafe);
        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);
 
                /// check phase stop
                auto& adaptKImp = arm->nonRtConfig.kpImpedance;
                auto& adaptKVel = arm->nonRtConfig.kpCartesianVel;
                /// TODO find a way to keep the original coefficient parameter
                Eigen::Vector6f originalKpImp = Eigen::Vector6f::Zero();
                Eigen::Vector6f originalKpVel = Eigen::Vector6f::Zero();
                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);
                    adaptKVel.head<2>() =
                        (adaptKVel.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());
                    adaptKVel.head<2>() = (adaptKVel.head<2>().array() + dT * cfg.adaptCoeff)
                                            .min(originalKpVel.head<2>().array());
                }
                adaptKImp(2) = originalKpImp(2);
                adaptKImp(2) = originalKpVel(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.numJoints, 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