FrameTracking.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/>.
 *
 * @package    RobotAPI::ArmarXObjects::FrameTracking
 * @author     Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FrameTracking.h"
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
#include <RobotAPI/components/units/KinematicUnitObserver.h>
 
#include <time.h>
 
 
namespace armarx
{
    void FrameTracking::onInitComponent()
    {
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
        usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
        if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
        {
            usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
        }
 
        enabled = false;
        frameName = getProperty<std::string>("FrameOnStartup").getValue();
 
        maxYawVelocity = getProperty<float>("MaxYawVelocity").getValue();
        yawAcceleration = getProperty<float>("YawAcceleration").getValue();
 
        maxPitchVelocity = getProperty<float>("MaxPitchVelocity").getValue();
        pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
    }
 
 
    void FrameTracking::onConnectComponent()
    {
        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
                getProperty<std::string>("RobotStateComponentName").getValue());
        kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(
                getProperty<std::string>("KinematicUnitName").getValue());
        kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(
                getProperty<std::string>("KinematicUnitObserverName").getValue());
 
        localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
        headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
        if (!headYawJoint || !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
        {
            ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue() << " is not a valid joint.";
            getArmarXManager()->asyncShutdown();
        }
        headPitchJoint = localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
        if (!headPitchJoint || !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
        {
            ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue() << " is not a valid joint.";
            getArmarXManager()->asyncShutdown();
        }
        cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
        if (!cameraNode)
        {
            ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue() << " is not a valid node.";
            getArmarXManager()->asyncShutdown();
        }
 
        processorTask = new PeriodicTask<FrameTracking>(this, &FrameTracking::process, 30);
        _enableTracking(getProperty<bool>("EnableTrackingOnStartup").getValue());
 
        if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
        {
            _remoteGui = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
            RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
 
            rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Tracking: ")).addChild(
                            RemoteGui::makeTextLabel("Enabled")).addChild(
                            RemoteGui::makeCheckBox("enabled").value(enabled)).addChild(
                            RemoteGui::makeTextLabel("Frame")).addChild(
                            RemoteGui::makeComboBox("tracking_frame").options(
                                    localRobot->getRobotNodeNames()).addOptions({""}).value(frameName)));
 
            rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at frame: ")).addChild(
                            RemoteGui::makeComboBox("frame_look").options(localRobot->getRobotNodeNames()).value(
                                    frameName)).addChild(
                            RemoteGui::makeButton("button_look_at_frame").label("look at")));
 
            rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at global point: ")).addChild(
                            RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(
                                    2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(
                                    2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(
                                    2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeButton("button_look_at_global_point").label("look at")));
 
            rootLayoutBuilder.addChild(RemoteGui::makeHBoxLayout().addChild(
                    RemoteGui::makeTextLabel("Look at point in robot frame: ")).addChild(
                    RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(
                            2 * 1000000000 / 10).value(0.f)).addChild(
                    RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(
                            2 * 1000000000 / 10).value(0.f)).addChild(
                    RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(
                            2 * 1000000000 / 10).value(0.f)).addChild(
                    RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
 
            rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
                            RemoteGui::makeTextLabel("yaw from ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(
                                    headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
                                    static_cast<int>(
                                            (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
                                            0.1))).addChild(RemoteGui::makeTextLabel("yaw to ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(
                                    headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
                                    static_cast<int>(
                                            (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
                                            0.1))).addChild(RemoteGui::makeTextLabel("pitch ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(
                                    headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(
                                    static_cast<int>(
                                            (headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) /
                                            0.1))).addChild(RemoteGui::makeTextLabel("velocity ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(
                                    static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
                            RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
 
            rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
                            RemoteGui::makeTextLabel("from ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(
                                    1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(
                                    1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(
                                    1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeTextLabel("to ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(
                                    1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(
                                    1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(
                                    1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
                            RemoteGui::makeTextLabel("velocity ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(
                                    static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
                            RemoteGui::makeTextLabel("acceleration ")).addChild(
                            RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(
                                    static_cast<int>(8 / 0.1)).value(4.0f)).addChild(
                            RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
 
            rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
 
            _guiTask = new SimplePeriodicTask<>([this]()
                                                {
                                                    bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
                                                    std::string oldFrameGui = _guiTab.getValue<std::string>(
                                                            "tracking_frame").get();
 
                                                    _guiTab.receiveUpdates();
 
                                                    if (oldEnabledGui == enabled)
                                                    {
                                                        // only apply changes of gui if not already changed by ice
                                                        _enableTracking(_guiTab.getValue<bool>("enabled").get());
                                                    }
                                                    _guiTab.getValue<bool>("enabled").set(enabled);
 
                                                    if (oldFrameGui == frameName && oldFrameGui !=
                                                                                    _guiTab.getValue<std::string>(
                                                                                            "tracking_frame").get())
                                                    {
                                                        // only apply changes of gui if not already changed by ice
                                                        setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
                                                    }
                                                    _guiTab.getValue<std::string>("tracking_frame").set(frameName);
 
                                                    _guiTab.sendUpdates();
 
                                                    if (_guiTab.getButton("button_look_at_frame").clicked())
                                                    {
                                                        lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
                                                    }
 
                                                    if (_guiTab.getButton("button_look_at_global_point").clicked())
                                                    {
                                                        lookAtPointInGlobalFrame(Vector3f{
                                                                _guiTab.getValue<float>("global_point_x").get(),
                                                                _guiTab.getValue<float>("global_point_y").get(),
                                                                _guiTab.getValue<float>("global_point_z").get()});
                                                    }
 
                                                    if (_guiTab.getButton("button_look_at_robot_point").clicked())
                                                    {
                                                        lookAtPointInRobotFrame(
                                                                Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
                                                                         _guiTab.getValue<float>("robot_point_y").get(),
                                                                         _guiTab.getValue<float>(
                                                                                 "robot_point_z").get()});
                                                    }
 
                                                    if (_guiTab.getButton(
                                                            "button_scan_in_configuration_space").clicked())
                                                    {
                                                        scanInConfigurationSpace(_guiTab.getValue<float>(
                                                                                         "scan_in_configuration_space_yaw_from").get(),
                                                                                 _guiTab.getValue<float>(
                                                                                         "scan_in_configuration_space_yaw_to").get(),
                                                                                 {_guiTab.getValue<float>(
                                                                                         "scan_in_configuration_space_pitch").get()},
                                                                                 _guiTab.getValue<float>(
                                                                                         "scan_in_configuration_space_velocity").get());
                                                    }
 
                                                    if (_guiTab.getButton("button_scan_in_workspace").clicked())
                                                    {
                                                        scanInWorkspace({{_guiTab.getValue<float>(
                                                                                "scan_in_workspace_from_x").get(),                _guiTab.getValue<
                                                                                float>(
                                                                                "scan_in_workspace_from_y").get(),                _guiTab.getValue<
                                                                                float>("scan_in_workspace_from_z").get()},
                                                                         {_guiTab.getValue<float>(
                                                                                 "scan_in_workspace_to_x").get(), _guiTab.getValue<
                                                                                 float>(
                                                                                 "scan_in_workspace_to_y").get(), _guiTab.getValue<
                                                                                 float>(
                                                                                 "scan_in_workspace_to_z").get()}},
                                                                        _guiTab.getValue<float>(
                                                                                "scan_in_workspace_velocity").get(),
                                                                        _guiTab.getValue<float>(
                                                                                "scan_in_workspace_acceleration").get());
                                                    }
                                                }, 10);
 
            RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
 
            _remoteGui->createTab(getName(), rootLayout);
            _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
 
            _guiTask->start();
        }
    }
 
 
    void FrameTracking::onDisconnectComponent()
    {
        _enableTracking(false);
        if (_guiTask)
        {
            _guiTask->stop();
            _guiTask = nullptr;
        }
    }
 
 
    void FrameTracking::onExitComponent()
    {
 
    }
 
    armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(getConfigIdentifier()));
    }
 
    void FrameTracking::enableTracking(bool enable, const Ice::Current&)
    {
        _enableTracking(enable);
    }
 
    void FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to set new frame.";
            return;
        }
        this->frameName = frameName;
    }
 
    std::string FrameTracking::getFrame(const Ice::Current&) const
    {
        return frameName;
    }
 
    void FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to use lookAt functions.";
            return;
        }
        if (!localRobot->hasRobotNode(frameName))
        {
            ARMARX_ERROR << frameName << " does not exist.";
            return;
        }
        syncronizeLocalClone();
        _lookAtFrame(frameName);
    }
 
    void FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to use lookAt functions.";
            return;
        }
        syncronizeLocalClone();
        _lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
    }
 
    bool FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to use lookAt functions.";
            return false;
        }
        syncronizeLocalClone();
        return _looksAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)), max_diff);
    }
 
    void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to use lookAt functions.";
            return;
        }
        syncronizeLocalClone();
        _lookAtPoint(ToEigen(point));
    }
 
    void FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
    {
        const float currentYaw = headYawJoint->getJointValue();
        const float currentPitch = headPitchJoint->getJointValue();
 
        const float currentYawVel = DatafieldRefPtr::dynamicCast(
                kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
                                                                         headYawJoint->getName()))->getFloat();
        const float currentPitchVel = DatafieldRefPtr::dynamicCast(
                kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
                                                                         headPitchJoint->getName()))->getFloat();
 
        FrameTracking::HeadState headState;
        headState.currentYawPos = currentYaw;
        headState.currentYawVel = currentYawVel;
        headState.currentPitchPos = currentPitch;
        headState.currentPitchVel = currentPitchVel;
 
 
        headState.desiredYawPos = yaw;
        headState.desiredPitchPos = pitch;
        _doPositionControl(headState);
        struct timespec req = {0, 30 * 1000000L};
        while (std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
               std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.))
        {
            ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: "
                        << headPitchJoint->getJointValue() << " -> " << pitch;
            syncronizeLocalClone();
            // sleep for 30 milliseconds
            nanosleep(&req, nullptr);
        }
        auto currentModes = kinematicUnitInterfacePrx->getControlModes();
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   currentModes[headYawJoint->getName()]},
                                                      {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
    }
 
    void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues,
                                                 float velocity, const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to use scan functions.";
            return;
        }
        velocity = std::abs(velocity);
 
        syncronizeLocalClone();
        auto currentModes = kinematicUnitInterfacePrx->getControlModes();
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   ControlMode::eVelocityControl},
                                                      {headPitchJoint->getName(), ControlMode::eVelocityControl}});
 
        // to initial yaw
        {
            bool wasGreater = headYawJoint->getJointValue() > yawFrom;
            float yawVelocityToInit = wasGreater ? -velocity : velocity;
            kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   yawVelocityToInit},
                                                           {headPitchJoint->getName(), 0.f}});
            // if the joint angle was greater before we want to run as long as it is greater
            // otherwise we want to run as long as it is smaler
            while ((wasGreater && headYawJoint->getJointValue() > yawFrom) ||
                   (!wasGreater && headYawJoint->getJointValue() < yawFrom))
            {
                syncronizeLocalClone();
            }
        }
 
        for (const auto& p: pitchValues)
        {
            // to pitch value
            bool wasGreaterP = headPitchJoint->getJointValue() > p;
            float velocityPitch = wasGreaterP ? -velocity : velocity;
            kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   0.f},
                                                           {headPitchJoint->getName(), velocityPitch}});
            while ((wasGreaterP && headPitchJoint->getJointValue() > p) ||
                   (!wasGreaterP && headPitchJoint->getJointValue() < p))
            {
                syncronizeLocalClone();
            }
 
            // to yaw value
            bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
            float velocityYaw = wasGreaterY ? -velocity : velocity;
            kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   velocityYaw},
                                                           {headPitchJoint->getName(), 0.f}});
            while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) ||
                   (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
            {
                syncronizeLocalClone();
            }
 
            std::swap(yawFrom, yawTo);
        }
        kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   0.f},
                                                       {headPitchJoint->getName(), 0.f}});
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   currentModes[headYawJoint->getName()]},
                                                      {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
    }
 
    void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration,
                                        const Ice::Current&)
    {
        if (enabled)
        {
            ARMARX_WARNING << "Disable tracking to use scan functions.";
            return;
        }
        syncronizeLocalClone();
        auto currentModes = kinematicUnitInterfacePrx->getControlModes();
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   ControlMode::eVelocityControl},
                                                      {headPitchJoint->getName(), ControlMode::eVelocityControl}});
        struct timespec req = {0, 30 * 1000000L};
        for (const auto& p: points)
        {
            auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
            auto target = _calculateJointAngles(pEigen);
            while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
                   std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
            {
                ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: "
                            << target.currentPitchPos << " - " << target.desiredPitchPos;
                syncronizeLocalClone();
                target = _calculateJointAngles(pEigen);
                _doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
                // sleep for 30 milliseconds
                nanosleep(&req, nullptr);
            }
        }
        kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   0.f},
                                                       {headPitchJoint->getName(), 0.f}});
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   currentModes[headYawJoint->getName()]},
                                                      {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
    }
 
    void FrameTracking::process()
    {
        if (!localRobot->hasRobotNode(frameName))
        {
            ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
            std::thread([this]()
                        {
                            _enableTracking(false);
                        }).detach();
            return;
        }
        syncronizeLocalClone();
        _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration,
                           maxPitchVelocity, pitchAcceleration);
    }
 
    void FrameTracking::syncronizeLocalClone()
    {
        armarx::RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
    }
 
    void FrameTracking::_lookAtFrame(const std::string& frameName)
    {
        auto frame = localRobot->getRobotNode(frameName);
        auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
        _lookAtPoint(posInRobotFrame);
    }
 
    void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
    {
        _doPositionControl(_calculateJointAngles(point));
    }
 
    bool FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
    {
        auto head_state = _calculateJointAngles(point);
        float diff = std::abs(head_state.desiredPitchPos - head_state.currentPitchPos) +
                     std::abs(head_state.currentYawPos - head_state.desiredYawPos);
        return max_diff > diff;
    }
 
    FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
    {
        auto frame = localRobot->getRobotNode(frameName);
        auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
        // do nothing if the robot works above his head
        // he should already look upwards because if this component runs continously
        if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() + posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
        {
            return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
        }
        return _calculateJointAngles(posInRobotFrame);
    }
 
    FrameTracking::HeadState FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
    {
        float yaw = -std::atan2(point.x(), point.y());
        // make shure the joint value satisfies the joint limits
        yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
        yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
        // we dont want the robot to move from one limit to the other in one step
        const float currentYaw = headYawJoint->getJointValue();
        if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) >
                                            headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() -
                                            static_cast<float>(M_PI) / 8)
        {
            yaw = currentYaw;
        }
 
        const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(
                localRobot->toGlobalCoordinateSystemVec(point));
        const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
        const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(
                cameraNode->getGlobalPosition()).z();
        float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) +
                      std::asin(headHeightRealativeToPitchJoint / pj.norm());
        // make shure the joint value satisfies the joint limits
        pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
        pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
        const float currentPitch = headPitchJoint->getJointValue();
 
        ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw
                    << " and pitch=" << pitch;
 
        const float currentYawVel = DatafieldRefPtr::dynamicCast(
                kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
                                                                         headYawJoint->getName()))->getFloat();
        const float currentPitchVel = DatafieldRefPtr::dynamicCast(
                kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
                                                                         headPitchJoint->getName()))->getFloat();
 
        FrameTracking::HeadState headState;
        headState.currentYawPos = currentYaw;
        headState.desiredYawPos = yaw;
        headState.currentYawVel = currentYawVel;
        headState.currentPitchPos = currentPitch;
        headState.desiredPitchPos = pitch;
        headState.currentPitchVel = currentPitchVel;
        return headState;
    }
 
    void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity,
                                           float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
    {
        if (headState.stop)
        {
            kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   0.f},
                                                           {headPitchJoint->getName(), 0.f}});
            return;
        }
 
        float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
                                                                                        headState.currentYawVel,
                                                                                        maxYawVelocity, yawAcceleration,
                                                                                        yawAcceleration,
                                                                                        headState.currentYawPos,
                                                                                        headState.desiredYawPos, 1.f);
        float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
                                                                                          headState.currentPitchVel,
                                                                                          maxPitchVelocity,
                                                                                          pitchAcceleration,
                                                                                          pitchAcceleration,
                                                                                          headState.currentPitchPos,
                                                                                          headState.desiredPitchPos,
                                                                                          1.f);
 
        // control mode is set when enable task
        kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   desiredYawVelocity},
                                                       {headPitchJoint->getName(), desiredPitchVelocity}});
    }
 
    void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
    {
        auto currentModes = kinematicUnitInterfacePrx->getControlModes();
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   ControlMode::ePositionControl},
                                                      {headPitchJoint->getName(), ControlMode::ePositionControl}});
        if (headState.stop)
        {
            return;
        }
        kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(),   headState.desiredYawPos},
                                                   {headPitchJoint->getName(), headState.desiredPitchPos}});
        kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   currentModes[headYawJoint->getName()]},
                                                      {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
    }
 
    void FrameTracking::_enableTracking(bool enable)
    {
        if (this->enabled == enable)
        {
            return;
        }
        this->enabled = enable;
        if (enable)
        {
            kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(),   ControlMode::eVelocityControl},
                                                          {headPitchJoint->getName(), ControlMode::eVelocityControl}});
            processorTask->start();
        } else
        {
            kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(),   0.f},
                                                           {headPitchJoint->getName(), 0.f}});
            processorTask->stop();
        }
    }
}