GazeController.cpp

Go to the documentation of this file.

#include "GazeController.h"
 
#include <algorithm>
#include <cmath>
#include <string>
 
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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/atan2/aron/ControllerConfig.aron.generated.h>
#include <armarx/view_selection/gaze_controller/atan2/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::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);
            _rtGazeNodeName = configData.params.gazeRootNodeName;
            _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*/)
    {
        auto updateConfigDto = arondto::Config::FromAron(dto);
 
        Config config;
        fromAron(updateConfigDto, config);
 
        _statusReporter.setTarget(config.targetId, config.target.name);
 
        setControlStruct(config);
    }
 
    void
    GazeController::onInitNJointController()
    {
        offeringTopic(constants::GazeControllerStatusTopicName);
    }
 
    void
    GazeController::onConnectNJointController()
    {
        _statusTopic = getTopic<GazeControllerStatusListenerPrx>(
            constants::GazeControllerStatusTopicName);
    }
 
    namespace
    {
        /**
         * @brief The gaze ray for a pair of joint angles, inverting the parametrization rtRun uses.
         *
         * rtRun takes yaw = -atan2(x, y) and then pitch = -atan2(z', y') on the point rotated by
         * -yaw about Z, which puts it in the y-z plane.  Going back the same way keeps the residual
         * in the controller's own convention, so no assumption about the camera's optical axis is
         * needed here.
         */
        Eigen::Vector3f
        gazeDirection(const float yaw, const float pitch)
        {
            const Eigen::Vector3f inYawPlane(0.F, std::cos(pitch), -std::sin(pitch));
 
            return Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * inYawPlane;
        }
    } // namespace
 
    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();
 
        // report debugging variables
        _publishCurrentYawAngle = currentYawAngle;
        _publishCurrentPitchAngle = currentPitchAngle;
 
        auto const gazeTarget = rtGetControlStruct().target;
        auto target = gazeTarget.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.
 
        // Against the requested target, not the clamped one: if it is out of reach the residual
        // has to stay large, and targetReachable below says why.
        const Eigen::Vector3f targetDirection = targetPoint;
        const float targetDistance = targetPoint.norm();
 
        // 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());
        }
 
        float targetPitchAngle = -std::atan2(targetPoint.z(), targetPoint.y());
 
        // check reachability
        const bool pitchTargetReachable = _rtPitchNode->checkJointLimits(targetPitchAngle);
        if (not pitchTargetReachable)
        {
            targetPitchAngle = std::clamp(targetPitchAngle,
                                          _rtPitchNode->getJointLimitLow(),
                                          _rtPitchNode->getJointLimitHigh());
        }
 
        // report debugging variables
        _publishTargetYawAngle = targetYawAngle;
        _publishTargetPitchAngle = targetPitchAngle;
 
        const GazeResidual residual =
            computeGazeResidual(gazeDirection(currentYawAngle, currentPitchAngle),
                                targetDirection,
                                targetDistance);
        _publishAngularError = residual.angularError;
        _publishLateralError = residual.lateralError;
        _statusReporter.update(
            rtGetControlStruct().targetId, residual, yawTargetReachable and pitchTargetReachable);
 
        // 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);
        datafields["angularError"] = new Variant(_publishAngularError.load());
        datafields["lateralError"] = new Variant(_publishLateralError.load());
 
        if (_tripRt2NonRt.updateReadBuffer())
        {
            gaze_targets::GazeTarget target = _tripRt2NonRt.getReadBuffer();
            // this->publishTarget(target);
        }
 
        debugObserver->setDebugChannel(getInstanceName(), datafields);
 
        _statusReporter.report(_statusTopic);
    }
 
    ::armarx::aron::data::dto::DictPtr
    GazeController::getConfig(const ::Ice::Current&)
    {
        ARMARX_ERROR << "NYI";
        return nullptr;
    }
 
} // namespace armarx::view_selection::gaze_controller::atan2