fix: Transfer gaze targets to a dedicated gaze root frame (relative to head...

fix: Transfer gaze targets to a dedicated gaze root frame (relative to head base frame); Normalize calculation of pitch depending on selected yaw.

The issue here was that the calibration using the quadratic regression worked okay for yaw=0. However, as soon as there is a small tilt of the head base frame (i.e., the robot root frame and the head root frame where not parallel as in ARMAR-6), this calculation failed and resulted in a sinuidal movement of the head when the platform was rotated.
To fix this, the gaze targets are now transformed to a gaze root frame which in turn is relative to the head frame. Additionally, the target yaw is compensated before calculating the pitch.

data/armarx_view_selection/controller_config/GazeControllerHemisphere/default.json

 {
   "params": {
+    "gazeRootNodeName": "Gaze_Root",
     "cameraNodeName": "VirtualCentralGaze",
     "hemiANodeName": "Neck_2_Hemisphere_A",
     "hemiBNodeName": "Neck_3_Hemisphere_B",

source/armarx/view_selection/gaze_controller/hemisphere/CMakeLists.txt

@@ -19,9 +19,10 @@ armarx_add_library(gaze_controller_hemisphere
         RobotUnit # RobotAPI
         Simox::VirtualRobot
         Simox::SimoxUtility
+        armarx_control::interface
         armarx_view_selection::gaze_targets
         armarx_view_selection::gaze_controller_aron
-        armarx_control::interface
+        armarx_view_selection::gaze_controller_hemisphere_aron
     DEPENDENCIES_PRIVATE
         range-v3::range-v3
 )

source/armarx/view_selection/gaze_controller/hemisphere/GazeController.cpp

-
 #include "GazeController.h"
 
 #include <ArmarXCore/core/logging/Logging.h>
@@ -42,6 +41,8 @@ namespace armarx::view_selection::gaze_controller::hemisphere
             _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);
@@ -89,11 +90,13 @@ namespace armarx::view_selection::gaze_controller::hemisphere
     void
     GazeController::onInitNJointController()
     {
+        ;
     }
 
     void
     GazeController::onConnectNJointController()
     {
+        ;
     }
 
     std::string
@@ -109,7 +112,7 @@ namespace armarx::view_selection::gaze_controller::hemisphere
     {
         const float currentYawAngle = _rtYawNode->getJointValue();
         // const float currentPitchAngle = _rtPitchNode->getJointValue();
-        const float h = _rtCameraNode->getPositionInRootFrame().z();
+        //const float h = _rtCameraNode->getPositionInRootFrame().z();
 
         // report debugging variables
         _publishCurrentYawAngle = currentYawAngle;
@@ -135,57 +138,68 @@ namespace armarx::view_selection::gaze_controller::hemisphere
             target.frame = GlobalFrame;
         }
 
-        const Eigen::Vector3f targetPoint = target.toRootEigen(_rtRobot);
+        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());
-        const float targetPitchAngle = std::atan2(h - targetPoint.z(), targetPoint.y());
-
-        // derive hemisphere targets from pitch
-        float targetHemiA = rtGetControlStruct().params.yawToHemiFactorLinear * targetPitchAngle +
-                            rtGetControlStruct().params.yawToHemiFactorQuadratic *
-                                targetPitchAngle * targetPitchAngle;
-        float targetHemiB = rtGetControlStruct().params.yawToHemiFactorLinear * targetPitchAngle +
-                            rtGetControlStruct().params.yawToHemiFactorQuadratic *
-                                targetPitchAngle * targetPitchAngle;
-        // check reachability
-        const bool targetReachable = _rtYawNode->checkJointLimits(targetYawAngle) &&
-                                     _rtHemiANode->checkJointLimits(targetHemiA) &&
-                                     _rtHemiBNode->checkJointLimits(targetHemiB);
-
-
-        if (not targetReachable)
+
+        // The pitch depends on the yaw because in most but one case the head reference frame
+        // (Gaze_Root) is not axis aligned.  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 sinuidal 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)
         {
-            // limit joint ranges to avoid damage
             targetYawAngle = std::clamp(
                 targetYawAngle, _rtYawNode->getJointLimitLow(), _rtYawNode->getJointLimitHigh());
+        }
 
-            targetHemiA = std::clamp(
-                targetHemiA, _rtHemiANode->getJointLimitLow(), _rtHemiANode->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());
 
-            targetHemiB = std::clamp(
-                targetHemiB, _rtHemiBNode->getJointLimitLow(), _rtHemiBNode->getJointLimitHigh());
+            targetPitchAngle = std::clamp(targetPitchAngle,
+                                          _rtHemiBNode->getJointLimitLow(),
+                                          _rtHemiBNode->getJointLimitHigh());
         }
 
