gaze_residual.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
namespace armarx::view_selection::gaze_controller
{
 
    /**
     * @brief How far the gaze currently is from a target.
     */
    struct GazeResidual
    {
        /// Angle between the gaze ray and the direction to the target [rad].
        float angularError = 0.F;
 
        /// Lateral miss distance at target range [mm].
        float lateralError = 0.F;
    };
 
    /**
     * @brief The gaze residual, in a form that means the same thing for every controller variant.
     *
     * Taking two directions rather than a robot node keeps the optical axis convention out of here:
     * each variant supplies its gaze ray using the convention it already works in -- `atan2` rebuilds
     * it from its own yaw/pitch parametrization, `gaze_ik` takes the axis of the virtual prismatic
     * joint that *is* its gaze ray.  Thresholds set on the result are therefore portable between them.
     *
     * Degenerate input (a target at the gaze origin, or a zero-length direction) yields a zero
     * residual rather than NaN.
     *
     * @param currentGazeDirection Where the robot is looking.  Need not be normalized.
     * @param targetDirection      Where the target is.  Need not be normalized.
     * @param targetDistance       Distance to the target [mm].
     */
    GazeResidual computeGazeResidual(const Eigen::Vector3f& currentGazeDirection,
                                     const Eigen::Vector3f& targetDirection,
                                     float targetDistance);
 
} // namespace armarx::view_selection::gaze_controller