GazeController.cpp

Go to the documentation of this file.

 
#include "GazeController.h"
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/observers/VariantBase.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 this
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.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/atan2/aron/ControllerConfig.aron.generated.h>
#include <armarx/view_selection/gaze_controller/atan2/aron_conversions.h>
 
namespace armarx::view_selection::gaze_controller::atan2
{
 
    const armarx::NJointControllerRegistration<GazeController> RegistrationControllerGazeController(
        common::ControllerTypeNames.to_name(common::ControllerType::GazeControllerAtan2));
 
    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 armarx::view_selection::gaze_controller::atan2::arondto::Config configData =
            arondto::Config::FromAron(cfg->config);
 
        // robot
        ARMARX_RT_LOGF("Setting up nodes");
        {
            _rtRobot = useSynchronizedRtRobot();
            _rtRobot->setThreadsafe(false);
 
            ARMARX_CHECK_NOT_NULL(_rtRobot);
            _rtCameraNode = _rtRobot->getRobotNode(configData.params.cameraNodeName);
            _rtPitchNode = _rtRobot->getRobotNode(configData.params.pitchNodeName);
            _rtYawNode = _rtRobot->getRobotNode(configData.params.yawNodeName);
            // _rtTorsoNode = _rtRobot->getRobotNode(_config->torsoNodeName);
            ARMARX_CHECK_NOT_NULL(_rtCameraNode);
            ARMARX_CHECK_NOT_NULL(_rtPitchNode);
            ARMARX_CHECK_NOT_NULL(_rtYawNode);
            // ARMARX_CHECK_NOT_NULL(_rtTorsoNode);
            // joint positions to be controlled
            _rtPitchCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                configData.params.pitchNodeName, ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtPitchCtrlTarget);
            //_rtPitchCtrlPos = &(_pitchCtrlTarget->position);
            _rtYawCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                configData.params.yawNodeName, ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtYawCtrlTarget);
            //_rtYawCtrlPos = &(yawCtrlTarget->position);
        }
 
        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_VERBOSE << "Controller::updateConfig";
 
        auto updateConfigDto = arondto::Config::FromAron(dto);
 
        Config config;
        fromAron(updateConfigDto, config);
 
        setControlStruct(config);
    }
 
    void
    GazeController::onInitNJointController()
    {
    }
 
    void
    GazeController::onConnectNJointController()
    {
    }
 
    std::string
    GazeController::getClassName(const Ice::Current&) const
    {
        return common::ControllerTypeNames.to_name(common::ControllerType::GazeControllerAtan2);
    }
 
    void
    GazeController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                          const IceUtil::Time& /*timeSinceLastIteration*/)
    {
        const float currentYawAngle = _rtYawNode->getJointValue();
        const float currentPitchAngle = _rtPitchNode->getJointValue();
        const float h = _rtCameraNode->getPositionInRootFrame().z();
 
        // report debugging variables
        _publishCurrentYawAngle = currentYawAngle;
        _publishCurrentPitchAngle = currentPitchAngle;
 
        // if target is not set, use default joint values
        if (not rtGetControlStruct().target.has_value())
        {
            // report debugging variables
            _publishTargetYawAngle = 0;
            _publishTargetPitchAngle = 0;
 
            // update control positions
            _rtYawCtrlTarget->position = 0;
            _rtPitchCtrlTarget->position = 0;
            return;
        }
 
        auto target = rtGetControlStruct().target->position;
        if (target.frame.empty())
        {
            target.frame = GlobalFrame;
        }
 
 
        const Eigen::Vector3f targetPoint = target.toRootEigen(_rtRobot);
 
        float targetYawAngle = -std::atan2(targetPoint.x(), targetPoint.y());
        float targetPitchAngle = std::atan2(h - targetPoint.z(), targetPoint.y());
 
        // check reachability
        const bool targetReachable = _rtYawNode->checkJointLimits(targetYawAngle) &&
                                     _rtPitchNode->checkJointLimits(targetPitchAngle);
        if (not targetReachable)
        {
            // limit joint ranges to avoid damage
            targetYawAngle = std::clamp(
                targetYawAngle, _rtYawNode->getJointLimitLow(), _rtYawNode->getJointLimitHigh());
            targetPitchAngle = std::clamp(targetPitchAngle,
                                          _rtPitchNode->getJointLimitLow(),
                                          _rtPitchNode->getJointLimitHigh());
        }
 
        // report debugging variables
        _publishTargetYawAngle = targetYawAngle;
        _publishTargetPitchAngle = targetPitchAngle;
 
        // update control positions
        _rtYawCtrlTarget->position = targetYawAngle;
        _rtPitchCtrlTarget->position = targetPitchAngle;
    }
 
    void
    GazeController::rtPreActivateController()
    {
    }
 
    void
    GazeController::rtPostDeactivateController()
    {
    }
 
    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);
        if (_tripRt2NonRt.updateReadBuffer())
        {
            gaze_targets::GazeTarget target = _tripRt2NonRt.getReadBuffer();
            // this->publishTarget(target);
        }
        debugObserver->setDebugChannel(getInstanceName(), datafields);
    }
 
    ::armarx::aron::data::dto::DictPtr
    GazeController::getConfig(const ::Ice::Current&)
    {
        ARMARX_ERROR << "NYI";
        return nullptr;
    }
} // namespace armarx::view_selection::gaze_controller::atan2