SafetyImpedanceController.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 "SafetyImpedanceController.h"
 
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointTSSafetyImpedanceMPController>
        registrationControllerNJointTSSafetyImpedanceMPController(
            "NJointTSSafetyImpedanceMPController");
 
    NJointTSSafetyImpedanceMPController::NJointTSSafetyImpedanceMPController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceSafetyImpedanceController{robotUnit, config, robot}, mp::MPPool{}
    {
        mpConfig = MPListConfig();
        ARMARX_IMPORTANT << getClassName() + " construction done";
    }
 
    std::string
    NJointTSSafetyImpedanceMPController::getClassName(const Ice::Current&) const
    {
        return "NJointTSSafetyImpedanceMPController";
    }
 
    void
    NJointTSSafetyImpedanceMPController::updateMPConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        std::lock_guard<std::recursive_mutex> lock(mtx_mps);
        if (isFinished("all"))
        {
            auto configData = MPListConfig::FromAron(dto);
            isMPReady.store(false);
            while (additionalTaskRunning.load())
            {
                usleep(1000);
            }
            for (const auto& _mp : configData.mpList)
            {
                if (limb.find(_mp.nodeSetName) == limb.end())
                {
                    ARMARX_WARNING << "You have to make sure the RobotNodeSet " << _mp.nodeSetName
                                   << " in your MP config "
                                   << "corresponds to one of the RobotNodeSet in the controllers";
                    return;
                }
            }
            mpConfig = configData;
            reconfigureMPs(mpConfig);
            //            isMPReady.store(true);
 
            reInitMPInputOutputData();
            ARMARX_INFO << "-- successfully updated MP config.";
        }
        else
        {
            ARMARX_WARNING << "MP is not finished yet, cannot reset MP\n"
                              "If you want to force reset, please call \n\n"
                              "      ctrl->stop('all')    \n\n"
                              "before you continue";
        }
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointTSSafetyImpedanceMPController::getMPConfig(const Ice::Current& iceCurrent)
    {
        return mpConfig.toAronDTO();
    }
 
    void
    NJointTSSafetyImpedanceMPController::additionalTask()
    {
        robotUnit->updateVirtualRobot(nonRtRobot);
 
        /// parse arguments and run mp
        if (isMPReady.load())
        {
            additionalTaskRunning.store(true);
            bool rtSafe = true;
            for (auto& _mp : mps)
            {
                auto& arm = limb.at(_mp.second.mp->getNodeSetName());
                arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
                arm->rtStatusInNonRT = arm->bufferRtStatusToNonRt.getUpToDateReadBuffer();
                rtSafe = rtSafe and arm->rtStatusInNonRT.rtSafe;
 
                if (_mp.second.mp->getClassName() == "TSMP")
                {
                    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->getClassName() == "JSMP")
                {
                    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;
                }
            }
            if (rtSafe)
            {
                runMPs(rtSafe);
            }
            else
            {
                for (auto& pair : limb)
                {
                    if (not pair.second->rtReady)
                        continue;
                    const auto& desiredPose =
                        pair.second->rtStatusInNonRT.desiredPose.topRightCorner<3, 1>();
                    const auto& currentPose =
                        pair.second->rtStatusInNonRT.currentPose.topRightCorner<3, 1>();
 
                    if ((desiredPose - currentPose).norm() >
                        pair.second->nonRtConfig.safeDistanceMMToGoal)
                    {
                        ARMARX_RT_LOGF_WARN(
                            "new target \n\n [%.2f, %.2f, %.2f]\n\nis too far away from the "
                            "current pose\n\n [%.2f, %.2f, %.2f]",
                            pair.second->rtStatusInNonRT.desiredPose(0, 3),
                            pair.second->rtStatusInNonRT.desiredPose(1, 3),
                            pair.second->rtStatusInNonRT.desiredPose(2, 3),
                            pair.second->rtStatusInNonRT.currentPose(0, 3),
                            pair.second->rtStatusInNonRT.currentPose(1, 3),
                            pair.second->rtStatusInNonRT.currentPose(2, 3))
                            .deactivateSpam(1.0);
                    }
                }
            }
            /// set mp target to rt buffer
            for (auto& _mp : mps)
            {
                auto& arm = limb.at(_mp.second.mp->getNodeSetName());
                if (_mp.second.mp->getClassName() == "TSMP")
                {
                    mp::TSMPOutputPtr out =
                        std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output);
                    arm->nonRtConfig.desiredPose = out->pose;
                    arm->nonRtConfig.desiredTwist = out->vel;
                }
                else if (_mp.second.mp->getClassName() == "JSMP")
                {
                    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;
                }
            }
            for (auto& pair : limb)
            {
                limbNonRT(pair.second);
            }
        }
        else
        {
            additionalTaskRunning.store(false);
        }
    }
 
    void
    NJointTSSafetyImpedanceMPController::limbNonRT(ArmPtr& arm)
    {
        arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
        arm->bufferConfigNonRtToRt.commitWrite();
    }
 
    void
    NJointTSSafetyImpedanceMPController::reInitMPInputOutputData()
    {
        ARMARX_INFO << "-- reconfigure MP: reinitialize the mp input output, as well as the rt "
                       "related buffer values";
        int index = 0;
        for (auto& _mp : mps)
        {
            const auto& nodeSetName = _mp.second.mp->getNodeSetName();
            ARMARX_CHECK_EQUAL(nodeSetName, limb.at(nodeSetName)->kinematicChainName);
            VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(nodeSetName);
            Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
 
            if (_mp.second.mp->getClassName() == "TSMP")
            {
                ARMARX_INFO << "---- (" << index << ") init input output data for "
                            << _mp.second.mp->getMPName();
                std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
                std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->vel.setZero();
                std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
                std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->vel.setZero();
            }
            else if (_mp.second.mp->getClassName() == "JSMP")
            {
                ARMARX_INFO << "---- (" << index << ") init input output data for "
                            << _mp.second.mp->getMPName() << " with "
                            << rns->getJointValues().size() << " joints";
                std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian =
                    rns->getJointValuesEigen();
                std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel =
                    Eigen::VectorXf::Zero(rns->getJointValues().size());
                std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian =
                    rns->getJointValuesEigen();
                std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel =
                    Eigen::VectorXf::Zero(rns->getJointValues().size());
            }
            index++;
        }
    }
} // namespace armarx::control::njoint_mp_controller::task_space