NJointCartesianNaturalPositionController.cpp

Go to the documentation of this file.

#include "NJointCartesianNaturalPositionController.h"
 
#include <iomanip>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.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::string
    vec2str(const std::vector<float>& vec)
    {
        std::stringstream ss;
        for (size_t i = 0; i < vec.size(); i++)
        {
            if (i > 0)
            {
                ss << ", ";
            }
            ss << vec.at(i);
        }
        return ss.str();
    }
 
    std::ostream&
    operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
    {
        out << "maxPositionAcceleration     " << cfg.maxPositionAcceleration << '\n'
            << "maxOrientationAcceleration  " << cfg.maxOrientationAcceleration << '\n'
            << "maxNullspaceAcceleration    " << cfg.maxNullspaceAcceleration << '\n'
 
            << "KpPos                       " << cfg.KpPos << '\n'
            << "KpOri                       " << cfg.KpOri << '\n'
            << "maxTcpPosVel                " << cfg.maxTcpPosVel << '\n'
            << "maxTcpOriVel                " << cfg.maxTcpOriVel << '\n'
            << "maxElpPosVel                " << cfg.maxElbPosVel << '\n'
 
            << "maxJointVelocity            " << vec2str(cfg.maxJointVelocity) << '\n'
            << "maxNullspaceVelocity        " << vec2str(cfg.maxNullspaceVelocity) << '\n'
            << "jointCosts                  " << vec2str(cfg.jointCosts) << '\n'
 
            << "jointLimitAvoidanceKp       " << cfg.jointLimitAvoidanceKp << '\n'
            << "elbowKp                     " << cfg.elbowKp << '\n'
            << "nullspaceTargetKp           " << cfg.nullspaceTargetKp << '\n'
 
            ;
        return out;
    }
 
    NJointControllerRegistration<NJointCartesianNaturalPositionController>
        registrationControllerNJointCartesianNaturalPositionController(
            "NJointCartesianNaturalPositionController");
 
    std::string
    NJointCartesianNaturalPositionController::getClassName(const Ice::Current&) const
    {
        return "NJointCartesianNaturalPositionController";
    }
 
    NJointCartesianNaturalPositionController::NJointCartesianNaturalPositionController(
        RobotUnit*,
        const NJointCartesianNaturalPositionControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ARMARX_CHECK_NOT_NULL(config);
 
        //ARMARX_IMPORTANT << VAROUT(config->rns);
        //ARMARX_IMPORTANT << config->runCfg;
 
        //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;
            _rtElbow = _rtRobot->getRobotNode(config->elbowNode);
            ARMARX_CHECK_NOT_NULL(_rtElbow) << "No elbow node " << config->elbowNode;
 
            _rtRobotRoot = _rtRobot->getRootNode();
        }
        //ARMARX_IMPORTANT << "got robot";
 
        //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";
            }
        }
        //ARMARX_IMPORTANT << "got sensor";
        //ctrl
        {
            _rtJointVelocityFeedbackBuffer =
                Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
 
            _rtPosController = std::make_unique<CartesianNaturalPositionController>(
                _rtRns,
                _rtElbow,
                config->runCfg.maxPositionAcceleration,
                config->runCfg.maxOrientationAcceleration,
                config->runCfg.maxNullspaceAcceleration);
 
            _rtPosController->setConfig(config->runCfg);
            ARMARX_IMPORTANT << "controller config: " << config->runCfg;
 
            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));
            }
        }
 
        _rtFFvelMaxAgeMS = config->feedforwardVelocityTTLms;
 
        _rtFTHistory.resize(config->ftHistorySize, FTval());
 
        //ARMARX_IMPORTANT << "got control";
        //visu
        {
            _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
            _tripFakeRobotGP.commitWrite();
        }
        //ARMARX_IMPORTANT << "got visu";
    }
 
    void
    NJointCartesianNaturalPositionController::rtPreActivateController()
    {
        //_publishIsAtTarget = false;
        //_rtForceOffset = Eigen::Vector3f::Zero();
 
        _publish.errorPos = 0;
        _publish.errorOri = 0;
        _publish.errorPosMax = 0;
        _publish.errorOriMax = 0;
        _publish.tcpPosVel = 0;
        _publish.tcpOriVel = 0;
        _publish.elbPosVel = 0;
        //_publish.targetReached = false;
        //_publishForceThreshold = _rtForceThreshold;
        _rtPosController->setTarget(_rtTcp->getPoseInRootFrame(),
                                    _rtElbow->getPositionInRootFrame());
    }
 
    void
    NJointCartesianNaturalPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                                    const IceUtil::Time& timeSinceLastIteration)
    {
        auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
 
        if (_tripBufCfg.updateReadBuffer())
        {
            ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer cfg");
            auto& r = _tripBufCfg._getNonConstReadBuffer();
            if (r.updated)
            {
                r.updated = false;
                _rtPosController->setConfig(r.cfg);
                ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
            }
        }
        if (_tripBufTarget.updateReadBuffer())
        {
            ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer target");
            auto& r = _tripBufTarget._getNonConstReadBuffer();
            if (r.updated)
            {
                r.updated = false;
                _rtSetOrientation = r.setOrientation;
                _rtPosController->setTarget(r.tcpTarget, r.elbowTarget);
                _rtStopConditionReached = false;
                _publishIsAtForceLimit = false;
            }
        }
        if (_tripBufNullspaceTarget.updateReadBuffer())
        {
            ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer NullspaceTarget");
            auto& r = _tripBufNullspaceTarget._getNonConstReadBuffer();
            if (r.updated)
            {
                r.updated = false;
                if (r.clearRequested)
                {
                    _rtPosController->clearNullspaceTarget();
                }
                else
                {
                    ARMARX_RT_LOGF_IMPORTANT("_rtPosController->setNullspaceTarget");
                    _rtPosController->setNullspaceTarget(r.nullspaceTarget);
                }
                r.clearRequested = false;
            }
        }
        if (_tripBufFFvel.updateReadBuffer())
        {
            _rtFFvel = _tripBufFFvel.getReadBuffer();
            _rtFFvelLastUpdateMS = sensorValuesTimestamp.toMilliSeconds();
        }
        if (_tripBufFToffset.updateReadBuffer())
        {
            _rtFTOffset = _tripBufFToffset.getReadBuffer();
        }
        if (_tripBufFTlimit.updateReadBuffer())
        {
            _rtFTlimit = _tripBufFTlimit.getReadBuffer();
            if (_rtFTlimit.force < 0 && _rtFTlimit.torque < 0)
            {
                _publishIsAtForceLimit = false;
            }
        }
        if (_tripBufFTfake.updateReadBuffer())
        {
            _rtFTfake = _tripBufFTfake.getReadBuffer();
        }
 
        //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();
 
            FTval ft;
            ft.force = *_rtForceSensor;
            ft.torque = *_rtTorqueSensor;
 
            if (_rtFTfake.use)
            {
                ft.force = _rtFTfake.force;
                ft.torque = _rtFTfake.torque;
                ARMARX_RT_LOGF_VERBOSE("Using fake ft values");
            }
 
            _rtFTHistory.at(_rtFTHistoryIndex) = ft;
            _rtFTHistoryIndex = (_rtFTHistoryIndex + 1) % _rtFTHistory.size();
 
            FTval ftAvg;
            for (size_t i = 0; i < _rtFTHistory.size(); i++)
            {
                ftAvg.force += _rtFTHistory.at(i).force;
                ftAvg.torque += _rtFTHistory.at(i).torque;
            }
            ftAvg.force = ftAvg.force / _rtFTHistory.size();
            ftAvg.torque = ftAvg.torque / _rtFTHistory.size();
 
 
            rt2nonrtBuf.currentFT = ft;
            rt2nonrtBuf.averageFT = ftAvg;
 
            if ((_rtFTlimit.force > 0 && ft.force.norm() > _rtFTlimit.force) ||
                (_rtFTlimit.torque > 0 && ft.torque.norm() > _rtFTlimit.torque))
            {
                _rtStopConditionReached = true;
                _publishIsAtForceLimit = true;
            }
 
            //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 ? _rtPosController->currentWaypointIndex : 0;
 
        rt2nonrtBuf.rootPose = _rtRobot->getGlobalPose();
        rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
        rt2nonrtBuf.elb = _rtElbow->getPoseInRootFrame();
        rt2nonrtBuf.tcpTarg = _rtPosController->getCurrentTarget();
        rt2nonrtBuf.elbTarg = ::math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(),
                                                          Eigen::Matrix3f::Identity());
 
 
        if (_rtStopConditionReached)
        {
            setNullVelocity();
        }
        else
        {
            VirtualRobot::IKSolver::CartesianSelection mode =
                _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
            _rtPosController->setNullSpaceControl(_nullspaceControlEnabled);
 
            if (_rtFFvel.use)
            {
                if (_rtFFvelLastUpdateMS + _rtFFvelMaxAgeMS >
                    sensorValuesTimestamp.toMilliSeconds())
                {
                    _rtPosController->setFeedForwardVelocity(_rtFFvel.feedForwardVelocity);
                }
                else
                {
                    ARMARX_RT_LOGF_WARN("Feed forward velocity cleared due to timeout");
                    _rtFFvel.use = false;
                }
            }
 
            const Eigen::VectorXf& goal =
                _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
 
            const Eigen::VectorXf actualTcpVel = _rtPosController->actualTcpVel(goal);
            const Eigen::VectorXf actualElbVel = _rtPosController->actualElbVel(goal);
            //ARMARX_IMPORTANT << VAROUT(actualTcpVel) << VAROUT(actualElbVel);
            _publish.tcpPosVel = actualTcpVel.block<3, 1>(0, 0).norm();
            _publish.tcpOriVel = actualTcpVel.block<3, 1>(3, 0).norm();
            _publish.elbPosVel = actualElbVel.block<3, 1>(0, 0).norm();
 
            //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();
    }
 
    void
    NJointCartesianNaturalPositionController::rtPostDeactivateController()
    {
    }
 
    void
    NJointCartesianNaturalPositionController::setConfig(
        const CartesianNaturalPositionControllerConfig& cfg,
        const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufCfg.getWriteBuffer();
        w.cfg = cfg;
        w.updated = true;
        _tripBufCfg.commitWrite();
        ARMARX_IMPORTANT << "set new config\n" << cfg;
    }
 
    void
    NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget,
                                                        const Eigen::Vector3f& elbowTarget,
                                                        bool setOrientation,
                                                        const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufTarget.getWriteBuffer();
        w.tcpTarget = tcpTarget;
        w.elbowTarget = elbowTarget;
        w.setOrientation = setOrientation;
        w.updated = true;
        _tripBufTarget.commitWrite();
        ARMARX_IMPORTANT << "set new target\n"
                         << tcpTarget << "\nelbow: " << elbowTarget.transpose();
    }
 
    void
    NJointCartesianNaturalPositionController::setTargetFeedForward(
        const Eigen::Matrix4f& tcpTarget,
        const Eigen::Vector3f& elbowTarget,
        bool setOrientation,
        const Eigen::Vector6f& ffVel,
        const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        {
            auto& w = _tripBufTarget.getWriteBuffer();
            w.tcpTarget = tcpTarget;
            w.elbowTarget = elbowTarget;
            w.setOrientation = setOrientation;
            w.updated = true;
        }
        {
            auto& w = _tripBufFFvel.getWriteBuffer();
            w.feedForwardVelocity = ffVel;
            w.updated = true;
        }
        _tripBufFFvel.commitWrite();
        _tripBufTarget.commitWrite();
    }
 
    void
    NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel,
                                                                     const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFFvel.getWriteBuffer();
        w.feedForwardVelocity = vel;
        w.use = true;
        w.updated = true;
        _tripBufFFvel.commitWrite();
        ARMARX_IMPORTANT << "set new FeedForwardVelocity\n" << vel.transpose();
    }
 
    void
    NJointCartesianNaturalPositionController::clearFeedForwardVelocity(const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFFvel.getWriteBuffer();
        w.feedForwardVelocity = Eigen::Vector6f::Zero();
        w.use = false;
        w.updated = true;
        _tripBufFFvel.commitWrite();
        ARMARX_IMPORTANT << "cleared FeedForwardVelocity";
    }
 
    void
    NJointCartesianNaturalPositionController::setNullVelocity()
    {
        for (auto ptr : _rtVelTargets)
        {
            *ptr = 0;
        }
    }
 
    //bool NJointCartesianNaturalPositionController::hasReachedForceLimit(const Ice::Current&)
    //{
    //    ARMARX_CHECK_NOT_NULL(_rtForceSensor);
    //    return _publishIsAtForceLimit;
    //}
 
    void
    NJointCartesianNaturalPositionController::setVisualizationRobotGlobalPose(
        const Eigen::Matrix4f& p,
        const Ice::Current&)
    {
        std::lock_guard g{_tripFakeRobotGPWriteMutex};
        _tripFakeRobotGP.getWriteBuffer() = p;
        _tripFakeRobotGP.commitWrite();
    }
 
    void
    NJointCartesianNaturalPositionController::resetVisualizationRobotGlobalPose(const Ice::Current&)
    {
        std::lock_guard g{_tripFakeRobotGPWriteMutex};
        _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
        _tripFakeRobotGP.commitWrite();
    }
 
    void
    NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
    {
        _stopNowRequested = true;
    }
 
    void
    NJointCartesianNaturalPositionController::setNullspaceTarget(
        const Ice::FloatSeq& nullspaceTarget,
        const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufNullspaceTarget.getWriteBuffer();
        w.nullspaceTarget = nullspaceTarget;
        w.clearRequested = false;
        w.updated = true;
        _tripBufNullspaceTarget.commitWrite();
        ARMARX_IMPORTANT << "set new nullspaceTarget\n" << nullspaceTarget;
    }
 
    void
    NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufNullspaceTarget.getWriteBuffer();
        w.clearRequested = true;
        w.updated = true;
        _tripBufNullspaceTarget.commitWrite();
        ARMARX_IMPORTANT << "reset nullspaceTarget";
    }
 
    void
    NJointCartesianNaturalPositionController::setNullspaceControlEnabled(bool enabled,
                                                                         const Ice::Current&)
    {
        _nullspaceControlEnabled = enabled;
    }
 
    FTSensorValue
    NJointCartesianNaturalPositionController::frost(const FTval& ft)
    {
        FTSensorValue r;
        r.force = ft.force;
        r.torque = ft.torque;
        return r;
    }
 
    FTSensorValue
    NJointCartesianNaturalPositionController::getCurrentFTValue(const Ice::Current&)
    {
        std::lock_guard g{_tripRt2NonRtMutex};
        return frost(_tripRt2NonRt.getUpToDateReadBuffer().currentFT);
    }
 
    FTSensorValue
    NJointCartesianNaturalPositionController::getAverageFTValue(const Ice::Current&)
    {
        std::lock_guard g{_tripRt2NonRtMutex};
        return frost(_tripRt2NonRt.getUpToDateReadBuffer().averageFT);
    }
 
    void
    NJointCartesianNaturalPositionController::setFTOffset(const FTSensorValue& offset,
                                                          const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFToffset.getWriteBuffer();
        w.force = offset.force;
        w.torque = offset.torque;
        w.updated = true;
        _tripBufFToffset.commitWrite();
        ARMARX_IMPORTANT << "set FT offset:\n"
                         << offset.force.transpose() << "\n"
                         << offset.torque.transpose();
    }
 
    void
    NJointCartesianNaturalPositionController::resetFTOffset(const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFToffset.getWriteBuffer();
        w.force = Eigen::Vector3f::Zero();
        w.torque = Eigen::Vector3f::Zero();
        w.updated = true;
        _tripBufFToffset.commitWrite();
        ARMARX_IMPORTANT << "reset FT offset";
    }
 
    void
    NJointCartesianNaturalPositionController::setFTLimit(float force,
                                                         float torque,
                                                         const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFTlimit.getWriteBuffer();
        w.force = force;
        w.torque = torque;
        w.updated = true;
        _tripBufFTlimit.commitWrite();
        ARMARX_IMPORTANT << "set FT limit:" << force << " " << torque;
    }
 
    void
    NJointCartesianNaturalPositionController::clearFTLimit(const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFTlimit.getWriteBuffer();
        w.force = -1;
        w.torque = -1;
        w.updated = true;
        _tripBufFTlimit.commitWrite();
        ARMARX_IMPORTANT << "reset FT limit";
    }
 
    void
    NJointCartesianNaturalPositionController::setFakeFTValue(const FTSensorValue& ftValue,
                                                             const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFTfake.getWriteBuffer();
        w.force = ftValue.force;
        w.torque = ftValue.torque;
        w.use = true;
        w.updated = true;
        _tripBufFTfake.commitWrite();
        ARMARX_IMPORTANT << "set fake FT value:\n"
                         << ftValue.force.transpose() << "\n"
                         << ftValue.torque.transpose();
    }
 
    void
    NJointCartesianNaturalPositionController::clearFakeFTValue(const Ice::Current&)
    {
        std::lock_guard g{_mutexSetTripBuf};
        auto& w = _tripBufFTfake.getWriteBuffer();
        w.force = Eigen::Vector3f::Zero();
        w.torque = Eigen::Vector3f::Zero();
        w.use = false;
        w.updated = true;
        _tripBufFTfake.commitWrite();
        ARMARX_IMPORTANT << "cleared fake FT value";
    }
 
    bool
    NJointCartesianNaturalPositionController::isAtForceLimit(const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(_rtForceSensor);
        return _publishIsAtForceLimit;
    }
 
    void
    NJointCartesianNaturalPositionController::onPublish(const SensorAndControl&,
                                                        const DebugDrawerInterfacePrx& drawer,
                                                        const DebugObserverInterfacePrx& obs)
    {
        const float errorPos = _publish.errorPos;
        const float errorOri = _publish.errorOri;
        const float errorPosMax = _publish.errorPosMax;
        const float errorOriMax = _publish.errorOriMax;
        //const float forceThreshold = _publishForceThreshold;
        //const float forceCurrent = _publishForceCurrent;
        const float tcpPosVel = _publish.tcpPosVel;
        const float tcpOriVel = _publish.tcpOriVel;
        const float elbPosVel = _publish.elbPosVel;
 
        {
            StringVariantBaseMap datafields;
 
            datafields["errorPos"] = new Variant(errorPos);
            datafields["errorOri"] = new Variant(errorOri);
            datafields["errorPosMax"] = new Variant(errorPosMax);
            datafields["errorOriMax"] = new Variant(errorOriMax);
 
            datafields["tcpPosVel"] = new Variant(tcpPosVel);
            datafields["tcpOriVel"] = new Variant(tcpOriVel);
            datafields["elbPosVel"] = new Variant(elbPosVel);
 
            //datafields["forceThreshold"] = new Variant(forceThreshold);
            //datafields["forceCurrent"] = new Variant(forceCurrent);
 
            obs->setDebugChannel(getInstanceName(), datafields);
        }
 
        ARMARX_INFO << deactivateSpam(1) << std::setprecision(4) << "perror " << errorPos << " / "
                    << errorPosMax << ", oerror " << errorOri << " / " << errorOriMax << ')';
 
        {
            std::lock_guard g{_tripRt2NonRtMutex};
            const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
 
            const Eigen::Matrix4f fakeGP = _tripFakeRobotGP.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;
                const Eigen::Matrix4f gelb = gp * buf.elb;
                const Eigen::Matrix4f gelbtarg = gp * buf.elbTarg;
                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});
                drawer->setLineVisu(getName(),
                                    "elb2targ",
                                    new Vector3(Eigen::Vector3f{gelb.topRightCorner<3, 1>()}),
                                    new Vector3(Eigen::Vector3f{gelbtarg.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);
            //}
        }
    }
} // namespace armarx