GazeController.cpp

Go to the documentation of this file.

#include "GazeController.h"
 
#include <algorithm>
#include <cstddef>
#include <map>
#include <memory>
#include <string>
#include <vector>
 
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/zip.hpp>
 
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid thi>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/view_selection/common/controller_types.h>
#include <armarx/view_selection/gaze_controller/aron_conversions.h>
#include <armarx/view_selection/gaze_controller/gaze_ik/aron/ControllerConfig.aron.generated.h>
#include <armarx/view_selection/gaze_controller/gaze_ik/aron_conversions.h>
#include <armarx/view_selection/gaze_controller/constants.h>
#include <armarx/view_selection/gaze_controller/gaze_residual.h>
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
 
namespace armarx::view_selection::gaze_controller::gaze_ik
{
 
    const armarx::NJointControllerRegistration<GazeController> RegistrationControllerGazeController(
        common::ControllerTypeNames.to_name(common::ControllerType::GazeControllerGazeIK));
 
    GazeController::GazeController(armarx::RobotUnitPtr robotUnit,
                                   const NJointControllerConfigPtr& config,
                                   const VirtualRobot::RobotPtr&)
    {
        ARMARX_RT_LOGF("Creating gaze controller");
        // config
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        //        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
 
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        const auto configData = arondto::Config::FromAron(cfg->config);
 
        // robot
        ARMARX_RT_LOGF("Setting up nodes");
        {
            _rtRobot = useSynchronizedRtRobot();
            ARMARX_CHECK_NOT_NULL(_rtRobot);
            _rtRobot->setThreadsafe(false);
 
            _robotAdditionalTask = _rtRobot->clone();
            _robotAdditionalTask->setThreadsafe(false);
            RobotState initialState;
            initialState.globalRobotPose = _rtRobot->getGlobalPose();
 
            ARMARX_INFO << "Using nodeset `" << configData.params.nodeSetName << "`.";
            _headNodeSet = _robotAdditionalTask->getRobotNodeSet(configData.params.nodeSetName);
            ARMARX_CHECK_NOT_NULL(_headNodeSet);
            ARMARX_INFO << VAROUT(_headNodeSet->size());
 
            for (const auto& [name, value] : _robotAdditionalTask->getJointValues())
            {
                initialState.jointValues.push_back(value);
 
                const auto node = _rtRobot->getRobotNode(name);
                ARMARX_CHECK_NOT_NULL(node) << name;
                _rtRobotNodes.push_back(node);
 
                _additionalTaskRobotNodeNames.push_back(name);
            }
 
            ARMARX_CHECK_EQUAL(initialState.jointValues.size(), _rtRobotNodes.size());
 
            _robotStateBuffer_rtToAdditionalTask.reinitAllBuffers(initialState);
            Config initialConfig;
            fromAron(configData, initialConfig);
 
            ARMARX_INFO << VAROUT(initialConfig.target.position);
 
            _configBuffer_updateConfigToAdditionalTask.reinitAllBuffers(initialConfig);
            ARMARX_CHECK_NOT_NULL(_headNodeSet);
 
            const auto tcpNode = _headNodeSet->getTCP();
            _virtualPrismaticJoint =
                std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
            ARMARX_CHECK_NOT_NULL(_virtualPrismaticJoint);
 
            ARMARX_INFO << "Setting up GazeIK.";
            _gazeIK = std::make_unique<VirtualRobot::GazeIK>(_headNodeSet, _virtualPrismaticJoint);
            _gazeIK->enableJointLimitAvoidance(configData.params.enableJointLimitAvoidance);
            _gazeIK->setup(configData.params.maxPositionError,
                           configData.params.maxLoops,
                           configData.params.maxGradientDecentSteps);
            _gazeIK->setVerbose(false);
 
            // ARMARX_CHECK_NOT_NULL(_rtTorsoNode);
            // joint positions to be controlled
 
            ARMARX_INFO << "Requesting control targets";
            std::vector<float> controlTargets;
 
            for (const auto& nodeName : _headNodeSet->getNodeNames())
            {
                if (nodeName == _virtualPrismaticJoint->getName())
                {
                    continue;
                }
 
                ARMARX_INFO << "Using control target `" << nodeName << "`.";
                auto& target = _rtCtrlTargets.emplace_back(
                    useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                        nodeName, ControlModes::Position1DoF));
                ARMARX_CHECK_NOT_NULL(target);
                target->position = 0;
 
                const auto node = _headNodeSet->getNode(nodeName);
                ARMARX_CHECK_NOT_NULL(node) << nodeName;
                //                controlTargets.emplace_back(node->getJointValue());
                controlTargets.emplace_back(0);
            }
            reinitTripleBuffer(controlTargets);
        }
 
