ObjectCollisionAvoidanceImpedanceController.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 "ObjectCollisionAvoidanceImpedanceController.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<NJointTSObjectCollisionAvoidanceImpedanceMPController>
        registrationControllerNJointTSObjectCollisionAvoidanceImpedanceMPController(
            "NJointTSObjectCollisionAvoidanceImpedanceMPController");
 
    NJointTSObjectCollisionAvoidanceImpedanceMPController::
        NJointTSObjectCollisionAvoidanceImpedanceMPController(const RobotUnitPtr& robotUnit,
                                                        const NJointControllerConfigPtr& config,
                                                        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceImpedanceController(robotUnit, config, robot),
        NJointTaskspaceObjectCollisionAvoidanceImpedanceController{robotUnit, config, robot}
    {
        ARMARX_IMPORTANT << getClassName() + " construction done";
    }
 
    std::string
    NJointTSObjectCollisionAvoidanceImpedanceMPController::getClassName(const Ice::Current&) const
    {
        return "NJointTSCollisionAvoidanceImpedanceMPController";
    }
 
    void
    NJointTSObjectCollisionAvoidanceImpedanceMPController::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
    NJointTSObjectCollisionAvoidanceImpedanceMPController::additionalTask()
    {
        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;
        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();
 
            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;
            }
        }
        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;
            }
            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();
    }
 
    void
    NJointTSObjectCollisionAvoidanceImpedanceMPController::onPublish(
        const SensorAndControl& sc,
        const DebugDrawerInterfacePrx& drawer,
        const DebugObserverInterfacePrx& debugObs)
    {
        NJointTaskspaceObjectCollisionAvoidanceImpedanceController::onPublish(sc, drawer, debugObs);
        for (auto& _mp : mps)
        {
            // visualize goal position
            if (!mpLayer)
            {
                mpLayer = arviz.layer("MP Visualization");
            }
 
            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);
        auto outPose = out->pose;
        outPose.block<3, 1>(0, 3) /= 1000;
 
                mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input);
                auto inPose = in->pose;
                inPose.block<3, 1>(0, 3) /= 1000;
 
                // name is current time
                auto outPoseSphere = viz::Sphere("poseOut_" + std::to_string(IceUtil::Time::now().toMicroSecondsDouble()))
                    .radius(10)
                    .position((coll->globalPose.matrix().cast<float>() * outPose).block<3, 1>(0, 3) * 1000)
                    .color(255, 0, 0, 128);
                mpLayer.value().add(outPoseSphere);
 
                auto inPoseSphere = viz::Sphere("poseIn_" + std::to_string(IceUtil::Time::now().toMicroSecondsDouble()))
                    .radius(10)
                    .position((coll->globalPose.matrix().cast<float>() * inPose).block<3, 1>(0, 3) * 1000)
                    .color(0, 0, 255, 128);
                mpLayer.value().add(inPoseSphere);
            }
            arviz.commit(mpLayer.value());
 
        }
    }
} // namespace armarx::control::njoint_mp_controller::task_space