diff --git a/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg b/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
index b023833a9d18ac3f4c1d92911b29ad1f8699a22c..55d034d9c687c16ad2f34f458f85ca66da54cdff 100644
--- a/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
+++ b/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -109,6 +109,14 @@
# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.RobotToArViz.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ArVizStorageName = ArVizStorage
+
+
# ArmarX.RobotToArViz.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index 6359111c24ef51d35360ff8def6853f9fdae55ae..10a8b7ae1b665966787ea6a056ab3f1056f8abeb 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -22,7 +22,13 @@
#include "ObjectPoseClientExample.h"
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
namespace armarx
@@ -32,6 +38,12 @@ namespace armarx
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ defs->optional(p.predictionsPerObject, "predictions.NumberPerObject",
+ "How many predictions with increasing time offsets to make per object.");
+
+ defs->optional(p.millisecondPredictionIncrement, "predictions.TimeIncrement",
+ "The size of the prediction time offset increment in milliseconds.");
+
return defs;
}
@@ -81,6 +93,7 @@ namespace armarx
sendDebugObserverBatch();
}
+ viz::StagedCommit stage = arviz.stage();
{
// Visualize the objects.
viz::Layer layer = arviz.layer("Objects");
@@ -91,10 +104,74 @@ namespace armarx
.fileByObjectFinder(objectPose.objectID)
.alpha(objectPose.confidence));
}
- arviz.commit({layer});
+ stage.add(layer);
}
+ stage.add(visualizePredictions(client, objectPoses));
+ arviz.commit(stage);
cycle.waitForCycleDuration();
}
}
-}
+
+
+ viz::Layer
+ ObjectPoseClientExample::visualizePredictions(const objpose::ObjectPoseClient& client,
+ const objpose::ObjectPoseSeq& objectPoses)
+ {
+ viz::Layer layer = arviz.layer("PredictionArray");
+
+ objpose::ObjectPosePredictionRequestSeq requests;
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ for (int i = 0; i < p.predictionsPerObject; ++i)
+ {
+ objpose::ObjectPosePredictionRequest request;
+ toIce(request.objectID, objectPose.objectID);
+ request.settings.predictionEngineID = "Linear Position Regression";
+ toIce(request.timestamp,
+ DateTime::Now() +
+ Duration::MilliSeconds((i + 1) * p.millisecondPredictionIncrement));
+ toIce(request.timeWindow, Duration::Seconds(10));
+ requests.push_back(request);
+ }
+ }
+
+ objpose::ObjectPosePredictionResultSeq results;
+ try
+ {
+ results = client.objectPoseStorage->predictObjectPoses(requests);
+ }
+ catch (const Ice::LocalException& e)
+ {
+ ARMARX_INFO << "Failed to get predictions for object poses: " << e.what();
+ }
+
+ for (size_t i = 0; i < results.size(); ++i)
+ {
+ const objpose::ObjectPosePredictionResult& result = results.at(i);
+ if (result.success)
+ {
+ auto predictedPose = armarx::fromIce<objpose::ObjectPose>(result.prediction);
+ Eigen::Vector3f predictedPosition =
+ simox::math::position(predictedPose.objectPoseGlobal);
+ int alpha =
+ (p.predictionsPerObject - (static_cast<int>(i) % p.predictionsPerObject)) *
+ 255 / p.predictionsPerObject; // NOLINT
+ layer.add(
+ viz::Arrow(predictedPose.objectID.str() + " Linear Prediction " +
+ std::to_string(i % p.predictionsPerObject))
+ .fromTo(simox::math::position(
+ objectPoses.at(i / p.predictionsPerObject).objectPoseGlobal),
+ predictedPosition)
+ .color(viz::Color::green(255, alpha))); // NOLINT
+ }
+ else
+ {
+ ARMARX_INFO << "Prediction for object '" << result.prediction.objectID
+ << "' failed: " << result.errorMessage;
+ }
+ }
+
+ return layer;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
index c49e28f3518755cc492ffa31b67710b005abf7a3..8cc667b26bad490c8fb314a6ef759e52f850430d 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
@@ -75,10 +75,20 @@ namespace armarx
void objectProcessingTaskRun();
+ viz::Layer visualizePredictions(const objpose::ObjectPoseClient& client,
+ const objpose::ObjectPoseSeq& objectPoses);
+
private:
armarx::SimpleRunningTask<>::pointer_type objectProcessingTask;
+ struct Properties
+ {
+ int predictionsPerObject = 5; // NOLINT
+ int millisecondPredictionIncrement = 200; // NOLINT
+ };
+ Properties p;
+
};
}
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 0dd286903f1cdfefcc683cb1883ac12674971ca7..cd7e8b18230e38d21cf700f04c116cca830eaa14 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -218,7 +218,7 @@ namespace armarx
ExampleMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
{
armem::prediction::data::PredictionResultSeq result;
- for (auto request : requests)
+ for (const auto& request : requests)
{
result.push_back(predictSingle(request));
}
@@ -254,29 +254,31 @@ namespace armarx
else
{
result.success = false;
- result.errorMessage <<
- "Could not find entity referenced by MemoryID " << boID;
+ result.errorMessage =
+ "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
}
}
else
{
result.success = false;
- result.errorMessage << "Could not find entity referenced by MemoryID " << boID;
+ result.errorMessage =
+ "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
}
}
else
{
result.success = false;
- result.errorMessage << "This memory does not support the prediction engine \"" << engine << "\"";
+ result.errorMessage =
+ "This memory does not support the prediction engine '" + engine + "'.";
}
return result.toIce();
}
- armem::prediction::data::PredictionEngineSeq
+ armem::prediction::data::EngineSupportMap
ExampleMemory::getAvailableEngines()
{
- return {{"Latest"}};
+ return {{armarx::toIce<armem::data::MemoryID>(workingMemory().id()), {{"Latest"}}}};
}
// REMOTE GUI
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index de89914ba8ed44517ce40639849afa826d022ef1..fa9c2b1eacc314bcb37de05f4f19039d9bcbd582 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -76,7 +76,7 @@ namespace armarx
armem::prediction::data::PredictionResultSeq
predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
- armem::prediction::data::PredictionEngineSeq getAvailableEngines() override;
+ armem::prediction::data::EngineSupportMap getAvailableEngines() override;
protected:
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index e67746b42af6fefdc379a50bb3f9559eee8b136b..02ac6e6aec08d86253f0512584a9921f8063bf9e 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -23,6 +23,13 @@
#include "ObjectMemory.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.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_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(predictionTimeWindow, "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
return defs;
}
@@ -215,6 +225,84 @@ 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;
+ toIce(objPoseRequest.timeWindow, Duration::SecondsDouble(predictionTimeWindow));
+ objPoseRequest.objectID = toIce(ObjectID(request.snapshotID.entityName));
+ objPoseRequest.settings = request.settings;
+ toIce(objPoseRequest.timestamp, boRequest.snapshotID.timestamp);
+ objpose::ObjectPosePredictionResult objPoseResult =
+ predictObjectPoses({objPoseRequest}).at(0);
+ result.success = objPoseResult.success;
+ result.errorMessage = 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..7067f42da1344fb4aa1936dfcafaa2b77f3df599 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 predictionTimeWindow = 2;
+
clazz::Segment classSegment;
attachments::Segment attachmentSegment;
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index 16f222d827a91c7eba560695487f756b3e7de2f7..c544ea8dd4b54eb15cebd9fb10e36a00e0623234 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -27,7 +27,6 @@
#include <ArmarXCore/core/util/algorithm.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
// #define DEBUG_POS_CTRL
#ifdef DEBUG_POS_CTRL
#include <boost/circular_buffer.hpp>
@@ -203,20 +202,6 @@ namespace armarx
{
public:
RampedAccelerationVelocityControllerConfiguration() = default;
- static RampedAccelerationVelocityControllerConfigurationPtr CreateFromXml(DefaultRapidXmlReaderNode node)
- {
- RampedAccelerationVelocityControllerConfigurationPtr config(new RampedAccelerationVelocityControllerConfiguration());
- FillFromXml(*config, node);
- return config;
- }
- static void FillFromXml(RampedAccelerationVelocityControllerConfiguration& config, DefaultRapidXmlReaderNode node)
- {
- config.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
- config.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
- config.jerk = node.first_node("jerk").value_as_float();
- config.maxDt = node.first_node("maxDt").value_as_float();
- config.directSetVLimit = node.first_node("directSetVLimit").value_as_float();
- }
float maxVelocityRad;
float maxDecelerationRad;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
index b18689724a120d17a3eff61158d9e57cfd2ee7a9..d393f02ea5f90ff5ed41eab507fb6bf94cc7415a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -23,6 +23,7 @@
#pragma once
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
diff --git a/source/RobotAPI/interface/armem/prediction.ice b/source/RobotAPI/interface/armem/prediction.ice
index f42535928e434be36702a949f9929246a8f08f7b..a2aae725e3ef5a399ccfacf819d07eb56693f8e4 100644
--- a/source/RobotAPI/interface/armem/prediction.ice
+++ b/source/RobotAPI/interface/armem/prediction.ice
@@ -17,6 +17,7 @@ module armarx
string engineID;
}
sequence<PredictionEngine> PredictionEngineSeq;
+ dictionary<armem::data::MemoryID, PredictionEngineSeq> EngineSupportMap;
// Settings to use for a given prediction (e.g. which engine to use)
struct PredictionSettings
diff --git a/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
index 5b8b668ab72a3c4aef15720e450813891b140124..ead5ad03b5edd7f1bbb39dd1c528376005c8b041 100644
--- a/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
@@ -13,14 +13,15 @@ module armarx
{
/* Request multiple predictions from the memory.
* The timestamp to requst a prediction for is encoded in the MemoryIDs.
- * The results are all packed into one QueryResult.
*/
prediction::data::PredictionResultSeq
predict(prediction::data::PredictionRequestSeq requests);
- /* Get the list of engines the memory offers for use in predictions.
+ /* Get a map from memory segments to the prediction engines they support.
+ * Best used with the armem::core::PrefixMap to easily retrieve
+ * the available engine selection for fully evaluated MemoryIDs.
*/
- prediction::data::PredictionEngineSeq getAvailableEngines();
+ prediction::data::EngineSupportMap getAvailableEngines();
}
};
};
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
index 1dfe8b9f299d24684a0acdd3d6f53ed61063d397..a069da9078a11beeb140d5477dd847beb85cf2c3 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
@@ -152,6 +152,23 @@ module armarx
long discardUpdatesUntilMilliSeconds = -1;
};
+ struct ObjectPosePredictionRequest
+ {
+ armarx::data::ObjectID objectID;
+ armarx::core::time::dto::DateTime timestamp;
+ armarx::core::time::dto::Duration timeWindow;
+ armem::prediction::data::PredictionSettings settings;
+ };
+ sequence<ObjectPosePredictionRequest> ObjectPosePredictionRequestSeq;
+ struct ObjectPosePredictionResult
+ {
+ bool success = false;
+ string errorMessage;
+
+ data::ObjectPose prediction;
+ };
+ sequence<ObjectPosePredictionResult> ObjectPosePredictionResultSeq;
+
interface ObjectPoseStorageInterface extends
ObjectPoseTopic
{
@@ -191,6 +208,15 @@ module armarx
SignalHeadMovementOutput signalHeadMovement(SignalHeadMovementInput input);
+ // Predicting
+
+ // Request predictions for the poses of the given objects at the given timestamps.
+ ObjectPosePredictionResultSeq
+ predictObjectPoses(ObjectPosePredictionRequestSeq requests);
+
+ // Get the prediction engines available for use with this storage interface.
+ armem::prediction::data::PredictionEngineSeq getAvailableObjectPoseEngines();
+
};
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 2b2fb5f9d4ea899930351b1234873b2354cb50bf..ec6658fa85afba280fef0613fbc60596da57ee8a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -31,6 +31,7 @@ set(LIB_FILES
plugins/ObjectPoseClientPlugin.cpp
plugins/RequestedObjects.cpp
+ predictions.cpp
util.cpp
)
set(LIB_HEADERS
@@ -57,6 +58,7 @@ set(LIB_HEADERS
plugins/ObjectPoseClientPlugin.h
plugins/RequestedObjects.h
+ predictions.h
util.h
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..427868519c15f4f3c31342f980d3ee245f528a88
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -0,0 +1,87 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects
+ * @author phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "predictions.h"
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+
+namespace armarx::objpose
+{
+
+ objpose::ObjectPosePredictionResult
+ predictObjectPoseLinear(const std::map<DateTime, ObjectPose>& poses,
+ const DateTime& time,
+ const ObjectPose& latestPose)
+ {
+ objpose::ObjectPosePredictionResult result;
+ result.success = false;
+
+ const DateTime timeOrigin = DateTime::Now();
+
+ std::vector<double> timestampsSec;
+ std::vector<Eigen::Vector3d> positions;
+
+ // ToDo: How to handle attached objects?
+ for (const auto& [timestamp, pose] : poses)
+ {
+ timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
+ positions.emplace_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
+ }
+
+ ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
+
+ Eigen::Vector3d prediction;
+ // Static objects don't move. Objects that haven't moved for a while probably won't either.
+ if (timestampsSec.size() <= 1 || latestPose.isStatic)
+ {
+ prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
+ }
+ else
+ {
+ using simox::math::LinearRegression3d;
+ const bool inputOffset = false;
+
+ const LinearRegression3d model =
+ LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+ const auto predictionTime = armarx::fromIce<DateTime>(time);
+ prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ }
+
+ // Used as a template for the returned pose prediction.
+ ObjectPose latestCopy = latestPose;
+
+ // Reuse the rotation from the latest pose.
+ // This is a pretty generous assumption, but the linear model doesn't cover rotations,
+ // so it's our best guess.
+ Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
+ simox::math::position(latest) = prediction.cast<float>();
+ latestCopy.setObjectPoseGlobal(latest);
+
+ result.success = true;
+ result.prediction = latestCopy.toIce();
+
+ return result;
+ }
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.h b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d9b647f9dd81a1e04ff435a0e20c94368cdb89f4
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects
+ * @author phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
+
+namespace armarx::objpose
+{
+ /**
+ * @brief Predict the pose of an object given a history of poses
+ * based on a linear regression.
+ *
+ * If `poses` is empty, `latestPose` is returned.
+ *
+ * @param poses the history of poses to base the prediction on
+ * @param time the timestamp to make the prediction for
+ * @param latestPose used for metadata so the result is valid even if poses is empty
+ * @param settings the settings to use for the prediction
+ * @return the result of the prediction
+ */
+ objpose::ObjectPosePredictionResult
+ predictObjectPoseLinear(
+ const std::map<DateTime, ObjectPose>& poses,
+ const DateTime& time,
+ const ObjectPose& latestPose);
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/armem/client/Prediction.cpp b/source/RobotAPI/libraries/armem/client/Prediction.cpp
index f58649a457fc8117309ceaf5426a0a84540f35f5..dce9473bd58d557d3a37a511c7c257c65305bee8 100644
--- a/source/RobotAPI/libraries/armem/client/Prediction.cpp
+++ b/source/RobotAPI/libraries/armem/client/Prediction.cpp
@@ -118,7 +118,7 @@ namespace armarx::armem::client
toIce(armem::prediction::data::PredictionResult& ice, const PredictionResult& result)
{
ice.success = result.success;
- ice.errorMessage = result.errorMessage.str();
+ ice.errorMessage = result.errorMessage;
toIce(ice.snapshotID, result.snapshotID);
ice.prediction = result.prediction->toAronDictDTO();
}
@@ -127,7 +127,7 @@ namespace armarx::armem::client
fromIce(const armem::prediction::data::PredictionResult& ice, PredictionResult& result)
{
result.success = ice.success;
- result.errorMessage.str(ice.errorMessage);
+ result.errorMessage = ice.errorMessage;
fromIce(ice.snapshotID, result.snapshotID);
result.prediction = aron::data::Dict::FromAronDictDTO(ice.prediction);
}
diff --git a/source/RobotAPI/libraries/armem/client/Prediction.h b/source/RobotAPI/libraries/armem/client/Prediction.h
index a1538758519228b93e4c6f95e131eed6c7fe9bad..0e9ac30d5e6e797dd228ca6f313c6cdbed19bf5b 100644
--- a/source/RobotAPI/libraries/armem/client/Prediction.h
+++ b/source/RobotAPI/libraries/armem/client/Prediction.h
@@ -57,7 +57,7 @@ namespace armarx::armem::client
struct PredictionResult
{
bool success;
- std::stringstream errorMessage;
+ std::string errorMessage;
armem::MemoryID snapshotID;
aron::data::DictPtr prediction;
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 52be34eb42c8f5c54f2138731690a5f43728d6f8..ce2d0cfcfa41bedc7f6cab56f45e79d959bc68da 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -479,7 +479,7 @@ namespace armarx::armem::client
return boResults;
}
- std::vector<PredictionEngine>
+ armem::prediction::data::EngineSupportMap
Reader::getAvailablePredictionEngines() const
{
if (!predictionPrx)
@@ -489,7 +489,7 @@ namespace armarx::armem::client
"Prediction interface proxy must be set to request a prediction.");
}
- std::vector<armem::prediction::data::PredictionEngine> engines;
+ armem::prediction::data::EngineSupportMap engines;
try
{
engines = predictionPrx->getAvailableEngines();
@@ -499,13 +499,13 @@ namespace armarx::armem::client
// Just leave engines empty in this case.
}
- std::vector<PredictionEngine> boEngines;
- boEngines.reserve(engines.size());
- for (const auto& engine : engines)
- {
- boEngines.push_back(armarx::fromIce<PredictionEngine>(engine));
- }
+ //std::vector<PredictionEngine> boEngines;
+ //boEngines.reserve(engines.size());
+ //for (const auto& engine : engines)
+ //{
+ // boEngines.push_back(armarx::fromIce<PredictionEngine>(engine));
+ //}
- return boEngines;
+ return engines;
}
} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index beb0e53f5ff25078cb881d4477c19c5cd291fe1a..34b3ce320182c7995412dc7e02b04c58fe1a0dfa 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -178,6 +178,7 @@ namespace armarx::armem::client
*/
std::vector<PredictionResult> predict(const std::vector<PredictionRequest>& requests) const;
+ //TODO(phesch): Fix this interface with the proper type
/**
* @brief Get the list of prediction engines supported by the memory.
*
@@ -185,7 +186,7 @@ namespace armarx::armem::client
*
* @return the vector of supported prediction engines
*/
- std::vector<PredictionEngine> getAvailablePredictionEngines() const;
+ armem::prediction::data::EngineSupportMap getAvailablePredictionEngines() const;
inline operator bool() const
{
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 115e38311ae09d9d5e1362bf362b1207a76173a8..f690cbaeef48944c8568ef58b0aae08b82735513 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -127,14 +127,14 @@ namespace armarx::armem::server::plugins
{
armem::client::PredictionResult singleResult;
singleResult.success = false;
- singleResult.errorMessage << "This memory does not implement predictions.";
+ singleResult.errorMessage = "This memory does not implement predictions.";
singleResult.prediction = nullptr;
result.push_back(singleResult.toIce());
}
return result;
}
- armem::prediction::data::PredictionEngineSeq
+ armem::prediction::data::EngineSupportMap
ReadWritePluginUser::getAvailableEngines()
{
return {};
@@ -147,7 +147,7 @@ namespace armarx::armem::server::plugins
return predict(requests);
}
- armem::prediction::data::PredictionEngineSeq
+ armem::prediction::data::EngineSupportMap
ReadWritePluginUser::getAvailableEngines(const ::Ice::Current& /*unused*/)
{
return getAvailableEngines();
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
index 1e0b4bc82568382ead82cbf7b337529db4f46251..37f25fd8f3199179402755dc76d98367964aa2af 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -57,10 +57,10 @@ namespace armarx::armem::server::plugins
// PredictingInterface interface
virtual armem::prediction::data::PredictionResultSeq predict(const armem::prediction::data::PredictionRequestSeq& requests);
- virtual armem::prediction::data::PredictionEngineSeq getAvailableEngines();
+ virtual armem::prediction::data::EngineSupportMap getAvailableEngines();
virtual armem::prediction::data::PredictionResultSeq predict(const armem::prediction::data::PredictionRequestSeq& requests, const ::Ice::Current&) override;
- virtual armem::prediction::data::PredictionEngineSeq getAvailableEngines(const ::Ice::Current&) override;
+ virtual armem::prediction::data::EngineSupportMap getAvailableEngines(const ::Ice::Current&) override;
public:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index b30c1a048e5a85f62a6efe79ae7d82ecab783ed8..6bad86c0f05ccdaa33855d9411c27d9cc04d50f6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -37,8 +37,10 @@
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
#include <Eigen/Geometry>
+#include <Eigen/Dense>
#include <sstream>
@@ -545,6 +547,35 @@ namespace armarx::armem::server::obj::instance
}
+ std::map<DateTime, objpose::ObjectPose>
+ Segment::getObjectPosesInRange(const wm::Entity& entity,
+ const DateTime& start,
+ const DateTime& end)
+ {
+ std::map<DateTime, objpose::ObjectPose> result;
+
+ entity.forEachSnapshotInTimeRange(
+ start,
+ end,
+ [&result](const wm::EntitySnapshot& snapshot)
+ {
+ snapshot.forEachInstance(
+ [&result, &snapshot](const wm::EntityInstance& instance)
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ ObjectPose pose;
+ fromAron(dto, pose);
+
+ result.emplace(snapshot.time(), pose);
+ });
+ });
+
+ return result;
+ }
+
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index da97aac841559b845fb308e93ac572739efb7c3c..0aa9b930411f0a6f6336ea439fbb98ff6dc74a50 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -18,6 +18,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
@@ -104,7 +105,8 @@ namespace armarx::armem::server::obj::instance
::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
-
+ static std::map<DateTime, ObjectPose>
+ getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
private:
@@ -139,7 +141,6 @@ namespace armarx::armem::server::obj::instance
std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
-
friend struct DetachVisitor;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index b7f7a63085b31cb3672ad6c0307fc5afe2096c4e..7c7dac41c1a74bca1606721b86de565f821a7608 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -25,8 +25,14 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <VirtualRobot/Robot.h>
@@ -88,6 +94,7 @@ namespace armarx::armem::server::obj::instance
robotHead.fetchDatafields();
visu.arviz = arviz;
+
if (!visu.updateTask)
{
visu.updateTask = new SimpleRunningTask<>([this]()
@@ -97,6 +104,7 @@ namespace armarx::armem::server::obj::instance
visu.updateTask->start();
}
+
segment.connect(arviz);
}
@@ -431,8 +439,94 @@ namespace armarx::armem::server::obj::instance
return robotHead.signalHeadMovement(input);
}
+ objpose::ObjectPosePredictionResultSeq
+ SegmentAdapter::predictObjectPoses(const objpose::ObjectPosePredictionRequestSeq& requests,
+ const Ice::Current&)
+ {
+ objpose::ObjectPosePredictionResultSeq results;
+ std::vector<std::map<DateTime, objpose::ObjectPose>> poses;
+ std::vector<objpose::ObjectPose> latestPoses;
+
+ segment.doLocked(
+ [this, &requests, &results, &poses, &latestPoses]()
+ {
+ for (const auto& request : requests)
+ {
+ auto& result = results.emplace_back();
+ const ObjectID objID = armarx::fromIce(request.objectID);
+
+ const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
+
+ const wm::Entity* entity = segment.findObjectEntity(objID);
+ // Use result.success as a marker for whether to continue later
+ result.success = false;
+ poses.emplace_back();
+ latestPoses.emplace_back();
+ if (!entity)
+ {
+ std::stringstream sstream;
+ sstream << "Could not find object with ID " << objID << ".";
+ result.errorMessage = sstream.str();
+ continue;
+ }
+
+ const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
+ if (!dto)
+ {
+ std::stringstream sstream;
+ sstream << "No snapshots of the object " << objID << " were found."
+ << " Cannot make a prediction.";
+ result.errorMessage = sstream.str();
+ continue;
+ }
+
+ result.success = true;
+ poses.back() = segment.getObjectPosesInRange(
+ *entity, Time::Now() - timeWindow, Time::Invalid());
+ latestPoses.back() = aron::fromAron<objpose::ObjectPose>(dto.value().pose);
+ }
+ });
+
+ for (size_t i = 0; i < requests.size(); ++i)
+ {
+ const objpose::ObjectPosePredictionRequest& request = requests.at(i);
+ objpose::ObjectPosePredictionResult& result = results.at(i);
+
+ if (result.success)
+ {
+ armem::client::PredictionSettings settings =
+ armem::client::PredictionSettings::fromIce(request.settings);
+
+ if (settings.predictionEngineID.empty()
+ or settings.predictionEngineID == "Linear Position Regression")
+ {
+ result = objpose::predictObjectPoseLinear(
+ poses.at(i),
+ armarx::fromIce<DateTime>(request.timestamp),
+ latestPoses.at(i));
+ }
+ else
+ {
+ result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
+ "' not available for object pose prediction.";
+ result.success = false;
+ }
+ }
+ }
+ return results;
+ }
+
+ armem::prediction::data::PredictionEngineSeq
+ SegmentAdapter::getAvailableObjectPoseEngines(const Ice::Current&)
+ {
+ armem::prediction::data::PredictionEngine engine;
+ // TODO(phesch): Make this a constant somewhere
+ engine.engineID = "Linear Position Regression";
+ return { engine };
+ }
- void SegmentAdapter::visualizeRun()
+ void
+ SegmentAdapter::visualizeRun()
{
ObjectFinder objectFinder;
@@ -447,9 +541,10 @@ namespace armarx::armem::server::obj::instance
TIMING_START(tVisu);
objpose::ObjectPoseMap objectPoses;
+ std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>> poseHistories;
visu.minConfidence = -1;
- segment.doLocked([this, &objectPoses, &objectFinder]()
+ segment.doLocked([this, &objectPoses, &poseHistories, &objectFinder]()
{
const Time now = Time::Now();
@@ -467,11 +562,27 @@ namespace armarx::armem::server::obj::instance
{
visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
}
+
+ // Get histories.
+ for (const auto& [id, objectPose] : objectPoses)
+ {
+ const wm::Entity* entity = segment.findObjectEntity(id);
+ if (entity != nullptr)
+ {
+ poseHistories[id] = instance::Segment::getObjectPosesInRange(
+ *entity,
+ Time::Now() - Duration::SecondsDouble(
+ visu.linearPredictionTimeWindowSeconds),
+ Time::Invalid());
+ }
+ }
});
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
+ const std::vector<viz::Layer> layers =
+ visu.visualizeCommit(objectPoses, poseHistories, objectFinder);
arviz.commit(layers);
+
TIMING_END_STREAM(tVisu, ARMARX_VERBOSE);
if (debugObserver)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index 353f2e0644dc044edfd1f260a76eb0c5d7857a11..69595f7b640ec57cb1dcd7222233e7681f6f20e1 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -107,9 +107,16 @@ namespace armarx::armem::server::obj::instance
virtual objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+ // PREDICTING
- private:
+ virtual objpose::ObjectPosePredictionResultSeq
+ predictObjectPoses(const objpose::ObjectPosePredictionRequestSeq& requests,
+ ICE_CURRENT_ARG) override;
+
+ virtual armem::prediction::data::PredictionEngineSeq
+ getAvailableObjectPoseEngines(ICE_CURRENT_ARG) override;
+ private:
void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index e5bfd134140eae11fe3c6135d781dde2571f98eb..47206a42fa74036015b875f104c0503172d9ac2e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -3,9 +3,14 @@
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
namespace armarx::armem::server::obj::instance
@@ -45,43 +50,77 @@ namespace armarx::armem::server::obj::instance
defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
"Prefer articulated object models if available.");
+
+ defs->optional(showLinearPredictions, prefix + "predictions.linear.show",
+ "Show arrows linearly predicting object positions.");
+ defs->optional(linearPredictionTimeOffsetSeconds, prefix + "predictions.linear.timeOffset",
+ "The offset (in seconds) to the current time to make predictions for.");
+ defs->optional(linearPredictionTimeWindowSeconds, prefix + "predictions.linear.timeWindow",
+ "The time window (in seconds) into the past to perform the regression on.");
}
- std::vector<viz::Layer> Visu::visualizeCommit(
+ std::vector<viz::Layer>
+ Visu::visualizeCommit(
const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
+ const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
+ poseHistories,
const ObjectFinder& objectFinder) const
{
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
- layers.push_back(visualizeProvider(name, poses, objectFinder));
+ auto poseHistoryMap = poseHistories.find(name);
+ if (poseHistoryMap != poseHistories.end())
+ {
+ layers.push_back(visualizeProvider(name, poses, poseHistoryMap->second, objectFinder));
+ }
+ else
+ {
+ layers.push_back(visualizeProvider(name, poses, {}, objectFinder));
+ }
}
return layers;
}
- std::vector<viz::Layer> Visu::visualizeCommit(
- const objpose::ObjectPoseSeq& objectPoses,
- const ObjectFinder& objectFinder) const
+ std::vector<viz::Layer>
+ Visu::visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
+ const ObjectFinder& objectFinder) const
{
std::map<std::string, viz::Layer> stage;
- for (const objpose::ObjectPose& objectPose : objectPoses)
+ for (size_t i = 0; i < objectPoses.size(); ++i)
{
- visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
+ visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
+ objectPoses.at(i),
+ poseHistories.at(i),
+ objectFinder);
}
return simox::alg::get_values(stage);
}
- std::vector<viz::Layer> Visu::visualizeCommit(
+ std::vector<viz::Layer>
+ Visu::visualizeCommit(
const objpose::ObjectPoseMap& objectPoses,
+ const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder) const
{
std::map<std::string, viz::Layer> stage;
- for (const auto& [id, objectPose] : objectPoses)
+ for (const auto& [id, pose] : objectPoses)
{
- visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
+
+ auto poseHistoryMap = poseHistories.find(id);
+ if (poseHistoryMap != poseHistories.end())
+ {
+ visualizeObjectPose(
+ getLayer(pose.providerName, stage), pose, poseHistoryMap->second, objectFinder);
+ }
+ else
+ {
+ visualizeObjectPose(getLayer(pose.providerName, stage), pose, {}, objectFinder);
+ }
}
return simox::alg::get_values(stage);
}
@@ -100,15 +139,17 @@ namespace armarx::armem::server::obj::instance
}
- viz::Layer Visu::visualizeProvider(
+ viz::Layer
+ Visu::visualizeProvider(
const std::string& providerName,
const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder) const
{
viz::Layer layer = arviz.layer(providerName);
- for (const objpose::ObjectPose& objectPose : objectPoses)
+ for (size_t i = 0; i < poseHistories.size(); ++i)
{
- visualizeObjectPose(layer, objectPose, objectFinder);
+ visualizeObjectPose(layer, objectPoses.at(i), poseHistories.at(i), objectFinder);
}
return layer;
}
@@ -116,6 +157,7 @@ namespace armarx::armem::server::obj::instance
void Visu::visualizeObjectPose(
viz::Layer& layer,
const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
const ObjectFinder& objectFinder) const
{
const bool show = objectPose.confidence >= minConfidence;
@@ -236,6 +278,28 @@ namespace armarx::armem::server::obj::instance
}
}
}
+ if (showLinearPredictions)
+ {
+ auto predictionResult = objpose::predictObjectPoseLinear(
+ poseHistory,
+ Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds),
+ objectPose);
+ if (predictionResult.success)
+ {
+ auto predictedPose =
+ armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
+ Eigen::Vector3f predictedPosition = simox::math::position(
+ inGlobalFrame ? predictedPose.objectPoseGlobal : predictedPose.objectPoseRobot);
+ layer.add(viz::Arrow(key + " Linear Prediction")
+ .fromTo(simox::math::position(pose), predictedPosition)
+ .color(viz::Color::blue()));
+ }
+ else
+ {
+ ARMARX_INFO << "Linear prediction for visualization failed: "
+ << predictionResult.errorMessage;
+ }
+ }
}
@@ -270,6 +334,7 @@ namespace armarx::armem::server::obj::instance
useArticulatedModels.setValue(visu.useArticulatedModels);
+
GridLayout grid;
int row = 0;
grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
@@ -291,10 +356,13 @@ namespace armarx::armem::server::obj::instance
grid.add(gaussians.group, {row, 0}, {1, 4});
row++;
-
grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
row++;
+ linearPredictions.setup(visu);
+ grid.add(linearPredictions.group, {row, 0}, {1, 4});
+ row++;
+
group = GroupBox();
group.setLabel("Visualization");
group.addChild(grid);
@@ -342,6 +410,48 @@ namespace armarx::armem::server::obj::instance
visu.gaussiansOrientationDisplaced = gaussians.orientationDisplaced.getValue();
visu.useArticulatedModels = useArticulatedModels.getValue();
+
+ linearPredictions.update(visu);
+ }
+
+
+ void Visu::RemoteGui::LinearPredictions::setup(const Visu& visu)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ show.setValue(visu.showLinearPredictions);
+ timeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
+ timeOffsetSeconds.setRange(-1e6, 1e6);
+ timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
+
+ timeWindowSeconds.setValue(visu.linearPredictionTimeWindowSeconds);
+ timeWindowSeconds.setRange(0, 1e6);
+ timeWindowSeconds.setSteps(2 * 1000 * 1000);
+
+
+ GridLayout grid;
+ int row = 0;
+
+ grid.add(Label("Show"), {row, 0}).add(show, {row, 1});
+ row++;
+ grid.add(Label("Time (seconds from now):"), {row, 0})
+ .add(timeOffsetSeconds, {row, 1});
+ row++;
+ grid.add(Label("Time Window (seconds):"), {row, 0})
+ .add(timeWindowSeconds, {row, 1});
+ row++;
+
+ group = GroupBox();
+ group.setLabel("Linear Predictions");
+ group.addChild(grid);
+ }
+
+
+ void Visu::RemoteGui::LinearPredictions::update(Visu& visu)
+ {
+ visu.showLinearPredictions = show.getValue();
+ visu.linearPredictionTimeOffsetSeconds = timeOffsetSeconds.getValue();
+ visu.linearPredictionTimeWindowSeconds = timeWindowSeconds.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index f1ce436435eec9de5c9fd1cbffcea98bc7201410..0341f514421e10a9158a446c1cb2c4a7eb3625e6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -7,6 +7,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
@@ -29,22 +30,26 @@ namespace armarx::armem::server::obj::instance
std::vector<viz::Layer> visualizeCommit(
const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
- const ObjectFinder& objectFinder
- ) const;
+ const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
+ poseHistories,
+ const ObjectFinder& objectFinder) const;
/// Visualize the given object poses, with one layer per provider.
std::vector<viz::Layer> visualizeCommit(
const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder
) const;
std::vector<viz::Layer> visualizeCommit(
const objpose::ObjectPoseMap& objectPoses,
+ const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder
) const;
viz::Layer visualizeProvider(
const std::string& providerName,
const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder
) const;
@@ -52,6 +57,7 @@ namespace armarx::armem::server::obj::instance
void visualizeObjectPose(
viz::Layer& layer,
const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
const ObjectFinder& objectFinder
) const;
@@ -86,8 +92,12 @@ namespace armarx::armem::server::obj::instance
/// Prefer articulated models if available.
bool useArticulatedModels = true;
- SimpleRunningTask<>::pointer_type updateTask;
+ /// Linear prediction arrows for object positions.
+ bool showLinearPredictions = false;
+ float linearPredictionTimeOffsetSeconds = 1;
+ float linearPredictionTimeWindowSeconds = 2;
+ SimpleRunningTask<>::pointer_type updateTask;
struct RemoteGui
{
@@ -121,6 +131,20 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::CheckBox useArticulatedModels;
+ struct LinearPredictions
+ {
+ armarx::RemoteGui::Client::CheckBox show;
+
+ armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
+ armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
+
+ armarx::RemoteGui::Client::GroupBox group;
+
+ void setup(const Visu& data);
+ void update(Visu& data);
+ };
+ LinearPredictions linearPredictions;
+
void setup(const Visu& visu);
void update(Visu& visu);
};