diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index d79d9a4ba4e3ee75d0860328997112e864c008d3..23bc568751839f9758cd09597b2b4f58b8b5e419 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -16,6 +16,7 @@ set(SOURCES
Client/elements/Robot.cpp
Client/elements/RobotHand.cpp
Client/drawer/ArVizDrawerBase.cpp
+ Client/ScopedClient.cpp
Coin/ElementVisualizer.cpp
@@ -65,6 +66,7 @@ set(HEADERS
Client/Layer.h
Client/Elements.h
Client/Client.h
+ Client/ScopedClient.h
Client/ClientCGALExtensions.h
Client/Color.h
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index cc8e7289a57f4302b65fae48574d04f8931d4bc5..f946e02d4298910c72bb4d8a59bf9d64d42ac5ab 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -15,12 +15,14 @@ namespace armarx::viz
{
public:
Client() = default;
+ virtual ~Client() = default;
Client(armarx::Component& component, std::string topicNameProperty = "ArVizTopicName")
{
componentName = component.getName();
component.getTopicFromProperty(_topic, topicNameProperty);
}
+
Client(ManagedIceObject& obj, const std::string& topicName = "ArVizTopic")
{
componentName = obj.getName();
@@ -51,7 +53,7 @@ namespace armarx::viz
// ////////////////////////////////////////////////////////////////// //
//layer
- Layer layer(std::string const& name) const
+ virtual Layer layer(std::string const& name) const
{
return Layer(componentName, name);
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 297a22469d52be8b7cda3c71438f6ee0617c0091..66f07992e0ba2dc0c382b3a9742a5413c9f192b6 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -122,6 +122,13 @@ namespace armarx::viz
)
: Box(name, b)
{}
+
+ Box(std::string const& id)
+ : ElementOps(id)
+ {
+ pose(Eigen::Matrix4f::Identity());
+ }
+
};
diff --git a/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp b/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed2f2221ef763270c461ab3c1c1bc436ac26a2a9
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp
@@ -0,0 +1,44 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "ScopedClient.h"
+
+namespace armarx::viz
+{
+ ScopedClient::ScopedClient(const Client& client): Client(client) {}
+
+ Layer ScopedClient::layer(std::string const& name) const
+ {
+ layers.insert(name);
+
+ return Client::layer(name);
+ }
+
+ ScopedClient::~ScopedClient()
+ {
+ for (const auto& layer : layers)
+ {
+ Client::commitDeleteLayer(layer);
+ }
+ }
+
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/ScopedClient.h b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..78adbcab32013de4b82f9a35671063e66a4aaa11
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
@@ -0,0 +1,53 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <set>
+
+#include "Client.h"
+
+namespace armarx::viz
+{
+ /**
+ * @brief `viz::Client` that will delete (clear) committed layers when destroyed.
+ *
+ * Note that, as a consequence, a network call will be performed in the destructor.
+ *
+ * This might be useful if you have a class `MyTask` perform visualizing while performing some task,
+ * but whose visualization should be removed when the task finished.
+ * In this case, `MyTask` can have a `viz::ScopedClient` (which can be created from a regular `viz::Client`).
+ * When destructing the instance of `MyTask`, all visualization done by `MyTask` will be cleared
+ * (as `viz::ScopedClient` will go out of scope as well).
+ *
+ */
+ class ScopedClient: virtual public Client
+ {
+ public:
+ using Client::Client;
+ ScopedClient(const Client& client);
+
+ Layer layer(std::string const& name) const override;
+
+ virtual ~ScopedClient();
+
+ private:
+ mutable std::set<std::string> layers;
+ };
+} // namespace armarx::viz
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 6753e0a6cbf4e95f5111bc22d7dce0faec279a31..d256ee202c02d8ebf204455afa241731557815c8 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -23,24 +23,23 @@
#include "RobotNameHelper.h"
-#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
// #include <ArmarXCore/core/system/ArmarXDataPath.cpp>
-
#include <Eigen/Dense>
+#include <algorithm>
namespace armarx
{
- void RobotNameHelper::writeRobotInfoNode(
- const RobotInfoNodePtr& n,
- std::ostream& str,
- std::size_t indent,
- char indentChar)
+ void RobotNameHelper::writeRobotInfoNode(const RobotInfoNodePtr& n,
+ std::ostream& str,
+ std::size_t indent,
+ char indentChar)
{
const std::string ind(4 * indent, indentChar);
if (!n)
@@ -59,11 +58,12 @@ namespace armarx
return robotInfo;
}
- const std::string RobotNameHelper::LocationLeft = "Left";
+ const std::string RobotNameHelper::LocationLeft = "Left";
const std::string RobotNameHelper::LocationRight = "Right";
- RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
- : root(Node(robotInfo)), robotInfo(robotInfo)
+ RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile) :
+ root(Node(robotInfo)), robotInfo(robotInfo)
{
ARMARX_TRACE;
StatechartProfilePtr p = profile;
@@ -73,7 +73,6 @@ namespace armarx
p = p->getParent();
}
profiles.emplace_back(""); // for matching the default/root
-
}
std::string RobotNameHelper::select(const std::string& path) const
@@ -94,17 +93,50 @@ namespace armarx
return node.value();
}
- RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+ RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile)
{
return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
}
+ RobotNameHelperPtr RobotNameHelper::Create(const std::string& robotXmlFilename,
+ const StatechartProfilePtr& profile)
+ {
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotXmlFilename);
+ RapidXmlReaderNode robotNode = reader->getRoot("Robot");
+
+ return Create(readRobotInfo(robotNode.first_node("RobotInfo")), profile);
+ }
+
+ RobotInfoNodePtr RobotNameHelper::readRobotInfo(const RapidXmlReaderNode& infoNode)
+ {
+ const std::string name = infoNode.name();
+ const std::string profile = infoNode.attribute_value_or_default("profile", "");
+ const std::string value = infoNode.attribute_value_or_default("value", "");
+
+ const auto nodes = infoNode.nodes();
+
+ std::vector<RobotInfoNodePtr> children;
+ children.reserve(nodes.size());
+
+ std::transform(nodes.begin(),
+ nodes.end(),
+ std::back_inserter(children),
+ [](const auto & childNode)
+ {
+ return readRobotInfo(childNode);
+ });
+
+ return new RobotInfoNode(name, profile, value, children);
+ }
+
RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side) const
{
return Arm(shared_from_this(), side);
}
- RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
+ RobotNameHelper::RobotArm
+ RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
{
return RobotArm(Arm(shared_from_this(), side), robot);
}
@@ -114,11 +146,7 @@ namespace armarx
return rnh;
}
- RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
- : robotInfo(robotInfo)
- {
-
- }
+ RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo) : robotInfo(robotInfo) {}
std::string RobotNameHelper::Node::value()
{
@@ -126,7 +154,8 @@ namespace armarx
return robotInfo->value;
}
- RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
+ RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name,
+ const std::vector<std::string>& profiles)
{
ARMARX_TRACE;
if (!isValid())
@@ -168,7 +197,6 @@ namespace armarx
}
}
-
std::string RobotNameHelper::Arm::getSide() const
{
return side;
@@ -232,8 +260,7 @@ namespace armarx
{
ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
auto path = getHandModelPath();
- return ArmarXDataPath::getAbsolutePath(path, path) ?
- path : "";
+ return ArmarXDataPath::getAbsolutePath(path, path) ? path : "";
}
std::string RobotNameHelper::Arm::getHandModelPackage() const
@@ -254,16 +281,17 @@ namespace armarx
return select("FullHandCollisionModel");
}
- RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+ RobotNameHelper::RobotArm
+ RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
{
ARMARX_TRACE;
return RobotArm(*this, robot);
}
- RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side)
- : rnh(rnh), side(side)
+ RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh,
+ const std::string& side) :
+ rnh(rnh), side(side)
{
-
}
std::string RobotNameHelper::Arm::select(const std::string& path) const
@@ -310,8 +338,9 @@ namespace armarx
{
ARMARX_TRACE;
std::string abs;
- ARMARX_CHECK_EXPRESSION(ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
- return VirtualRobot::TriMeshModel::FromFile(abs);
+ ARMARX_CHECK_EXPRESSION(
+ ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
+ return VirtualRobot::TriMeshModel::FromFile(abs);
}
VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
@@ -341,8 +370,8 @@ namespace armarx
return arm;
}
- RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
- : arm(arm), robot(robot)
+ RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) :
+ arm(arm), robot(robot)
{
ARMARX_CHECK_NOT_NULL(robot);
}
@@ -351,4 +380,4 @@ namespace armarx
{
return profiles;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index d37ecea9db567faab03640bc9d2ce12815f6f65b..65fbc12c2b037ab7b1f7e9101d651e2592e168aa 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -26,6 +26,7 @@
#include <RobotAPI/interface/core/RobotState.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
namespace armarx
@@ -33,6 +34,8 @@ namespace armarx
using RobotNameHelperPtr = std::shared_ptr<class RobotNameHelper>;
using StatechartProfilePtr = std::shared_ptr<class StatechartProfile>;
+ class RapidXmlReaderNode;
+
class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
{
public:
@@ -128,8 +131,10 @@ namespace armarx
std::string select(const std::string& path) const;
static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+ static RobotNameHelperPtr Create(const std::string& robotXmlFilename, const StatechartProfilePtr& profile = nullptr);
RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
+
RobotNameHelper(RobotNameHelper&&) = default;
RobotNameHelper(const RobotNameHelper&) = default;
RobotNameHelper& operator=(RobotNameHelper&&) = default;
@@ -140,6 +145,9 @@ namespace armarx
const std::vector<std::string>& getProfiles() const;
const RobotInfoNodePtr& getRobotInfoNodePtr() const;
private:
+
+ static RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
+
RobotNameHelper& self()
{
return *this;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index 456671e79cc764fc577121082967fcc52e341361..4fcd3e452040b179aa92e8a3928d32d05abaaf60 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -57,7 +57,7 @@ namespace armarx
controller->activateController();
}
- void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf& cv)
+ void VelocityControllerHelper::setTargetVelocity(const Eigen::Vector6f& cv)
{
controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
index c93de51afea1f663deda3e5f458cfe9f65acf8d7..ab47ffaccc480f999f214df3f5dbb908549f1ad7 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
@@ -43,7 +43,7 @@ namespace armarx
void init();
- void setTargetVelocity(const Eigen::VectorXf& cv);
+ void setTargetVelocity(const Eigen::Vector6f& cv);
void setNullSpaceControl(bool enabled);
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 44a32b8317022bbcee867413de19b6f6688a853b..16da9f1709528dc7b9c7d484be494a2f1e6da851 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -21,10 +21,11 @@
#pragma once
-#include "ArmarXCore/core/logging/Logging.h"
#include <vector>
#include <optional>
+#include "ArmarXCore/core/logging/Logging.h"
+
#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 6484c97ca0695848fc92dcb34008fc5b1d7c70f9..498a422fb201bf435cfb8ddbfdc43b71ba1944a0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -50,7 +50,6 @@ armarx_add_library(
./client/common/RobotReader.cpp
./client/common/VirtualRobotReader.cpp
-
./client/localization/TransformReader.cpp
./client/localization/TransformWriter.cpp
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 5253659423cb5b30d57a18ad8738ae8b5ba79597..2d8319dc9f6517c41e9a38225ad4dbd11b9ae607 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -7,19 +7,23 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) : memoryClient(component), transformReader(component) {}
+ RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) :
+ memoryClient(component), transformReader(component)
+ {
+ }
void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
@@ -28,7 +32,8 @@ namespace armarx::armem::robot_state
def->optional(properties.memoryName, propertyPrefix + "Memory");
def->optional(properties.descriptionCoreSegment, propertyPrefix + "descriptionSegment");
def->optional(properties.localizationCoreSegment, propertyPrefix + "localizationSegment");
- def->optional(properties.proprioceptionCoreSegment, propertyPrefix + "proprioceptionSegment");
+ def->optional(properties.proprioceptionCoreSegment,
+ propertyPrefix + "proprioceptionSegment");
}
void RobotReader::connect()
@@ -36,8 +41,7 @@ namespace armarx::armem::robot_state
transformReader.connect();
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName << "' ...";
auto result = memoryClient.useMemory(properties.memoryName);
if (not result.success)
{
@@ -50,7 +54,8 @@ namespace armarx::armem::robot_state
memoryReader.setReadingMemory(result.proxy);
}
- std::optional<robot::Robot> RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ std::optional<robot::Robot> RobotReader::get(const std::string& name,
+ const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -63,17 +68,13 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
-
robot::Robot RobotReader::get(const robot::RobotDescription& description,
const armem::Time& timestamp)
{
- robot::Robot robot
- {
- .description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp
- };
+ robot::Robot robot{.description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
synchronize(robot, timestamp);
@@ -94,7 +95,8 @@ namespace armarx::armem::robot_state
return true;
}
- std::optional<robot::RobotDescription> RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ std::optional<robot::RobotDescription>
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -127,7 +129,6 @@ namespace armarx::armem::robot_state
}
return getRobotDescription(qResult.memory, name);
-
}
catch (...)
{
@@ -135,32 +136,34 @@ namespace armarx::armem::robot_state
}
return std::nullopt;
-
}
- std::optional<robot::RobotState> RobotReader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+ std::optional<robot::RobotState>
+ RobotReader::queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
{
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
+ ARMARX_WARNING << "Failed to query joint state";
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
+ ARMARX_WARNING << "Failed to query global pose";
return std::nullopt;
}
return robot::RobotState
{
- .timestamp = timestamp,
- .globalPose = *globalPose,
- .jointMap = *jointMap
- };
+ .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
- std::optional<robot::RobotState::JointMap> RobotReader::queryJointState(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ std::optional<robot::RobotState::JointMap>
+ RobotReader::queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -181,13 +184,16 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
+ ARMARX_WARNING << qResult.errorMessage;
return std::nullopt;
}
return getRobotJointState(qResult.memory, description.name);
}
- std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ std::optional<robot::RobotState::Pose>
+ RobotReader::queryGlobalPose(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
{
const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
if (not result)
@@ -198,8 +204,9 @@ namespace armarx::armem::robot_state
return result.transform.transform;
}
-
- std::optional<robot::RobotState> RobotReader::getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ std::optional<robot::RobotState>
+ RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -231,44 +238,67 @@ namespace armarx::armem::robot_state
return robot::convertRobotState(instance);
}
+ // FIXME remove this, use armem/util/util.h
+ template <typename AronClass>
+ std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ {
+ static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ AronClass>::value);
+
+ try
+ {
+ AronClass t;
+ t.fromAron(item.data());
+ return t;
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ return std::nullopt;
+ }
+ }
- std::optional<robot::RobotState::JointMap> RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ std::optional<robot::RobotState::JointMap>
+ RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
- // // clang-format off
- // const armem::wm::ProviderSegment& providerSegment = memory
- // .getCoreSegment(properties.proprioceptionCoreSegment)
- // .getProviderSegment(name);
- // // clang-format on
- // const auto entities = simox::alg::get_values(providerSegment.entities());
- // // TODO entitiesToRobotState()
+ robot::RobotState::JointMap jointMap;
- // if (entities.empty())
- // {
- // ARMARX_WARNING << "No entity found";
- // return std::nullopt;
- // }
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
- // const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ {
- // if (entitySnapshots.empty())
- // {
- // ARMARX_WARNING << "No entity snapshots found";
- // return std::nullopt;
- // }
+ for (const auto &[name, entity] : providerSegment.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // // TODO(fabian.reister): check if 0 available
- // const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // // Here, we access the RobotUnit streaming data stored in the proprioception segment.
- // return robot::convertRobotState(instance);
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
- return std::nullopt; // TODO implement
- }
+ jointMap.emplace(jointState->name, jointState->position);
+ }
+ }
+ if (jointMap.empty())
+ {
+ return std::nullopt;
+ }
+ return jointMap;
+ }
- std::optional<robot::RobotDescription> RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ std::optional<robot::RobotDescription>
+ RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -297,4 +327,4 @@ namespace armarx::armem::robot_state
return robot::convertRobotDescription(instance);
}
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 97633b788dcfae4cd93d6a1fd7a72850a9a009b1..a3655fee1ae2597b768424f0b89fd581a1bb0680 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -6,6 +6,7 @@
#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/system/ArmarXDataPath.h"
#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
@@ -41,6 +42,7 @@ namespace armarx::armem::robot_state
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
+ ARMARX_WARNING << "Querying robot state failed!";
return false;
}
@@ -66,7 +68,18 @@ namespace armarx::armem::robot_state
ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
return VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+ }
+
+
+ VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ {
+ auto robot = getRobot(name, timestamp);
+
+ synchronizeRobot(*robot, timestamp);
+ return robot;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 5958bbca18cb618210b67b72b970bf0435d24bd9..a280e71b6132f065aec28a008ebb5ab803845dad 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -53,6 +53,12 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode =
VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ VirtualRobot::RobotPtr
+ getSynchronizedRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
};
} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index ff1ff84535fad72d2fd3ef06e3adda8b7c9d08ba..1a73b84900971b6cafe810bedd219998362a29a4 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -49,7 +49,7 @@ namespace armarx
CartesianVelocityControllerWithRamp(CartesianVelocityControllerWithRamp&&) = default;
CartesianVelocityControllerWithRamp& operator=(CartesianVelocityControllerWithRamp&&) = default;
- [[deprecated("compued null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
+ [[deprecated("computed null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
void switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode);