Add calibration capabilities to the ObjectPoseObserver

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp

@@ -51,6 +51,8 @@ namespace armarx
         defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
         defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
         defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent).");
+        defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated");
+        defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated");
 
         return defs;
     }
@@ -126,7 +128,14 @@ namespace armarx
         objpose::ObjectPoseSeq objectPoses;
         {
             std::scoped_lock lock(dataMutex);
-            RobotState::synchronizeLocalClone("robot");
+            synchronizeLocalClone(robot);
+
+            if (robot->hasRobotNode(calibration.robotNode))
+            {
+                VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+                float value = robotNode->getJointValue();
+                robotNode->setJointValue(value + calibration.offset);
+            }
 
             for (const auto& provided : providedPoses)
             {

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h

@@ -150,6 +150,13 @@ namespace armarx
         };
         Visu visu;
 
+        struct Calibration
+        {
+            std::string robotNode = "Neck_2_Pitch";
+            float offset = 0.0f;
+        };
+        Calibration calibration;
+
 
         struct RemoteGuiTab : RemoteGui::Client::Tab
         {