diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 4bd80b117687e0553723ea8e044754ea97cc97dc..b3cb8043a8f03fd677a20b790c1454de7efb8129 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -22,7 +22,14 @@
#include "ObjectMemory.h"
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
+#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
namespace armarx::armem::server::obj
@@ -54,6 +61,9 @@ namespace armarx::armem::server::obj
// defs->component(kinematicUnitObserver); // Optional dependency.
defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
"Name of the kinematic unit observer.");
+ defs->optional(predictionLookbackSeconds, "prediction.LookbackSeconds",
+ "Lookback to use for predictions when requested via the PredictingMemoryInterface"
+ " (in seconds).");
return defs;
}
@@ -215,6 +225,89 @@ namespace armarx::armem::server::obj
return outputs;
}
+ armem::prediction::data::PredictionResultSeq
+ ObjectMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
+ {
+ std::vector<armem::prediction::data::PredictionResult> results;
+ for (const auto& request : requests)
+ {
+ auto boRequest = armarx::fromIce<armem::client::PredictionRequest>(request);
+ armem::client::PredictionResult result;
+ result.snapshotID = boRequest.snapshotID;
+ if (armem::contains(workingMemory().id().withCoreSegmentName("Instance"),
+ boRequest.snapshotID) &&
+ !boRequest.snapshotID.hasGap() && boRequest.snapshotID.hasInstanceIndex())
+ {
+ objpose::ObjectPosePredictionRequest objPoseRequest;
+ objPoseRequest.lookback = armarx::toIce<armarx::core::time::dto::Duration>(
+ Duration::SecondsDouble(predictionLookbackSeconds));
+ objPoseRequest.objectID = armarx::toIce<armarx::data::ObjectID>(
+ ObjectID(boRequest.snapshotID.entityName + "/" +
+ boRequest.snapshotID.instanceIndexStr()));
+ objPoseRequest.settings = request.settings;
+ objPoseRequest.timestamp = armarx::toIce<armarx::core::time::dto::DateTime>(
+ boRequest.snapshotID.timestamp);
+ objpose::ObjectPosePredictionResult objPoseResult =
+ predictObjectPoses({objPoseRequest}).at(0);
+ result.success = objPoseResult.success;
+ result.errorMessage.str(objPoseResult.errorMessage);
+
+ if (objPoseResult.success)
+ {
+
+ armem::client::QueryBuilder builder;
+ builder.singleEntitySnapshot(boRequest.snapshotID);
+ auto queryResult = armarx::fromIce<armem::client::QueryResult>(
+ query(builder.buildQueryInputIce()));
+ std::string instanceError =
+ "Could not find instance '" + boRequest.snapshotID.str() + "' in memory";
+ if (!queryResult.success)
+ {
+ result.success = false;
+ result.errorMessage << instanceError << ":\n" << queryResult.errorMessage;
+ }
+ else
+ {
+ auto* aronInstance = queryResult.memory.findInstance(boRequest.snapshotID);
+ if (aronInstance == nullptr)
+ {
+ result.success = false;
+ result.errorMessage << instanceError;
+ }
+ else
+ {
+ auto instance =
+ armem::arondto::ObjectInstance::FromAron(aronInstance->data());
+ objpose::toAron(
+ instance.pose,
+ armarx::fromIce<objpose::ObjectPose>(objPoseResult.prediction));
+ result.prediction = instance.toAron();
+ }
+ }
+ }
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage << "No predictions are supported for MemoryID "
+ << boRequest.snapshotID
+ << ". Have you given an instance index if requesting"
+ << " an object pose prediction?";
+ }
+ results.push_back(result.toIce());
+ }
+
+ return results;
+ }
+
+ armem::prediction::data::EngineSupportMap ObjectMemory::getAvailableEngines()
+ {
+ // TODO(phesch): Replace with generic code in Memory implementation
+ return {{armarx::toIce<armem::data::MemoryID>(
+ workingMemory().id().withCoreSegmentName("Instance")),
+ {{"Linear Position Regression"}}}};
+ }
+
void ObjectMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 82f39db591bd56537fa280e0dd10a56fa9806ca3..f078b436c9663ad7421baf1d17367de763d64f6a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -96,11 +96,21 @@ namespace armarx::armem::server::obj
// Without this, ObjectMemory draws the original
// methods from ObjectMemoryInterface and complains
// that an overload is being hidden.
- using ReadWritePluginUser::getActions;
using ReadWritePluginUser::executeActions;
+ using ReadWritePluginUser::getActions;
+ using ReadWritePluginUser::getAvailableEngines;
+ using ReadWritePluginUser::predict;
- armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs) override;
- armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
+ // Actions
+ armem::actions::GetActionsOutputSeq
+ getActions(const armem::actions::GetActionsInputSeq& inputs) override;
+ armem::actions::ExecuteActionOutputSeq
+ executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
+
+ // Predictions
+ armem::prediction::data::PredictionResultSeq
+ predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
+ armem::prediction::data::EngineSupportMap getAvailableEngines() override;
// Remote GUI
void createRemoteGuiTab();
@@ -113,6 +123,8 @@ namespace armarx::armem::server::obj
RobotStateComponentInterfacePrx robotStateComponent;
KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+ double predictionLookbackSeconds = 2;
+
clazz::Segment classSegment;
attachments::Segment attachmentSegment;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 8239d5ce052fcba95b3c32ca82739be0273e1897..819eb729f577b18492191b3ac204770e7d567d7d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -579,6 +579,14 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPosePredictionResult result;
result.success = false;
+ if (!request.settings.predictionEngineID.empty() &&
+ request.settings.predictionEngineID != "Linear Position Regression")
+ {
+ result.errorMessage = "Prediction engine '" + request.settings.predictionEngineID +
+ "' not available for object pose prediction.";
+ return result;
+ }
+
const ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
const Duration lookbackDuration = armarx::fromIce<Duration>(request.lookback);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 39e5d51ab0cf6d181f7c0d348655e38a6d982f9c..433b2d248d130847af8f2873e85183178707f813 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -450,6 +450,7 @@ namespace armarx::armem::server::obj::instance
SegmentAdapter::getAvailableObjectPoseEngines(const Ice::Current&)
{
armem::prediction::data::PredictionEngine engine;
+ // TODO(phesch): Make this a constant somewhere
engine.engineID = "Linear Position Regression";
return { engine };
}