Fix calibration offset in object instance segment

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -51,6 +51,15 @@
 namespace armarx::armem::server::obj::instance
 {
 
+    void Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(robotName, prefix + "robotName",
+                       "Name of robot whose note can be calibrated.\n"
+                       "If not given, the 'fallbackName' is used.");
+        defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+        defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+    }
+
     Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
         SpecializedCoreSegment(memoryToIceAdapter,
                                objects::instaceSegmentID.coreSegmentName,
@@ -150,6 +159,7 @@ namespace armarx::armem::server::obj::instance
     Segment::CommitStats Segment::commitObjectPoses(
         const std::string& providerName,
         const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+        const Calibration& calibration,
         std::optional<armem::Time> discardUpdatesUntil)
     {
         CommitStats stats;
@@ -209,6 +219,43 @@ namespace armarx::armem::server::obj::instance
                 {
                     ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp));
                     robotSyncTimestamp = timestamp;
+
+
+                    // Apply calibration offset after synchronizing the robot.
+                    {
+                        if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
+                        {
+                            VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+
+                            float value = robotNode->getJointValue();
+                            float newValue = value + calibration.offset;
+
+                            /*
+                             * If newValue is out of the joint's limits, then the calibration would be ignored.
+                             * In that case, we expand the joint value limits of the local robot model to allow
+                             * for the calibrated value.
+                             * As this is just for perception (and not for controlling the robot), this should be fine^TM.
+                             */
+                            VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+                            bool limitsChanged = false;
+                            if (newValue < limits.low)
+                            {
+                                limits.low = newValue;
+                                limitsChanged = true;
+                            }
+                            if (newValue > limits.high)
+                            {
+                                limits.high = newValue;
+                                limitsChanged = true;
+                            }
+                            if (limitsChanged)
+                            {
+                                robotNode->setJointLimits(limits.low, limits.high);
+                            }
+
+                            robotNode->setJointValue(newValue);
+                        }
+                    }
                 }
 
                 objpose::ObjectPose& newPose = newObjectPoses.emplace_back();

source/RobotAPI/libraries/armem_objects/server/instance/Segment.h

@@ -47,6 +47,15 @@ namespace armarx::armem::server::obj::instance
         using ObjectPoseSeq = objpose::ObjectPoseSeq;
         using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
 
+        struct Calibration
+        {
+            std::string robotName = "";
+            std::string robotNode = "Neck_2_Pitch";
+            float offset = 0.0f;
+
+            void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+        };
+
 
     public:
 
@@ -63,6 +72,7 @@ namespace armarx::armem::server::obj::instance
         CommitStats commitObjectPoses(
             const std::string& providerName,
             const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+            const Calibration& calibration,
             std::optional<Time> discardUpdatesUntil = std::nullopt);
         void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
 

source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp

@@ -175,21 +175,9 @@ namespace armarx::armem::server::obj::instance
         {
             const auto timestamp = armarx::Clock::Now();
 
-            VirtualRobot::RobotPtr calibrationRobot = segment.robots.get(calibration.robotName);
-            if (calibrationRobot)
-            {
-                ARMARX_CHECK(segment.robots.reader->synchronizeRobot(*calibrationRobot, timestamp));
-                if (calibrationRobot->hasRobotNode(calibration.robotNode))
-                {
-                    VirtualRobot::RobotNodePtr robotNode = calibrationRobot->getRobotNode(calibration.robotNode);
-                    float value = robotNode->getJointValue();
-                    robotNode->setJointValue(value + calibration.offset);
-                }
-            }
-
             TIMING_START(tCommitObjectPoses);
             Segment::CommitStats stats =
-                segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
+                segment.commitObjectPoses(providerName, providedPoses, calibration, discard.updatesUntil);
             TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
 
             if (debugObserver)
@@ -606,16 +594,6 @@ namespace armarx::armem::server::obj::instance
     const std::vector<PredictionEngine> SegmentAdapter::predictionEngines{{linearPredictionEngineID}};
 
 
-    void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
-    {
-        defs->optional(robotName, prefix + "robotName",
-                       "Name of robot whose note can be calibrated.\n"
-                       "If not given, the 'fallbackName' is used.");
-        defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
-        defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
-    }
-
-
     void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
     {
         using namespace armarx::RemoteGui::Client;

source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h

@@ -149,16 +149,7 @@ namespace armarx::armem::server::obj::instance
         instance::Visu visu;
         std::mutex visuMutex;
 
-
-        struct Calibration
-        {
-            std::string robotName = "";
-            std::string robotNode = "Neck_2_Pitch";
-            float offset = 0.0f;
-
-            void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
-        };
-        Calibration calibration;
+        Segment::Calibration calibration;
 
 
     public: