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)
    {
        const auto adaptionCfg = arondto::WipingAdaptionConfigs::FromAron(dto);
 
        bool valid = true;
 
        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";
                valid = false;
            }
        }
 
        if (valid)
        {
            bufferAdaptationConfigUserToNonRt.getWriteBuffer() = adaptionCfg;
            bufferAdaptationConfigUserToNonRt.commitWrite();
            ARMARX_INFO << "Updated adaptation config";
 
            adaptionReady.store(true);
        }
        else
        {
            ARMARX_ERROR << "Failed to update adaption config";
        }
    }
 
    void
    NJointWipingMixImpVelColMPController::rtPreActivateController()
    {
        for (auto& pair : limb)
        {
            pair.second->reInitPreActivate.store(true);
        }
 
        NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController::
            rtPreActivateController();
 
        for (const auto& pair : limb)
        {
            AdaptionStatus status;
            status.kpImpedance = pair.second->rtConfig.kpImpedance;
            status.kpCartesianVel = pair.second->rtConfig.kpCartesianVel;
            status.desiredPose = pair.second->rtConfig.desiredPose;
            adaptionStatus[pair.first] = status;
        }
    }
 
    void
    NJointWipingMixImpVelColMPController::additionalTask()
    {
        if (not adaptionReady.load())
        {
            return;
        }
        adaptionCfg = bufferAdaptationConfigUserToNonRt.getUpToDateReadBuffer();
 
        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] = limb.at(pair.first)->controller.ftsensor.ftSafe.load();
        }
 
        const double timeScalingFactor =
            adaptionCfg.timeScalingFactor.has_value()
                ? fmin(fmax(adaptionCfg.timeScalingFactor.value(), 0.25), 4.)
                : 1.;
 
        const double sizeScalingFactor =
            adaptionCfg.sizeScalingFactor.has_value() ? adaptionCfg.sizeScalingFactor.value() : 1.;
 
        std::map<std::string, bool> mpRunning;
        for (auto& _mp : mps)
        {
            const auto mpNodeSet = _mp.second.mp->getNodeSetName();
            auto search = flagMPToStop.find(mpNodeSet);
            if (_mp.second.mp->isStopRequested())
            {
                _mp.second.mp->stop();
                ARMARX_INFO << "user requested to stop mp execution at canonical value: "
                            << _mp.second.mp->getCanonicalValue();
            }
            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 * timeScalingFactor;
            }
            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 * timeScalingFactor;
            }
            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 template.h";
                //                in->angleRadian =
                //                    hand->bufferRTToNonRT.getUpToDateReadBuffer().jointPosition.value();
                in->deltaT = hand->bufferRTToNonRT.getReadBuffer().deltaT * timeScalingFactor;
            }
        }
        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")
            {
                const 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
                arm->nonRtConfig.desiredPose = s.desiredPose;
                const 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.desiredTwist = out->vel;
 
                /// Note: use force to generate acceleration target and aggregate to the target velocity
                if (arm->rtStatusInNonRT.rtTargetSafe)
                {
                    const float desiredVelZ =
                        cfg.kpf * (cfg.targetForce - arm->rtStatusInNonRT.currentForceTorque(2));
                    arm->nonRtConfig.desiredTwist(2) -= desiredVelZ;
                }
                arm->nonRtConfig.desiredTwist.tail<3>().setZero();
 
                const float dT = static_cast<float>(arm->rtStatusInNonRT.deltaT);
 
                /// check phase stop
                auto& adaptKImp = s.kpImpedance;
                auto& adaptKVel = s.kpCartesianVel;
 
                const Eigen::Vector2f& currentForceXY =
                    arm->rtStatusInNonRT.currentForceTorque.head<2>();
                if (currentForceXY.norm() > cfg.deadZoneForce)
                {
                    // Gradually decrease kp until zero
                    const Eigen::Vector2f dragForce =
                        currentForceXY - cfg.deadZoneForce * currentForceXY / currentForceXY.norm();
                    adaptKImp.head<2>().array() -= dT * cfg.adaptForceCoeff * dragForce.norm();
                    adaptKVel.head<2>().array() -= dT * cfg.adaptForceCoeff * dragForce.norm();
                }
                else
                {
                    // Gradually increase kp until original value
                    adaptKImp.head<2>().array() += fabs(dT * cfg.adaptCoeff);
                    adaptKVel.head<2>().array() += fabs(dT * cfg.adaptCoeff);
                }
 
                // Clip between 0. and original Kp
                adaptKImp.head<2>() = adaptKImp.head<2>()
                                          .array()
                                          .min(arm->nonRtConfig.kpImpedance.head<2>().array())
                                          .max(0.0);
                adaptKVel.head<2>() = adaptKVel.head<2>()
                                          .array()
                                          .min(arm->nonRtConfig.kpCartesianVel.head<2>().array())
                                          .max(0.0);
 
                arm->nonRtConfig.kpImpedance = adaptKImp;
                arm->nonRtConfig.kpCartesianVel = adaptKVel;
 
                // Scaling inverting trajectory
                arm->nonRtConfig.desiredTwist.head<2>() *= sizeScalingFactor * timeScalingFactor;
 
                /// Note: integrate the target pose
                arm->nonRtConfig.desiredPose.block<3, 1>(0, 3) +=
                    dT * arm->nonRtConfig.desiredTwist.head<3>();
 
                // Optionally restrict the workspace
                if (cfg.minX.has_value())
                {
                    arm->nonRtConfig.desiredPose(0, 3) =
                        fmax(arm->nonRtConfig.desiredPose(0, 3), *cfg.minX);
                }
                if (cfg.maxX.has_value())
                {
                    arm->nonRtConfig.desiredPose(0, 3) =
                        fmin(arm->nonRtConfig.desiredPose(0, 3), *cfg.maxX);
                }
                if (cfg.minY.has_value())
                {
                    arm->nonRtConfig.desiredPose(1, 3) =
                        fmax(arm->nonRtConfig.desiredPose(1, 3), *cfg.minY);
                }
                if (cfg.maxY.has_value())
                {
                    arm->nonRtConfig.desiredPose(1, 3) =
                        fmin(arm->nonRtConfig.desiredPose(1, 3), *cfg.maxY);
                }
                if (cfg.minZ.has_value())
                {
                    arm->nonRtConfig.desiredPose(2, 3) =
                        fmax(arm->nonRtConfig.desiredPose(2, 3), *cfg.minZ);
                }
 
                if (cfg.desiredUserPosition.has_value() and not cfg.desiredUserPosition->isZero())
                {
                    if (not(*cfg.desiredUserPosition - s.lastDesiredUserPosition).isZero())
                    {
                        if (s.openDistanceToDesiredUserPosition > 0.)
                        {
                            ARMARX_INFO << "Didn't reach previously defined desired position "
                                        << s.lastDesiredUserPosition;
                        }
                        s.lastDesiredUserPosition = *cfg.desiredUserPosition;
                        const Eigen::Vector2f distance =
                            s.lastDesiredUserPosition -
                            arm->nonRtConfig.desiredPose.block<2, 1>(0, 3);
                        s.openDistanceToDesiredUserPosition = distance.norm();
                        if (cfg.distanceToUserPositionScalingFactor.has_value())
                        {
                            s.openDistanceToDesiredUserPosition *=
                                *cfg.distanceToUserPositionScalingFactor;
                        }
                        ARMARX_INFO << "Initialized new desired user position at "
                                    << s.lastDesiredUserPosition << " which is "
                                    << s.openDistanceToDesiredUserPosition
                                    << " mm from current desired position";
                    }
 
                    if (s.openDistanceToDesiredUserPosition > 0.)
                    {
                        Eigen::Vector2f desiredLinearVelocity =
                            s.lastDesiredUserPosition -
                            arm->nonRtConfig.desiredPose.block<2, 1>(0, 3);
                        desiredLinearVelocity = dT * cfg.kpLinear * desiredLinearVelocity /
                                                desiredLinearVelocity.norm();
                        s.openDistanceToDesiredUserPosition -= desiredLinearVelocity.norm();
                        arm->nonRtConfig.desiredPose.block<2, 1>(0, 3) += desiredLinearVelocity;
                        if (s.openDistanceToDesiredUserPosition < 0.)
                        {
                            ARMARX_INFO << "Reached desired user position "
                                        << s.lastDesiredUserPosition;
                        }
                    }
                }
 
                /// Note: checking the duration that arm is tooFar to indicate a new location for intended wiping
                if (tooFar)
                {
                    const 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;
                        ARMARX_INFO << "Updated desired position to "
                                    << arm->nonRtConfig.desiredPose.block<3, 1>(0, 3).transpose();
                    }
                }
                else
                {
                    s.changeTimer = 0;
                }
 
                s.desiredPose = arm->nonRtConfig.desiredPose;
            }
            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