        ARMARX_RT_LOGF("Nodes set up successfully");
    }
 
    GazeController::~GazeController() = default;
 
    void
    GazeController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                 const Ice::Current& /*iceCurrent*/)
    {
        ARMARX_INFO << "Controller::updateConfig";
 
        auto updateConfigDto = arondto::Config::FromAron(dto);
 
        Config config;
        fromAron(updateConfigDto, config);
 
        ARMARX_INFO << VAROUT(config.target.position.frame);
 
        _statusReporter.setTarget(config.targetId, config.target.name);
 
        _configBuffer_updateConfigToAdditionalTask.getWriteBuffer() = config;
        _configBuffer_updateConfigToAdditionalTask.commitWrite();
    }
 
    void
    GazeController::onInitNJointController()
    {
        offeringTopic(constants::GazeControllerStatusTopicName);
 
        runTask("GazeControllerAdditionalTask",
                [&]
                {
                    CycleUtil c(10);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT << "Create a new thread alone PlatformTrajectory controller";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive() and rtReady.load())
                        {
                            ARMARX_VERBOSE << "additional task";
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    GazeController::onConnectNJointController()
    {
        _statusTopic =
            getTopic<GazeControllerStatusListenerPrx>(constants::GazeControllerStatusTopicName);
    }
 
    std::string
    GazeController::getClassName(const Ice::Current&) const
    {
        return common::ControllerTypeNames.to_name(common::ControllerType::GazeControllerGazeIK);
    }
 
    void
    GazeController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                          const IceUtil::Time& /*timeSinceLastIteration*/)
    {
        rtUpdateControlStruct();
 
        // update control devices
        ARMARX_CHECK_EQUAL(_rtCtrlTargets.size(), rtGetControlStruct().size());
        for (std::size_t i = 0; i < _rtCtrlTargets.size(); i++)
        {
            _rtCtrlTargets.at(i)->position = rtGetControlStruct().at(i);
        }
 
        // read data (for non-rt)
        _robotStateBuffer_rtToAdditionalTask.getWriteBuffer().globalRobotPose =
            rtGetRobot()->getGlobalPose();
 
        ARMARX_CHECK_EQUAL(_robotStateBuffer_rtToAdditionalTask.getWriteBuffer().jointValues.size(),
                           _rtRobotNodes.size());
        for (std::size_t i = 0; i < _rtRobotNodes.size(); i++)
        {
            ARMARX_CHECK_NOT_NULL(_rtRobotNodes.at(i)) << i;
            _robotStateBuffer_rtToAdditionalTask.getWriteBuffer().jointValues.at(i) =
                _rtRobotNodes.at(i)->getJointValue();
        }
 
        _robotStateBuffer_rtToAdditionalTask.commitWrite();
    }
 
    void
    GazeController::rtPreActivateController()
    {
        rtReady.store(true);
    }
 
    void
    GazeController::rtPostDeactivateController()
    {
        rtReady.store(false);
    }
 
    void
    GazeController::onPublish(const SensorAndControl&,
                              const DebugDrawerInterfacePrx&,
                              const DebugObserverInterfacePrx& debugObserver)
    {
 
        const float currentPitchAngle = _publishCurrentPitchAngle;
        const float currentYawAngle = _publishCurrentYawAngle;
        const float targetPitchAngle = _publishTargetPitchAngle;
        const float targetYawAngle = _publishTargetYawAngle;
 
        StringVariantBaseMap datafields;
        datafields["currentPitchAngle"] = new Variant(currentPitchAngle);
        datafields["currentYawAngle"] = new Variant(currentYawAngle);
        datafields["targetPitchAngle"] = new Variant(targetPitchAngle);
        datafields["targetYawAngle"] = new Variant(targetYawAngle);
        datafields["angularError"] = new Variant(_publishAngularError.load());
        datafields["lateralError"] = new Variant(_publishLateralError.load());
        if (_gazeTarget_rtRunToPublishing.updateReadBuffer())
        {
            gaze_targets::GazeTarget target = _gazeTarget_rtRunToPublishing.getReadBuffer();
            // this->publishTarget(target);
        }
        debugObserver->setDebugChannel(getInstanceName(), datafields);
 
        _statusReporter.report(_statusTopic);
    }
 
    void
    GazeController::additionalTask()
    {
        const auto& configBuffer =
            _configBuffer_updateConfigToAdditionalTask.getUpToDateReadBuffer();
        const auto& robotStateBuffer = _robotStateBuffer_rtToAdditionalTask.getUpToDateReadBuffer();
 
        // update robot
        _robotAdditionalTask->setGlobalPose(robotStateBuffer.globalRobotPose);
 
        ARMARX_CHECK_EQUAL(_additionalTaskRobotNodeNames.size(),
                           robotStateBuffer.jointValues.size());
        const std::map<std::string, float> jointPositions =
            ranges::views::zip(_additionalTaskRobotNodeNames, robotStateBuffer.jointValues) |
            ranges::to<std::map>();
 
        _robotAdditionalTask->setJointValues(jointPositions);
 
        ARMARX_CHECK_NOT_NULL(_gazeIK);
        ARMARX_CHECK_NOT_EMPTY(configBuffer.target.position.frame);
 
        const Eigen::Vector3f globalPosition =
            configBuffer.target.position.toGlobalEigen(_robotAdditionalTask);
 
        // Measured against the state just written above -- solve() below moves the model onto the
        // IK solution, after which the model no longer says where the robot is actually looking.
        const Eigen::Vector3f gazeOrigin = _virtualPrismaticJoint->getGlobalPosition();
        const GazeResidual residual =
            computeGazeResidual(_virtualPrismaticJoint->getJointTranslationDirection(),
                                globalPosition - gazeOrigin,
                                (globalPosition - gazeOrigin).norm());
 
        ARMARX_VERBOSE << deactivateSpam(5) << "Trying to solve IK for target global position "
                       << globalPosition;
        const bool solutionFound = _gazeIK->solve(globalPosition);
 
        _publishAngularError = residual.angularError;
        _publishLateralError = residual.lateralError;
        _statusReporter.update(configBuffer.targetId, residual, solutionFound);
 
        // store result
        if (solutionFound)
        {
            for (std::size_t i = 0; i < _headNodeSet->getSize(); i++)
            {
                if (auto node = _headNodeSet->getNode(i); node != _headNodeSet->getTCP())
                {
                    getWriterControlStruct().at(i) = node->getJointValue();
                }
 
                ARMARX_DEBUG << _headNodeSet->getNode(i)->getName() << ": "
                             << _headNodeSet->getNode(i)->getJointValue();
            }
        }
        else
        {
            ARMARX_WARNING << deactivateSpam(5)
                           << "No IK solution found for Gaze IK to global target "
                           << globalPosition;
        }
        writeControlStruct();
 
        // store results (onPublish)
    }
 
    ::armarx::aron::data::dto::DictPtr
    GazeController::getConfig(const ::Ice::Current&)
    {
        ARMARX_ERROR << "NYI";
        return nullptr;
    }
} // namespace armarx::view_selection::gaze_controller::gaze_ik