-
         // report debugging variables
         _publishTargetYawAngle = targetYawAngle;
         // _publishTargetPitchAngle = targetPitchAngle;
 
         // update control positions
         _rtYawCtrlTarget->position = targetYawAngle;
-        _rtHemiACtrlTarget->position = targetHemiA;
-        _rtHemiBCtrlTarget->position = targetHemiB;
+        _rtHemiACtrlTarget->position = targetPitchAngle;
+        _rtHemiBCtrlTarget->position = targetPitchAngle;
     }
 
     void
     GazeController::rtPreActivateController()
     {
+        ;
     }
 
     void
     GazeController::rtPostDeactivateController()
     {
+        ;
     }
 
     void

source/armarx/view_selection/gaze_controller/hemisphere/GazeController.h

@@ -34,16 +34,15 @@
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/libraries/core/PIDController.h>
 
-#include <armarx/view_selection/gaze_controller/types.h>
-#include <armarx/view_selection/gaze_controller/hemisphere/aron/ControllerConfig.aron.generated.h>
 #include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
+#include <armarx/view_selection/gaze_controller/hemisphere/aron/ControllerConfig.aron.generated.h>
+#include <armarx/view_selection/gaze_controller/types.h>
 #include <armarx/view_selection/gaze_targets/GazeTarget.h>
 
 namespace armarx::view_selection::gaze_controller::hemisphere
 {
     TYPEDEF_PTRS_HANDLE(GazeController);
 
-
     struct Config
     {
         arondto::Params params;
@@ -77,7 +76,8 @@ namespace armarx::view_selection::gaze_controller::hemisphere
         void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                           const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
 
-        ::armarx::aron::data::dto::DictPtr getConfig(const ::Ice::Current& = ::Ice::emptyCurrent) override
+        ::armarx::aron::data::dto::DictPtr
+        getConfig(const ::Ice::Current& = ::Ice::emptyCurrent) override
         {
             ARMARX_ERROR << "NYI";
             return nullptr;
@@ -111,6 +111,8 @@ namespace armarx::view_selection::gaze_controller::hemisphere
 
         // rt variables
         VirtualRobot::RobotPtr _rtRobot;
+        std::string _rtGazeNodeName;
+        VirtualRobot::RobotNodePtr _rtGazeNode;
         VirtualRobot::RobotNodePtr _rtYawNode;
         VirtualRobot::RobotNodePtr _rtHemiANode;
         VirtualRobot::RobotNodePtr _rtHemiBNode;

source/armarx/view_selection/gaze_controller/hemisphere/aron/ControllerConfig.xml

-
 <?xml version="1.0" encoding="UTF-8" ?>
 <AronTypeDefinition>
     <AronIncludes>
@@ -9,6 +8,10 @@
 
         <Object name='armarx::view_selection::gaze_controller::hemisphere::arondto::Params'>
 
+            <ObjectChild key="gazeRootNodeName">
+                <string />
+            </ObjectChild>
+
           <ObjectChild key='cameraNodeName'>
                 <string />
           </ObjectChild>
@@ -32,10 +35,8 @@
 	  <ObjectChild key='yawToHemiFactorQuadratic'>
                 <float32 />
           </ObjectChild>
-          
-         
-        </Object>
 
+        </Object>
 
         <Object name='armarx::view_selection::gaze_controller::hemisphere::arondto::Config' extends='armarx::view_selection::gaze_controller::arondto::ConfigBase' >
             <ObjectChild key='params'>