GazeController.cpp

Go to the documentation of this file.

#include "GazeController.h"
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.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/NJointController.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/hemisphere/aron/ControllerConfig.aron.generated.h>
#include <armarx/view_selection/gaze_controller/hemisphere/aron_conversions.h>
 
namespace armarx::view_selection::gaze_controller::hemisphere
{
 
    const armarx::NJointControllerRegistration<GazeController> RegistrationControllerGazeController(
        common::ControllerTypeNames.to_name(common::ControllerType::GazeControllerHemisphere));
 
    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::hemisphere::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);
            _rtGazeNodeName = configData.params.gazeRootNodeName;
            _rtGazeNode = _rtRobot->getRobotNode(_rtGazeNodeName);
            _rtCameraNode = _rtRobot->getRobotNode(configData.params.cameraNodeName);
            _rtHemiANode = _rtRobot->getRobotNode(configData.params.hemiANodeName);
            _rtHemiBNode = _rtRobot->getRobotNode(configData.params.hemiBNodeName);
            _rtYawNode = _rtRobot->getRobotNode(configData.params.yawNodeName);
            // _rtTorsoNode = _rtRobot->getRobotNode(_config->torsoNodeName);
            ARMARX_CHECK_NOT_NULL(_rtCameraNode);
            ARMARX_CHECK_NOT_NULL(_rtHemiANode);
            ARMARX_CHECK_NOT_NULL(_rtHemiBNode);
            ARMARX_CHECK_NOT_NULL(_rtYawNode);
            // ARMARX_CHECK_NOT_NULL(_rtTorsoNode);
            // joint positions to be controlled
            _rtHemiACtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                configData.params.hemiANodeName, ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtHemiACtrlTarget);
 
            _rtHemiBCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                configData.params.hemiBNodeName, ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtHemiBCtrlTarget);
 
            //_rtPitchCtrlPos = &(_pitchCtrlTarget->position);
            _rtYawCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
                configData.params.yawNodeName, ControlModes::Position1DoF);
            ARMARX_CHECK_NOT_NULL(_rtYawCtrlTarget);
        }
 
        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::GazeControllerHemisphere);
    }
 
    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;
            _rtHemiACtrlTarget->position = 0;
            _rtHemiBCtrlTarget->position = 0;
            return;
        }
 
        auto target = rtGetControlStruct().target->position;
        if (target.frame.empty())
        {
            target.frame = GlobalFrame;
        }
 
        target.changeFrame(_rtRobot, _rtGazeNodeName); // Change frame to gaze root frame.
        Eigen::Vector3f targetPoint = target.toEigen(); // In gaze root frame.
 
        // compute spherical targets: pitch and yaw
        float targetYawAngle = -std::atan2(targetPoint.x(), targetPoint.y());
 
        // The pitch depends on the yaw because in most but one case the head reference frame
        // (Gaze_Root) is not axis aligned with the robot root frame.  We need to rotate the target
        // point by the negative yaw angle to account for that.  If this is not done, the head will
        // show a sinusoidal movement when the platform is rotated.
        targetPoint = Eigen::AngleAxisf(-targetYawAngle, Eigen::Vector3f::UnitZ()) * targetPoint;
 
        // Limit yaw range to avoid damage.
        const bool yawTargetReachable = _rtYawNode->checkJointLimits(targetYawAngle);
        if (not yawTargetReachable)
        {
            targetYawAngle = std::clamp(
                targetYawAngle, _rtYawNode->getJointLimitLow(), _rtYawNode->getJointLimitHigh());
        }
 
        // Derive target head pitch angle.
        float targetPitchAngle = -std::atan2(targetPoint.z(), targetPoint.y());
        // Adjust pitch to target position of hemisphere actuators using a simplified quadratic
        // regression.
        targetPitchAngle = (rtGetControlStruct().params.yawToHemiFactorLinear * targetPitchAngle) +
                           (rtGetControlStruct().params.yawToHemiFactorQuadratic *
                            targetPitchAngle * targetPitchAngle);
 
        // limit pitch range to avoid damage.
        const bool pitchTargetReachable = _rtHemiANode->checkJointLimits(targetPitchAngle) and
                                          _rtHemiBNode->checkJointLimits(targetPitchAngle);
        if (not pitchTargetReachable)
        {
            targetPitchAngle = std::clamp(targetPitchAngle,
                                          _rtHemiANode->getJointLimitLow(),
                                          _rtHemiANode->getJointLimitHigh());
 
            targetPitchAngle = std::clamp(targetPitchAngle,
                                          _rtHemiBNode->getJointLimitLow(),
                                          _rtHemiBNode->getJointLimitHigh());
        }
 
        // report debugging variables
        _publishTargetYawAngle = targetYawAngle;
        // _publishTargetPitchAngle = targetPitchAngle;
 
 
        _publishPlatformOrientation =
            simox::math::mat3f_to_rpy(_rtRobot->getGlobalOrientation()).z();
 
        // update control positions
        _rtYawCtrlTarget->position = targetYawAngle;
        _rtHemiACtrlTarget->position = targetPitchAngle;
        _rtHemiBCtrlTarget->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.load();
        // const float targetPitchAngle = _publishTargetPitchAngle;
        const float targetYawAngle = _publishTargetYawAngle.load();
 
        StringVariantBaseMap datafields;
        // datafields["currentPitchAngle"] = new Variant(currentPitchAngle);
        datafields["currentYawAngle"] = new Variant(currentYawAngle);
        // datafields["targetPitchAngle"] = new Variant(targetPitchAngle);
        datafields["targetYawAngle"] = new Variant(targetYawAngle);
        datafields["platformYaw"] = new Variant(_publishPlatformOrientation.load());
        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::hemisphere