NJointCartesianWaypointController.cpp

#include "NJointCartesianWaypointController.h"

#include <iomanip>

#include <ArmarXCore/util/CPPUtility/trace.h>

#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>

namespace armarx
{
    std::ostream&
    operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
    {
        out << "maxPositionAcceleration     " << cfg.wpCfg.maxPositionAcceleration << '\n'
            << "maxOrientationAcceleration  " << cfg.wpCfg.maxOrientationAcceleration << '\n'
            << "maxNullspaceAcceleration    " << cfg.wpCfg.maxNullspaceAcceleration << '\n'

            << "kpJointLimitAvoidance       " << cfg.wpCfg.kpJointLimitAvoidance << '\n'
            << "jointLimitAvoidanceScale    " << cfg.wpCfg.jointLimitAvoidanceScale << '\n'

            << "thresholdPositionNear       " << cfg.wpCfg.thresholdPositionNear << '\n'
            << "thresholdPositionReached    " << cfg.wpCfg.thresholdPositionReached << '\n'
            << "maxPosVel                   " << cfg.wpCfg.maxPosVel << '\n'
            << "kpPos                       " << cfg.wpCfg.kpPos << '\n'

            << "thresholdOrientationNear    " << cfg.wpCfg.thresholdOrientationNear << '\n'
            << "thresholdOrientationReached " << cfg.wpCfg.thresholdOrientationReached << '\n'
            << "maxOriVel                   " << cfg.wpCfg.maxOriVel << '\n'
            << "kpOri                       " << cfg.wpCfg.kpOri << '\n'

            << "forceThreshold              " << cfg.forceThreshold << '\n';
        return out;
    }

    NJointControllerRegistration<NJointCartesianWaypointController>
        registrationControllerNJointCartesianWaypointController(
            "NJointCartesianWaypointController");

    std::string
    NJointCartesianWaypointController::getClassName(const Ice::Current&) const
    {
        return "NJointCartesianWaypointController";
    }

    NJointCartesianWaypointController::NJointCartesianWaypointController(
        RobotUnit*,
        const NJointCartesianWaypointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ARMARX_CHECK_NOT_NULL(config);

        //robot
        {
            _rtRobot = useSynchronizedRtRobot();
            ARMARX_CHECK_NOT_NULL(_rtRobot);

            _rtRns = _rtRobot->getRobotNodeSet(config->rns);
            ARMARX_CHECK_NOT_NULL(_rtRns) << "No rns " << config->rns;
            _rtTcp = _rtRns->getTCP();
            ARMARX_CHECK_NOT_NULL(_rtTcp) << "No tcp in rns " << config->rns;

            _rtRobotRoot = _rtRobot->getRootNode();
        }

        //sensor
        {
            if (!config->ftName.empty())
            {
                const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
                ARMARX_CHECK_NOT_NULL(svalFT) << "No sensor value of name " << config->ftName;
                _rtForceSensor = &(svalFT->force);
                _rtTorqueSensor = &(svalFT->force);

                const auto sdev = peekSensorDevice(config->ftName);
                ARMARX_CHECK_NOT_NULL(sdev);

                const auto reportFrameName = sdev->getReportingFrame();
                ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
                    << VAROUT(sdev->getReportingFrame());

                _rtFT = _rtRobot->getRobotNode(reportFrameName);
                ARMARX_CHECK_NOT_NULL(_rtFT)
                    << "No sensor report frame '" << reportFrameName << "' in robot";
            }
        }
        //ctrl
        {
            _rtJointVelocityFeedbackBuffer =
                Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));

            VirtualRobot::RobotNodePtr referenceFrame = nullptr;
            if (!config->referenceFrameName.empty())
            {
                referenceFrame = _rtRobot->getRobotNode(config->referenceFrameName);
                ARMARX_CHECK_NOT_NULL(referenceFrame)
                    << "No node with name '" << config->referenceFrameName
                    << "' for referenceFrame in robot";
            }

            _rtWpController = std::make_unique<CartesianWaypointController>(
                _rtRns,
                _rtJointVelocityFeedbackBuffer,
                config->runCfg.wpCfg.maxPositionAcceleration,
                config->runCfg.wpCfg.maxOrientationAcceleration,
                config->runCfg.wpCfg.maxNullspaceAcceleration,
                nullptr,
                referenceFrame);

