From f33d0017ff2e395430eac6017f31b9c034ca3eb8 Mon Sep 17 00:00:00 2001
From: Fabian Peller-Konrad <fabian.peller-konrad@kit.edu>
Date: Fri, 12 Aug 2022 10:41:34 +0200
Subject: [PATCH] added mutexes to memory viewer to avoid data races. Removed
data race of removed address in asynch method
---
.../libraries/armem/client/Reader.cpp | 2 +
.../RobotAPI/libraries/armem/client/Reader.h | 2 +-
.../armem/server/MemoryToIceAdapter.cpp | 1 -
.../libraries/armem_gui/MemoryViewer.cpp | 94 ++++---
.../libraries/armem_gui/MemoryViewer.h | 17 +-
.../armem_gui/query_widgets/QueryWidget.cpp | 8 +-
.../armem_gui/query_widgets/QueryWidget.h | 4 +
source/RobotAPI/libraries/core/FramedPose.cpp | 233 ++++++++++++++----
source/RobotAPI/libraries/core/FramedPose.h | 29 ++-
9 files changed, 289 insertions(+), 101 deletions(-)
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 872f4b7dd..48efb7bb9 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -16,6 +16,7 @@
#include "query/Builder.h"
#include "query/query_fns.h"
+#include <mutex>
namespace armarx::armem::client
{
@@ -46,6 +47,7 @@ namespace armarx::armem::client
try
{
+ ARMARX_TRACE;
result = readingPrx->query(input);
}
catch (const Ice::LocalException& e)
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 8f78b33ec..875250eb8 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -4,6 +4,7 @@
// STD/STL
#include <optional>
#include <vector>
+#include <mutex>
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
@@ -203,7 +204,6 @@ namespace armarx::armem::client
server::ReadingMemoryInterfacePrx readingPrx;
server::PredictingMemoryInterfacePrx predictionPrx;
-
};
} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 93673c8aa..6aa17f4ce 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -279,7 +279,6 @@ namespace armarx::armem::server
if (not ltmResult.empty())
{
- ARMARX_INFO << "The LTM returned data after query";
// convert memory ==> meaning resolving references
// upon query, the LTM only returns a structure of the data (memory without data)
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index be54e55ab..699a41d01 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -212,6 +212,7 @@ namespace armarx::armem::gui
component.usingProxy(debugObserverName);
}
+ is_initialized = true;
emit initialized();
}
@@ -234,8 +235,10 @@ namespace armarx::armem::gui
{
component.getProxy(debugObserver, debugObserverName, false, "", false);
}
+
updateWidget->startTimerIfEnabled();
+ is_connected = true;
emit connected();
}
@@ -243,6 +246,7 @@ namespace armarx::armem::gui
void
MemoryViewer::onDisconnect(ManagedIceObject&)
{
+ periodicUpdateTimer->stop();
updateWidget->stopTimer();
emit disconnected();
@@ -252,6 +256,7 @@ namespace armarx::armem::gui
const armem::wm::Memory*
MemoryViewer::getSingleMemoryData(const std::string& memoryName)
{
+ std::scoped_lock l(memoryDataMutex);
auto it = memoryData.find(memoryName);
if (it == memoryData.end())
{
@@ -308,6 +313,8 @@ namespace armarx::armem::gui
void
MemoryViewer::startLTMRecording()
{
+ std::scoped_lock l(memoryReaderMutex);
+
TIMING_START(MemoryStartRecording);
auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
@@ -327,6 +334,8 @@ namespace armarx::armem::gui
void
MemoryViewer::stopLTMRecording()
{
+ std::scoped_lock l(memoryReaderMutex);
+
TIMING_START(MemoryStopRecording);
auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
@@ -345,6 +354,8 @@ namespace armarx::armem::gui
void MemoryViewer::commit()
{
+ std::scoped_lock l(memoryWriterMutex);
+
TIMING_START(Commit);
auto now = armem::Time::Now();
@@ -367,7 +378,6 @@ namespace armarx::armem::gui
// ToDo: multiple objects
auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(json);
- memoryWriters = mns.getAllWriters(true);
if (const auto& it = memoryWriters.find(memId.memoryName); it == memoryWriters.end())
{
ARMARX_WARNING << "No memory with name '" << memId.memoryName << "' available for commit.";
@@ -391,6 +401,7 @@ namespace armarx::armem::gui
void
MemoryViewer::storeOnDisk(QString directory)
{
+ std::scoped_lock l(memoryDataMutex);
TIMING_START(MemoryExport)
std::string status;
@@ -404,11 +415,12 @@ namespace armarx::armem::gui
void
MemoryViewer::loadFromDisk(QString directory)
{
+ std::scoped_lock l(memoryWriterMutex);
+
std::string status;
std::map<std::string, wm::Memory> data =
diskControl->loadFromDisk(directory, memoryGroup->queryInput(), &status);
- memoryWriters = mns.getAllWriters(true);
for (auto& [name, memory] : data)
{
if (memoryWriters.count(name) > 0)
@@ -431,16 +443,14 @@ namespace armarx::armem::gui
void
MemoryViewer::startQueries()
{
- memoryReaders = mns.getAllReaders(true);
- startDueQueries(runningQueries, memoryReaders);
+ startDueQueries();
}
void
MemoryViewer::processQueryResults()
{
- const std::map<std::string, client::QueryResult> results =
- collectQueryResults(runningQueries, memoryReaders);
+ const std::map<std::string, client::QueryResult> results = collectQueryResults();
int errorCount = 0;
applyQueryResults(results, &errorCount);
@@ -469,17 +479,18 @@ namespace armarx::armem::gui
void
- MemoryViewer::startDueQueries(
- std::map<std::string, std::future<armem::query::data::Result>>& queries,
- const std::map<std::string, armem::client::Reader>& readers)
+ MemoryViewer::startDueQueries()
{
+ std::scoped_lock l(memoryReaderMutex);
+ std::scoped_lock l2(runningQueriesMutex);
+
armem::client::QueryInput input = memoryGroup->queryInput();
int recursionDepth = memoryGroup->queryWidget()->queryLinkRecursionDepth();
// Can't use a structured binding here because you can't capture those in a lambda
// according to the C++ standard.
auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
- for (const auto& pair : readers)
+ for (const auto& pair : memoryReaders)
{
// skip if memory should not be queried
if (std::find(enabledMemories.begin(), enabledMemories.end(), pair.first) == enabledMemories.end())
@@ -489,40 +500,44 @@ namespace armarx::armem::gui
const auto& name = pair.first;
const auto& reader = pair.second;
- if (queries.count(name) == 0)
+
+ // skip if query already running
+ if (runningQueries.count(name) != 0)
{
- // You could pass the query function itself to async here,
- // but that caused severe template headaches when I tried it.
- queries[name] =
- std::async(std::launch::async,
- [&reader, input, recursionDepth, this]()
- {
- // Can't resolve MemoryLinks without data
- return recursionDepth == 0 || input.dataMode == armem::query::DataMode::NoData
- ? reader.query(input.toIce())
- : reader.query(input.toIce(), mns, recursionDepth);
- });
+ continue;
}
+
+ // You could pass the query function itself to async here,
+ // but that caused severe template headaches when I tried it.
+ runningQueries[name] = std::async(std::launch::async,
+ [reader, input, recursionDepth, this]()
+ {
+ // Can't resolve MemoryLinks without data
+ return recursionDepth == 0 || input.dataMode == armem::query::DataMode::NoData
+ ? reader.query(input.toIce())
+ : reader.query(input.toIce(), mns, recursionDepth);
+ });
}
}
std::map<std::string, client::QueryResult>
- MemoryViewer::collectQueryResults(
- std::map<std::string, std::future<armem::query::data::Result>>& queries,
- const std::map<std::string, client::Reader>& readers)
+ MemoryViewer::collectQueryResults()
{
+ std::scoped_lock l(memoryReaderMutex);
+ std::scoped_lock l2(runningQueriesMutex);
+
TIMING_START(tCollectQueryResults)
std::map<std::string, client::QueryResult> results;
- for (auto it = queries.begin(); it != queries.end();)
+ for (auto it = runningQueries.begin(); it != runningQueries.end();)
{
const std::string& name = it->first;
std::future<armem::query::data::Result>* queryPromise = &it->second;
if (queryPromise->wait_for(std::chrono::seconds(0)) == std::future_status::ready)
{
- if (auto jt = memoryReaders.find(name); jt != readers.end())
+ if (auto jt = memoryReaders.find(name); jt != memoryReaders.end())
{
try
{
@@ -536,7 +551,7 @@ namespace armarx::armem::gui
// else: Server is gone (MNS knew about it) => Skip result.
// Promise is completed => Clean up in any case.
- it = queries.erase(it);
+ it = runningQueries.erase(it);
}
else
{
@@ -561,9 +576,9 @@ namespace armarx::armem::gui
void
- MemoryViewer::applyQueryResults(const std::map<std::string, client::QueryResult>& results,
- int* outErrorCount)
+ MemoryViewer::applyQueryResults(const std::map<std::string, client::QueryResult>& results, int* outErrorCount)
{
+ std::scoped_lock l(memoryDataMutex);
TIMING_START(tProcessQueryResults)
for (const auto& [name, result] : results)
{
@@ -573,8 +588,7 @@ namespace armarx::armem::gui
}
else
{
- ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n"
- << result.errorMessage;
+ ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n" << result.errorMessage;
if (outErrorCount)
{
outErrorCount++;
@@ -635,6 +649,7 @@ namespace armarx::armem::gui
void
MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
{
+ std::scoped_lock l(memoryDataMutex);
const armem::wm::Memory* data = getSingleMemoryData(selectedID.memoryName);
if (data)
{
@@ -686,6 +701,7 @@ namespace armarx::armem::gui
void
MemoryViewer::resolveMemoryID(const MemoryID& id)
{
+ std::scoped_lock l(memoryDataMutex);
// ARMARX_IMPORTANT << "Resolving memory ID: " << id;
auto handleError = [this](const std::string& msg)
@@ -757,19 +773,24 @@ namespace armarx::armem::gui
void
MemoryViewer::updateAvailableMemories()
{
- if (mns) // mns must be initialized
+ std::scoped_lock l(memoryReaderMutex);
+ std::scoped_lock l2(memoryWriterMutex);
+
+ if (is_connected and mns) // mns must be connected and mns must be available
{
try
{
+ memoryReaders = mns.getAllReaders(true);
+ memoryWriters = mns.getAllWriters(true);
+
std::vector<std::string> convVec;
- memoryReaders = mns.getAllReaders(true); // we only need the readers
std::transform(memoryReaders.begin(), memoryReaders.end(), std::back_inserter(convVec), [](const auto& p){return p.first;});
TIMING_START(GuiUpdateAvailableMemories);
memoryGroup->queryWidget()->update(convVec);
TIMING_END_STREAM(GuiUpdateAvailableMemories, ARMARX_VERBOSE);
}
- catch (const std::exception& e)
+ catch (...)
{
// MNS was killed/stopped
// ignore?!
@@ -785,6 +806,7 @@ namespace armarx::armem::gui
void
MemoryViewer::updateMemoryTree()
{
+ std::scoped_lock l(memoryDataMutex);
std::map<std::string, const armem::wm::Memory*> convMap;
for (auto& [name, data] : memoryData)
{
@@ -923,6 +945,8 @@ namespace armarx::armem::gui
const armarx::DateTime& timestamp,
const std::string& engineID)
{
+ std::scoped_lock l(memoryReaderMutex);
+
std::stringstream errorStream;
auto showError = [this, &errorStream]()
{
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 8cc41498c..f9ceb7471 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -129,14 +129,10 @@ namespace armarx::armem::gui
/// Start a query for each entry in `memoryReaders` which is not in `runningQueries`.
- void startDueQueries(
- std::map<std::string, std::future<armem::query::data::Result>>& queries,
- const std::map<std::string, armem::client::Reader>& readers);
+ void startDueQueries();
std::map<std::string, client::QueryResult>
- collectQueryResults(
- std::map<std::string, std::future<armem::query::data::Result>>& queries,
- const std::map<std::string, armem::client::Reader>& readers);
+ collectQueryResults();
void applyQueryResults(
const std::map<std::string, client::QueryResult>& results,
@@ -148,10 +144,15 @@ namespace armarx::armem::gui
std::string mnsName;
armem::client::MemoryNameSystem mns;
+ mutable std::recursive_mutex memoryReaderMutex;
+ mutable std::recursive_mutex memoryWriterMutex;
std::map<std::string, armem::client::Reader> memoryReaders;
std::map<std::string, armem::client::Writer> memoryWriters;
+
+ mutable std::recursive_mutex memoryDataMutex;
std::map<std::string, armem::wm::Memory> memoryData;
+ mutable std::recursive_mutex runningQueriesMutex;
std::map<std::string, std::future<armem::query::data::Result>> runningQueries;
/// Periodically triggers query result collection and updates the available memories
@@ -177,6 +178,10 @@ namespace armarx::armem::gui
// Queries.
armem::query::data::EntityQueryPtr entityQuery;
+ // others
+ std::atomic_bool is_initialized = false;
+ std::atomic_bool is_connected = false;
+
};
}
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 1719e6cac..54e5aaec3 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -103,11 +103,7 @@ namespace armarx::armem::gui
void QueryWidget::update(const std::vector<std::string>& activeMemoryNames)
{
- // since this is the only method that adds or hides elements of the groupbox after initialization,
- // we can use a static mutex, only available to this method
- //static std::mutex m;
-
- //std::scoped_lock l(m);
+ std::scoped_lock l(enabledMemoriesMutex);
std::vector<std::string> alreadyPresentMemoryCheckboxes;
// get information about memories already present and activate inactive ones if necessary
@@ -169,6 +165,8 @@ namespace armarx::armem::gui
std::vector<std::string> QueryWidget::enabledMemories() const
{
+ std::scoped_lock l(enabledMemoriesMutex);
+
std::vector<std::string> enabledMemoryCheckboxes;
int maxIndex = _availableMemoriesGroupBox->layout()->count();
for (int i = 0; i < maxIndex; ++i)
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
index bb285a9a4..d3d534e5b 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
@@ -1,5 +1,7 @@
#pragma once
+#include <mutex>
+
#include <RobotAPI/libraries/armem/core/query/DataMode.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -68,6 +70,8 @@ namespace armarx::armem::gui
QGroupBox* _additionalSettingsGroupBox;
QGroupBox* _availableMemoriesGroupBox;
+
+ mutable std::mutex enabledMemoriesMutex;
};
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index e84ad2fc7..21049a274 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -84,15 +84,21 @@ namespace armarx
}
FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame)
+ {
+ ARMARX_CHECK_NOT_NULL(robot);
+ return ChangeFrame(*robot, framedVec, newFrame);
+ }
+
+ FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::Robot& robot, const FramedDirection& framedVec, const std::string& newFrame)
{
if (framedVec.getFrame() == newFrame)
{
return FramedDirectionPtr::dynamicCast(framedVec.clone());
}
- if (!robot->hasRobotNode(newFrame))
+ if (!robot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot->getName();
+ throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot.getName();
}
Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
@@ -106,11 +112,17 @@ namespace armarx
result->y = newVec(1);
result->z = newVec(2);
result->frame = newFrame;
- result->agent = robot->getName();
+ result->agent = robot.getName();
return result;
}
void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
+ {
+ ARMARX_CHECK_NOT_NULL(robot);
+ changeFrame(*robot, newFrame);
+ }
+
+ void FramedDirection::changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame)
{
if (frame == "")
{
@@ -129,14 +141,14 @@ namespace armarx
return;
}
- if (!robot->hasRobotNode(newFrame))
+ if (!robot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exist in the robot " << robot->getName();
+ throw LocalException() << "The requested frame '" << newFrame << "' does not exist in the robot " << robot.getName();
}
- if (frame != GlobalFrame && !robot->hasRobotNode(frame))
+ if (frame != GlobalFrame && !robot.hasRobotNode(frame))
{
- throw LocalException() << "The current frame '" << frame << "' does not exist in the robot " << robot->getName();
+ throw LocalException() << "The current frame '" << frame << "' does not exist in the robot " << robot.getName();
}
@@ -151,7 +163,7 @@ namespace armarx
y = newVec(1);
z = newVec(2);
frame = newFrame;
- agent = robot->getName();
+ agent = robot.getName();
}
void FramedDirection::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
@@ -161,14 +173,20 @@ namespace armarx
}
void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeToGlobal(*referenceRobot);
+ }
+
+ void FramedDirection::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
return;
}
- changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Vector3f pos = referenceRobot->getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
+ changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
+ Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
x = pos[0];
y = pos[1];
z = pos[2];
@@ -182,6 +200,11 @@ namespace armarx
return toGlobal(sharedRobot);
}
FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobal(*referenceRobot);
+ }
+ FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
newDirection->changeToGlobal(referenceRobot);
@@ -194,6 +217,11 @@ namespace armarx
return toGlobalEigen(sharedRobot);
}
Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobalEigen(*referenceRobot);
+ }
+ Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirection newDirection(toEigen(), frame, agent);
newDirection.changeToGlobal(referenceRobot);
@@ -206,9 +234,14 @@ namespace armarx
return toRootFrame(sharedRobot);
}
FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootFrame(*referenceRobot);
+ }
+ FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
- newDirection->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newDirection->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newDirection;
}
@@ -218,9 +251,14 @@ namespace armarx
return toRootEigen(sharedRobot);
}
Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootEigen(*referenceRobot);
+ }
+ Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirection newDirection(toEigen(), frame, agent);
- newDirection.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newDirection.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newDirection.toEigen();
}
@@ -231,21 +269,21 @@ namespace armarx
return s.str();
}
- Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState)
+ Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, const VirtualRobot::Robot& robotState)
{
Eigen::Matrix4f toNewFrame;
if (oldFrame == GlobalFrame)
{
- auto node = robotState->getRobotNode(newFrame);
+ auto node = robotState.getRobotNode(newFrame);
ARMARX_CHECK_EXPRESSION(node) << newFrame;
toNewFrame = node->getGlobalPose();
}
else
{
- auto node = robotState->getRobotNode(oldFrame);
- ARMARX_CHECK_EXPRESSION(node) << "old frame: " + oldFrame + ValueToString(robotState->getConfig()->getRobotNodeJointValueMap());
- auto newNode = robotState->getRobotNode(newFrame);
+ auto node = robotState.getRobotNode(oldFrame);
+ ARMARX_CHECK_EXPRESSION(node) << "old frame: " + oldFrame + ValueToString(robotState.getConfig()->getRobotNodeJointValueMap());
+ auto newNode = robotState.getRobotNode(newFrame);
ARMARX_CHECK_EXPRESSION(newNode) << newFrame;
toNewFrame = node->getTransformationTo(newNode);
}
@@ -369,6 +407,12 @@ namespace armarx
}
void FramedPose::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeFrame(*referenceRobot, newFrame);
+ }
+
+ void FramedPose::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -383,32 +427,32 @@ namespace armarx
Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
- if (frame != GlobalFrame and !referenceRobot->hasRobotNode(frame))
+ if (frame != GlobalFrame and !referenceRobot.hasRobotNode(frame))
{
- throw LocalException() << "The current frame '" << frame << "' does not exists in the robot " << referenceRobot->getName();
+ throw LocalException() << "The current frame '" << frame << "' does not exists in the robot " << referenceRobot.getName();
}
- if (!referenceRobot->hasRobotNode(newFrame))
+ if (!referenceRobot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
}
if (frame != GlobalFrame)
{
- oldFrameTransformation = referenceRobot->getRobotNode(frame)->getPoseInRootFrame();
+ oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
}
else
{
- oldFrameTransformation = referenceRobot->getRootNode()->getGlobalPose().inverse();
+ oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
}
Eigen::Matrix4f newPose =
- (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
+ (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
orientation = new Quaternion(newPose);
Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
position = new Vector3(pos);
frame = newFrame;
- agent = referenceRobot->getName();
+ agent = referenceRobot.getName();
init();
}
@@ -418,16 +462,20 @@ namespace armarx
changeToGlobal(sharedRobot);
}
-
void FramedPose::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeToGlobal(*referenceRobot);
+ }
+ void FramedPose::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
return;
}
- changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Matrix4f global = referenceRobot->getRootNode()->getGlobalPose();
+ changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
+ Eigen::Matrix4f global = referenceRobot.getRootNode()->getGlobalPose();
const Eigen::Matrix4f pose = global * toEigen();
position->x = pose(0, 3);
position->y = pose(1, 3);
@@ -447,6 +495,11 @@ namespace armarx
return toGlobal(sharedRobot);
}
FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobal(*referenceRobot);
+ }
+ FramedPosePtr FramedPose::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
newPose->changeToGlobal(referenceRobot);
@@ -459,6 +512,11 @@ namespace armarx
return toGlobalEigen(sharedRobot);
}
Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobalEigen(*referenceRobot);
+ }
+ Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPose newPose(toEigen(), frame, agent);
newPose.changeToGlobal(referenceRobot);
@@ -471,9 +529,14 @@ namespace armarx
return toRootFrame(sharedRobot);
}
FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootFrame(*referenceRobot);
+ }
+ FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
- newPose->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newPose->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPose;
}
@@ -483,9 +546,14 @@ namespace armarx
return toRootEigen(sharedRobot);
}
Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootEigen(*referenceRobot);
+ }
+ Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPose newPose(toEigen(), frame, agent);
- newPose.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newPose.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPose.toEigen();
}
@@ -575,6 +643,12 @@ namespace armarx
}
void FramedPosition::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeFrame(*referenceRobot, newFrame);
+ }
+
+ void FramedPosition::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -596,30 +670,30 @@ namespace armarx
Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
- if (!referenceRobot->hasRobotNode(newFrame))
+ if (!referenceRobot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
}
if (frame != GlobalFrame)
{
- oldFrameTransformation = referenceRobot->getRobotNode(frame)->getPoseInRootFrame();
+ oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
}
else
{
- oldFrameTransformation = referenceRobot->getRootNode()->getGlobalPose().inverse();
+ oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
}
Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
oldPose.block<3, 1>(0, 3) = toEigen();
Eigen::Matrix4f newPose =
- (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
+ (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
x = pos[0];
y = pos[1];
z = pos[2];
- agent = referenceRobot->getName();
+ agent = referenceRobot.getName();
frame = newFrame;
}
@@ -628,17 +702,21 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
-
void FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeToGlobal(*referenceRobot);
+ }
+ void FramedPosition::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
return;
}
- changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Vector3f pos = referenceRobot->getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen()
- + referenceRobot->getRootNode()->getGlobalPose().block<3, 1>(0, 3);
+ changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
+ Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen()
+ + referenceRobot.getRootNode()->getGlobalPose().block<3, 1>(0, 3);
x = pos[0];
y = pos[1];
z = pos[2];
@@ -652,6 +730,11 @@ namespace armarx
return toGlobal(sharedRobot);
}
FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobal(*referenceRobot);
+ }
+ FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
newPosition->changeToGlobal(referenceRobot);
@@ -664,6 +747,11 @@ namespace armarx
return toGlobalEigen(sharedRobot);
}
Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobalEigen(*referenceRobot);
+ }
+ Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosition newPosition(toEigen(), frame, agent);
newPosition.changeToGlobal(referenceRobot);
@@ -676,9 +764,14 @@ namespace armarx
return toRootFrame(sharedRobot);
}
FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootFrame(*referenceRobot);
+ }
+ FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
- newPosition->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newPosition->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPosition;
}
@@ -688,9 +781,14 @@ namespace armarx
return toRootEigen(sharedRobot);
}
Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootEigen(*referenceRobot);
+ }
+ Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosition newPosition(toEigen(), frame, agent);
- newPosition.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newPosition.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPosition.toEigen();
}
@@ -829,6 +927,12 @@ namespace armarx
}
void FramedOrientation::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeFrame(*referenceRobot, newFrame);
+ }
+
+ void FramedOrientation::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -843,32 +947,32 @@ namespace armarx
Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
- if (!referenceRobot->hasRobotNode(newFrame))
+ if (!referenceRobot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
}
if (frame != GlobalFrame)
{
- oldFrameTransformation = referenceRobot->getRobotNode(frame)->getPoseInRootFrame();
+ oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
}
else
{
- oldFrameTransformation = referenceRobot->getRootNode()->getGlobalPose().inverse();
+ oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
}
Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
oldPose.block<3, 3>(0, 0) = toEigen();
Eigen::Matrix4f newPose =
- (referenceRobot->getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
+ (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
Eigen::Quaternionf quat(newPose.block<3, 3>(0, 0));
qw = quat.w();
qx = quat.x();
qy = quat.y();
qz = quat.z();
- agent = referenceRobot->getName();
+ agent = referenceRobot.getName();
frame = newFrame;
}
@@ -879,14 +983,19 @@ namespace armarx
}
void FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ changeToGlobal(*referenceRobot);
+ }
+ void FramedOrientation::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
return;
}
- changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
- Eigen::Matrix3f rot = referenceRobot->getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
+ changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
+ Eigen::Matrix3f rot = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
Eigen::Quaternionf quat(rot);
qw = quat.w();
qx = quat.x();
@@ -902,6 +1011,11 @@ namespace armarx
return toGlobal(sharedRobot);
}
FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobal(*referenceRobot);
+ }
+ FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
newOrientation->changeToGlobal(referenceRobot);
@@ -914,6 +1028,11 @@ namespace armarx
return toGlobalEigen(sharedRobot);
}
Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toGlobalEigen(*referenceRobot);
+ }
+ Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientation newOrientation(toEigen(), frame, agent);
newOrientation.changeToGlobal(referenceRobot);
@@ -926,9 +1045,14 @@ namespace armarx
return toRootFrame(sharedRobot);
}
FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootFrame(*referenceRobot);
+ }
+ FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
- newOrientation->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newOrientation->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newOrientation;
}
@@ -938,9 +1062,14 @@ namespace armarx
return toRootEigen(sharedRobot);
}
Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ {
+ ARMARX_CHECK_NOT_NULL(referenceRobot);
+ return toRootEigen(*referenceRobot);
+ }
+ Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientation newOrientation(toEigen(), frame, agent);
- newOrientation.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+ newOrientation.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newOrientation.toEigen();
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index e0ca6fdaa..01f5c9c6a 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -93,18 +93,25 @@ namespace armarx
std::string getFrame() const;
static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame);
+ static FramedDirectionPtr ChangeFrame(const VirtualRobot::Robot& robot, const FramedDirection& framedVec, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
+ void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedDirectionPtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Vector3f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Vector3f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
FramedDirectionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedDirectionPtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Vector3f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const override;
@@ -120,7 +127,7 @@ namespace armarx
void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
private:
- static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState);
+ static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, const VirtualRobot::Robot& robotState);
};
class FramedPosition;
@@ -149,16 +156,22 @@ namespace armarx
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
+ void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedPositionPtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Vector3f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Vector3f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedPositionPtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Vector3f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const override;
@@ -207,16 +220,22 @@ namespace armarx
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
+ void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedOrientationPtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Matrix3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix3f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Matrix3f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
FramedOrientationPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedOrientationPtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Matrix3f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs);
@@ -264,20 +283,28 @@ namespace armarx
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
+ void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
+ void changeToGlobal(const VirtualRobot::Robot& referenceRobot);
FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedPosePtr toGlobal(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Matrix4f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix4f toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Matrix4f toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const;
FramedPosePtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+ FramedPosePtr toRootFrame(const VirtualRobot::Robot& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
+ Eigen::Matrix4f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
FramedPosePtr toFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const;
FramedPosePtr toFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const;
+ FramedPosePtr toFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame) const;
Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const;
Eigen::Matrix4f toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const;
+ Eigen::Matrix4f toFrameEigen(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs);
FramedPositionPtr getPosition() const;
--
GitLab