CollisionAvoidanceImpedanceController.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       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CollisionAvoidanceImpedanceController.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<NJointTSCollisionAvoidanceImpedanceMPController>
        registrationControllerNJointTSCollisionAvoidanceImpedanceMPController(
            "NJointTSCollisionAvoidanceImpedanceMPController");
 
    NJointTSCollisionAvoidanceImpedanceMPController::
        NJointTSCollisionAvoidanceImpedanceMPController(const RobotUnitPtr& robotUnit,
                                                        const NJointControllerConfigPtr& config,
                                                        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceImpedanceController(robotUnit, config, robot),
        NJointTaskspaceCollisionAvoidanceImpedanceController{robotUnit, config, robot}
    {
        ARMARX_IMPORTANT << getClassName() + " construction done";
    }
 
    std::string
    NJointTSCollisionAvoidanceImpedanceMPController::getClassName(const Ice::Current&) const
    {
        return "NJointTSCollisionAvoidanceImpedanceMPController";
    }
 
    void
    NJointTSCollisionAvoidanceImpedanceMPController::updateMPConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        if (resetMPsV1(dto, controllableNodeSets, coordinator))
        {
            for (auto& pair : limb)
            {
                pair.second->rtFirstRun.store(true);
            }
        }
    }
 
    void
    NJointTSCollisionAvoidanceImpedanceMPController::additionalTask()
    {
        const std::lock_guard<std::recursive_mutex> lock(mtx_mps);
        auto [rtTargetSafe, ftSafe] = additionalTaskUpdateStatus();
        if (coordinator)
        {
            coordinator->updateNonRt();
        }
 
        // 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] = not limb.at(pair.first)->controller.ftsensor.ftSafe.load();
            pair.second->nonRTDeltaT =
                pair.second->rtStatusInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
            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();
 
            /// TODO add loop the check if MP for the sync mode coordinator need to stop
            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->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);
                //                in->angleRadian = hand->rtsInNonRT.jointPosition.value();
                in->deltaT = hand->nonRTDeltaT;
            }
            else if (mp.second.mp->getRole() == "object")
            {
                if (not coordinator)
                {
                    ARMARX_WARNING << "when using the object for MP, the coordinator must be first "
                                      "instantiated";
                    return;
                }
                // auto& nodesetList = coord->getNodesetList();
                // auto& rtsL = limb.at(coord->getLeftNodesetName())->rtStatusInNonRT;
                // auto& rtsR = limb.at(coord->getRightNodesetName())->rtStatusInNonRT;
 
                // setPos(currentPose, 0.5f * (getPos(rtsL.currentPose) + getPos(rtsR.currentPose)));
                // setOri(currentPose, getOri(rtsL.currentPose));
                const std::scoped_lock lock_vo_non_rt(coordinator->mtx_data_non_rt);
                mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(mp.second.input);
                in->pose = coordinator->dataNonRt.currentPose;
                in->vel = coordinator->dataNonRt.currentVel;
                in->deltaT = coordinator->dataNonRt.deltaT;
            }
        }
        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.
        }
        runMPs(rtTargetSafe);
 
        /// 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& arm = limb.at(mp.second.mp->getNodeSetName());
                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
                common::detectAndReportPoseDeviationWarning(
                    arm->rtStatusInNonRT.currentPose,
                    out->pose,
                    "current pose",
                    "TSMP pose",
                    arm->nonRtConfig.safeDistanceMMToGoal,
                    arm->nonRtConfig.safeRotAngleDegreeToGoal,
                    mp.second.mp->getMPName() + "mp_set_target");
                arm->nonRtConfig.desiredPose = out->pose;
                arm->nonRtConfig.desiredTwist = out->vel;
                //                arm->bufferNonRtToPublish.getWriteBuffer().poseVec = out->poseVec;
                //                arm->bufferNonRtToPublish.commitWrite();
            }
            else if (mp.second.mp->getRole() == "object")
            {
                if (not coordinator)
                {
                    ARMARX_WARNING << "when using the object for MP, the coordinator must be first "
                                      "instantiated";
                    return;
                }
                mp::TSMPOutputPtr out = std::dynamic_pointer_cast<mp::TSMPOutput>(mp.second.output);
                coordinator->cfgInNonRt.desiredPose = out->pose;
            }
            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);
                }
                if (coordinator)
                {
                    coordinator->resetBufferUserCfgAfterMP();
                }
            }
        }
        // TODO apply coordinator (for loosely-coupled/tightly-coupled tasks)
        //  sync mode is handled directly by "object" type of mp
        if (coordinator)
        {
            coordinator->commitNonRt();
        }
 
        additionalTaskSetTarget();
    }
} // namespace armarx::control::njoint_mp_controller::task_space