            _rtWpController->setConfig({});

            for (size_t i = 0; i < _rtRns->getSize(); ++i)
            {
                std::string jointName = _rtRns->getNode(i)->getName();
                auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(
                    jointName, ControlModes::Velocity1DoF);
                auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
                ARMARX_CHECK_NOT_NULL(ct);
                ARMARX_CHECK_NOT_NULL(sv);

                _rtVelTargets.emplace_back(&(ct->velocity));
                _rtVelSensors.emplace_back(&(sv->velocity));
            }
        }
        //visu
        {
            _tripRt2NonRtRobotGP.getWriteBuffer().setIdentity();
            _tripRt2NonRtRobotGP.commitWrite();
        }
    }

    void
    NJointCartesianWaypointController::rtPreActivateController()
    {
        _publishIsAtTarget = false;
        _rtForceOffset = Eigen::Vector3f::Zero();

        _publishErrorPos = 0;
        _publishErrorOri = 0;
        _publishErrorPosMax = 0;
        _publishErrorOriMax = 0;
        _publishForceThreshold = _rtForceThreshold;

        //feedback
        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
            for (std::size_t idx = 0; idx < _rtVelSensors.size(); ++idx)
            {
                const float* ptr = _rtVelSensors[idx];
                _rtJointVelocityFeedbackBuffer(idx) = *ptr;
            }
            _rtWpController->setCurrentJointVelocity(_rtJointVelocityFeedbackBuffer);
#pragma GCC diagnostic pop
        }
    }

    void
    NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                             const IceUtil::Time& timeSinceLastIteration)
    {
        auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();

        if (_tripBufWpCtrl.updateReadBuffer())
        {
            ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
            auto& r = _tripBufWpCtrl._getNonConstReadBuffer();
            if (r.cfgUpdated)
            {
                r.cfgUpdated = false;
                _rtWpController->setConfig(r.cfg.wpCfg);
                _rtForceThreshold = r.cfg.forceThreshold;
                _publishForceThreshold = _rtForceThreshold;
                _rtOptimizeNullspaceIfTargetWasReached = r.cfg.optimizeNullspaceIfTargetWasReached;
                _rtForceThresholdInRobotRootZ = r.cfg.forceThresholdInRobotRootZ;
                _publishForceThresholdInRobotRootZ = r.cfg.forceThresholdInRobotRootZ;
                ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
            }
            if (r.wpsUpdated)
            {
                _rtStopConditionReached = false;
                _publishIsAtTarget = false;
                r.wpsUpdated = false;
                ARMARX_RT_LOGF_IMPORTANT("updated waypoints (# = %u)", r.wps.size());
                _publishWpsNum = r.wps.size();
                _rtHasWps = !r.wps.empty();
                _rtWpController->swapWaypoints(r.wps);
                if (r.cfg.skipToClosestWaypoint)
                {
                    _rtWpController->skipToClosestWaypoint(r.skipRad2mmFactor);
                }
            }
            _publishIsAtForceLimit = false;
        }
        //run
        bool reachedTarget = false;
        bool reachedFtLimit = false;

        if (_rtForceSensor)
        {
            const Eigen::Vector3f ftInRoot =
                _rtFT->getTransformationTo(_rtRobotRoot).topLeftCorner<3, 3>().transpose() *
                (*_rtForceSensor);
            rt2nonrtBuf.ft.force = ftInRoot;
            rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
            rt2nonrtBuf.ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
            rt2nonrtBuf.ftInRoot = _rtFT->getPoseInRootFrame();

            Eigen::Vector3f ftVal = ftInRoot;
            if (_rtForceThresholdInRobotRootZ)
            {
                ftVal(0) = 0;
                ftVal(1) = 0;
            }
            if (_setFTOffset)
            {
                _rtForceOffset = ftVal;
                _setFTOffset = false;
                _publishIsAtForceLimit = false;
            }
            rt2nonrtBuf.ftUsed = ftVal;

            const float currentForce = std::abs(ftVal.norm() - _rtForceOffset.norm());

            reachedFtLimit = (_rtForceThreshold >= 0) && currentForce > _rtForceThreshold;
            _publishForceCurrent = currentForce;
            _publishIsAtForceLimit = _publishIsAtForceLimit || reachedFtLimit;
        }

        _publishWpsCur = _rtHasWps ? _rtWpController->currentWaypointIndex : 0;

        rt2nonrtBuf.rootPose = _rtRobot->getGlobalPose();
        rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
        if (_rtHasWps)
        {
            const float errorPos = _rtWpController->getPositionError();
            const float errorOri = _rtWpController->getOrientationError();

            _publishErrorPos = errorPos;
            _publishErrorOri = errorOri;
            if (!_rtWpController->isLastWaypoint())
            {
                _publishErrorPosMax = _rtWpController->thresholdPositionNear;
                _publishErrorOriMax = _rtWpController->thresholdOrientationNear;
            }
            else
            {
                _publishErrorPosMax = _rtWpController->thresholdPositionReached;
                _publishErrorOriMax = _rtWpController->thresholdOrientationReached;
                reachedTarget = (errorPos < _rtWpController->thresholdPositionReached) &&
                                (errorOri < _rtWpController->thresholdOrientationReached);
            }
            rt2nonrtBuf.tcpTarg = _rtWpController->getCurrentTarget();
        }
        else
        {
            rt2nonrtBuf.tcpTarg = rt2nonrtBuf.tcp;
        }
        _rtStopConditionReached = _rtStopConditionReached || reachedFtLimit ||
                                  (reachedTarget && !_rtOptimizeNullspaceIfTargetWasReached);
        _publishIsAtTarget = _publishIsAtTarget || reachedTarget;
        if (!_rtHasWps || _rtStopConditionReached)
        {
            setNullVelocity();
        }
        else
        {
            const Eigen::VectorXf& goal =
                _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
            //write targets
            ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
            for (std::size_t idx = 0; idx < _rtVelTargets.size(); ++idx)
            {
                float* ptr = _rtVelTargets[idx];
                *ptr = goal(idx);
            }
        }
        _tripRt2NonRt.commitWrite();

        _tripRt2NonRtRobotGP.getWriteBuffer() = _rtRobot->getGlobalPose();
        _tripRt2NonRtRobotGP.commitWrite();
    }

    void
    NJointCartesianWaypointController::rtPostDeactivateController()
    {
    }

    bool
    NJointCartesianWaypointController::hasReachedTarget(const Ice::Current&)
    {
        return _publishIsAtTarget;
    }

    void
    NJointCartesianWaypointController::setConfig(
        const NJointCartesianWaypointControllerRuntimeConfig& cfg,
        const Ice::Current&)
    {
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.cfgUpdated = true;
        w.cfg = cfg;
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "set new config\n" << cfg;
    }

    void
    NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps,
                                                    const Ice::Current&)
    {
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.wpsUpdated = true;
        w.wps = wps;
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "set new waypoints\n" << wps;
    }

    void
    NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
    {
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.wpsUpdated = true;
        w.wps.clear();
        w.wps.emplace_back(wp);
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "set new waypoint\n" << wp;
    }

    void
    NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
    {
        Eigen::Matrix4f wp(data.data());
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.wpsUpdated = true;
        w.wps.clear();
        w.wps.emplace_back(wp);
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "set new waypoint\n" << wp;
    }

    void
    NJointCartesianWaypointController::setConfigAndWaypoints(
        const NJointCartesianWaypointControllerRuntimeConfig& cfg,
        const std::vector<Eigen::Matrix4f>& wps,
        const Ice::Current&)
    {
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.wpsUpdated = true;
        w.cfgUpdated = true;
        w.cfg = cfg;
        w.wps = wps;
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "set new config\n" << cfg << " and new waypoints\n" << wps;
    }

    void
    NJointCartesianWaypointController::setConfigAndWaypoint(
        const NJointCartesianWaypointControllerRuntimeConfig& cfg,
        const Eigen::Matrix4f& wp,
        const Ice::Current&)
    {
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.wpsUpdated = true;
        w.cfgUpdated = true;
        w.cfg = cfg;
        w.wps.clear();
        w.wps.emplace_back(wp);
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "set new config\n" << cfg << "and a new waypoint\n" << wp;
    }

    void
    NJointCartesianWaypointController::setNullVelocity()
    {
        for (auto ptr : _rtVelTargets)
        {
            *ptr = 0;
        }
    }

    bool
    NJointCartesianWaypointController::hasReachedForceLimit(const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(_rtForceSensor);
        return _publishIsAtForceLimit;
    }

    FTSensorValue
    NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(_rtForceSensor);
        std::lock_guard g{_tripRt2NonRtMutex};
        return _tripRt2NonRt.getUpToDateReadBuffer().ft;
    }

    void
    NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(_rtForceSensor);
        _setFTOffset = true;
    }

    void
    NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
                                                                       const Ice::Current&)
    {
        ; // No longer used ...
    }

    void
    NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
    {
        ; // No longer used ...
    }

    void
    NJointCartesianWaypointController::stopMovement(const Ice::Current&)
    {
        std::lock_guard g{_tripBufWpCtrlMut};
        auto& w = _tripBufWpCtrl.getWriteBuffer();
        w.wpsUpdated = true;
        w.cfgUpdated = false;
        w.wps.clear();
        _tripBufWpCtrl.commitWrite();
        ARMARX_IMPORTANT << "stop movement by setting 0 waypoints";
    }

    void
    NJointCartesianWaypointController::onPublish(const SensorAndControl&,
                                                 const DebugDrawerInterfacePrx& drawer,
                                                 const DebugObserverInterfacePrx& obs)
    {
        const std::size_t wpsNum = _publishWpsNum;
        const std::size_t wpsCur = wpsNum ? _publishWpsCur + 1 : 0;

        const float errorPos = _publishErrorPos;
        const float errorOri = _publishErrorOri;
        const float errorPosMax = _publishErrorPosMax;
        const float errorOriMax = _publishErrorOriMax;
        const float forceThreshold = _publishForceThreshold;
        const float forceCurrent = _publishForceCurrent;

        {
            StringVariantBaseMap datafields;
            datafields["WpNum"] = new Variant(static_cast<int>(wpsNum));
            datafields["WpCur"] = new Variant(static_cast<int>(wpsCur));

            datafields["errorPos"] = new Variant(errorPos);
            datafields["errorOri"] = new Variant(errorOri);
            datafields["errorPosMax"] = new Variant(errorPosMax);
            datafields["errorOriMax"] = new Variant(errorOriMax);

            datafields["forceThreshold"] = new Variant(forceThreshold);
            datafields["forceCurrent"] = new Variant(forceCurrent);

            obs->setDebugChannel(getInstanceName(), datafields);
        }

        ARMARX_INFO << deactivateSpam(1) << std::setprecision(4) << "At waypoint " << wpsCur
                    << " / " << wpsNum << " (last " << _publishIsAtTarget << ", ft limit "
                    << _publishIsAtForceLimit << ", perror " << errorPos << " / " << errorPosMax
                    << ", oerror " << errorOri << " / " << errorOriMax << ')';

        {
            std::lock_guard g{_tripRt2NonRtMutex};
            const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();

            const Eigen::Matrix4f fakeGP = _tripRt2NonRtRobotGP.getUpToDateReadBuffer();
            const Eigen::Matrix4f gp = std::isfinite(fakeGP(0, 0)) ? fakeGP : buf.rootPose;

            if (buf.tcp != buf.tcpTarg)
            {
                const Eigen::Matrix4f gtcp = gp * buf.tcp;
                const Eigen::Matrix4f gtcptarg = gp * buf.tcpTarg;
                drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
                drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
                drawer->setLineVisu(getName(),
                                    "tcp2targ",
                                    new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
                                    new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
                                    3,
                                    {0, 0, 1, 1});
            }
            else
            {
                drawer->removePoseVisu(getName(), "tcp");
                drawer->removePoseVisu(getName(), "tcptarg");
                drawer->removeLineVisu(getName(), "tcp2targ");
            }
            if (_rtForceSensor)
            {
                const Eigen::Matrix4f gft = gp * buf.ftInRoot;
                const Vector3Ptr pos = new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}};
                drawer->setArrowVisu(getName(),
                                     "force",
                                     pos,
                                     new Vector3{buf.ft.force.normalized()},
                                     {1, 1, 0, 1},
                                     buf.ft.force.norm(),
                                     2.5);
                drawer->setArrowVisu(getName(),
                                     "forceUsed",
                                     pos,
                                     new Vector3{buf.ftUsed.normalized()},
                                     {1, 0.5, 0, 1},
                                     buf.ftUsed.norm(),
                                     2.5);
            }
        }
    }

    int
    NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
    {
        return _publishWpsCur;
    }
} // namespace armarx