diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 27aaa07ddf1c0f0dc819a9a74366036cdadfe11c..c9eae54f7d33aeeab1afede2612eea3f0c2bf868 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -66,9 +66,6 @@ namespace armarx::armem::server::robot_state
defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
"Prefix of all sensor values.");
- defs->optional(robotUnit.writer.properties.memoryBatchSize, robotUnitPrefix + "MemoryBatchSize",
- "The size of the entity snapshot to send to the memory. Minimum is 1.")
- .setMin(1);
defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
"The frequency to store values in Hz. All other values get discarded. "
"Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
@@ -102,7 +99,6 @@ namespace armarx::armem::server::robot_state
commonVisu.init();
robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, ROBOT_UNIT_MAXIMUM_FREQUENCY);
- robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
std::vector<std::string> includePaths;
std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 21e579c803afd33d2ad5864de6c4b283ae69733c..1bf28496f697da38e2172a41836ecaccceaa5561 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -109,30 +109,5 @@ namespace armarx::armem
dto.objectJointValues = bo.jointMap;
}
- void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
- {
- bo.twist.linear.setZero();
- bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
-
- bo.twist.angular.setZero();
- bo.twist.angular.z() = dto.velocity.z(); // yaw
- }
-
- void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
- {
- ARMARX_ERROR << "Not implemented yet.";
- }
-
- void robot::fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
- {
- bo.force = dto.force;
- bo.torque = dto.torque;
- }
-
- void robot::toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
- {
- dto.force = bo.force;
- dto.torque = bo.torque;
- }
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 62744724f1d4c42ebe882cf4072e87d43428dd02..f2ea2ba959897ad48d9aed390a569d2e8009dd17 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,7 +8,6 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
@@ -27,12 +26,6 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
- void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
- void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
-
- void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo);
- void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo);
-
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 785d25bdca4757ebe1c07f0bab767aa08a02a24d..03a578fab3b9250f724efcd646c24400d8a898d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -1,31 +1,30 @@
#include "aron_conversions.h"
-// STL
#include <string>
-// Ice
#include <IceUtil/Time.h>
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-
namespace armarx::armem
{
/* Transform */
- void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ void
+ fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
{
fromAron(dto.header, bo.header);
aron::fromAron(dto.transform, bo.transform);
}
- void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ void
+ toAron(arondto::Transform& dto, const robot_state::Transform& bo)
{
toAron(dto.header, bo.header);
aron::toAron(dto.transform, bo.transform);
@@ -33,7 +32,8 @@ namespace armarx::armem
/* TransformHeader */
- void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ void
+ toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
{
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
@@ -41,7 +41,8 @@ namespace armarx::armem
dto.timestamp = bo.timestamp;
}
- void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ void
+ fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
{
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
@@ -51,16 +52,49 @@ namespace armarx::armem
/* JointState */
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+ void
+ fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
{
aron::fromAron(dto.name, bo.name);
aron::fromAron(dto.position, bo.position);
}
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+ void
+ toAron(arondto::JointState& dto, const robot_state::JointState& bo)
{
aron::toAron(dto.name, bo.name);
aron::toAron(dto.position, bo.position);
}
-} // namespace armarx::armem
+
+ void
+ fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
+
+ void
+ fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ {
+ bo.force = dto.force;
+ bo.torque = dto.torque;
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ {
+ dto.force = bo.force;
+ dto.torque = bo.torque;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 202a81f089702e07155232200911fa0e0afa197d..17fe89152ef063ab932ace41a9ade84eb2f2f556 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,6 +21,9 @@
#pragma once
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
namespace armarx::armem
{
namespace robot_state
@@ -41,6 +44,7 @@ namespace armarx::armem
} // namespace arondto
+
void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
@@ -50,4 +54,11 @@ namespace armarx::armem
void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-} // namespace armarx::armem
\ No newline at end of file
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+
+
+} // namespace armarx::armem
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 923b6f73937ef422da80bec8ce5408d9916b20a5..4f969f35c114b56d5627dc95ae118441b568aca2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -4,11 +4,10 @@
#include <optional>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
-#include "RobotAPI/libraries/armem_robot/types.h"
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -18,6 +17,7 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
namespace fs = ::std::filesystem;
@@ -203,6 +203,37 @@ namespace armarx::armem::robot_state
return getRobotJointState(qResult.memory, description.name);
}
+ RobotReader::JointTrajectory
+ RobotReader::queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin,
+ const armem::Time& end) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+ << "` on time interval [" << begin << "," << end << "]";
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().timeRange(begin, end);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return {};
+ }
+
+ return getRobotJointStates(qResult.memory, description.name);
+ }
+
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
@@ -275,6 +306,7 @@ namespace armarx::armem::robot_state
instance = &i;
return false; // break
});
+
if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
@@ -346,6 +378,46 @@ namespace armarx::armem::robot_state
return jointMap;
}
+ RobotReader::JointTrajectory
+ RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+
+ RobotReader::JointTrajectory jointTrajectory;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&jointTrajectory](const wm::Entity& entity)
+ {
+ entity.forEachSnapshot(
+ [&](const auto& snapshot)
+ {
+ if (not snapshot.hasInstance(0))
+ {
+ return;
+ }
+
+ const auto& entityInstance = snapshot.getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+ jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
+ });
+ });
+
+ ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
+
+ return jointTrajectory;
+ }
+
// force torque for left and right
std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
@@ -434,7 +506,7 @@ namespace armarx::armem::robot_state
ARMARX_CHECK(proprioception.has_value());
platformState = robot::PlatformState(); // initialize optional
- robot::fromAron(proprioception->platform, platformState.value());
+ fromAron(proprioception->platform, platformState.value());
});
return platformState;
@@ -480,7 +552,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
robot::ForceTorque forceTorque;
- robot::fromAron(dtoFt, forceTorque);
+ fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
forceTorques.emplace(hand, forceTorque);
@@ -514,7 +586,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
robot::ForceTorque forceTorque;
- robot::fromAron(dtoFt, forceTorque);
+ fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
@@ -538,7 +610,7 @@ namespace armarx::armem::robot_state
const armem::wm::EntityInstance* instance = nullptr;
providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
{ instance = &i; });
- if (!instance)
+ if (instance == nullptr)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 884c85fd964b8150a615dfd62e16b0e60de7b5ee..20cafbd2225160c7b9abacc2d73b981f60260e34 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -66,6 +66,12 @@ namespace armarx::armem::robot_state
queryJointState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+
+ JointTrajectory
+ queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin, const armem::Time& end) const;
+
std::optional<robot::RobotState::Pose>
queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
@@ -100,6 +106,10 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ JointTrajectory
+ getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
std::optional<robot::PlatformState>
getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index b73e65693e30f205b590a4e0c516fb5fff317127..c8a06a1b8329cb3df77280b1da18da556900ae93 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::lock_guard lock{dataMutex};
queueSize = dataQueue.size();
- if (dataQueue.size() >= properties.memoryBatchSize)
+ if (!dataQueue.empty())
{
std::swap(batch, dataQueue);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 22a07994e31341c8ebebb6edff872f6eea507f9c..4a4f14672b038132243bdbd5fc5a5617f3f5d668 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -84,7 +84,6 @@ namespace armarx::armem::server::robot_state::proprioception
struct Properties
{
- unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID;
};
Properties properties;