From f29e76a0b570cb96d0a60702639b9bb5323094d2 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 23 Jan 2023 19:59:47 +0100
Subject: [PATCH] Fix calibration offset in object instance segment
---
.../armem_objects/server/instance/Segment.cpp | 47 +++++++++++++++++++
.../armem_objects/server/instance/Segment.h | 10 ++++
.../server/instance/SegmentAdapter.cpp | 24 +---------
.../server/instance/SegmentAdapter.h | 11 +----
4 files changed, 59 insertions(+), 33 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 7f3aee34f..7de8f496d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/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();
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 08163aee2..b9c5a6d37 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/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 = "");
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index a77c3e851..b3ea262d1 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index 26295b451..88ab9ca34 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/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:
--
GitLab