From 1e3ef0b1f11cf65a58abf8c4b49db5553b5991e5 Mon Sep 17 00:00:00 2001
From: ARMAR-DE <>
Date: Wed, 30 Aug 2023 13:56:26 +0200
Subject: [PATCH] draft (by CP)
---
.../ArticulatedObjectWriter.cpp | 10 ++--
.../client/articulated_object/Reader.cpp | 2 +-
.../libraries/armem_robot/aron/RobotState.xml | 6 ++
source/RobotAPI/libraries/armem_robot/types.h | 4 +-
.../armem_robot_state/CMakeLists.txt | 1 -
.../armem_robot_state/aron/JointState.xml | 14 -----
.../armem_robot_state/aron_conversions.cpp | 18 ------
.../armem_robot_state/aron_conversions.h | 3 -
.../client/common/RobotReader.cpp | 59 +++++++------------
.../client/common/RobotReader.h | 8 +--
.../client/common/RobotWriter.cpp | 1 -
.../client/common/VirtualRobotWriter.cpp | 2 +-
12 files changed, 41 insertions(+), 87 deletions(-)
delete mode 100644 source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 0b466d404..5e5b15f15 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -10,7 +10,6 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
namespace armarx::armem::articulated_object
{
armem::articulated_object::ArticulatedObject
@@ -37,13 +36,14 @@ namespace armarx::armem::articulated_object
return armem::articulated_object::ArticulatedObject{
.description = {.name = obj.getType(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject(
- {package}, fileRelPath),
- obj.getFilename())},
+ .xml = PackagePath(
+ armarx::ArmarXDataPath::getProject({package}, fileRelPath),
+ obj.getFilename())},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
.globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()},
+ .jointMap = obj.getJointValues(),
+ .proprioception = std::nullopt},
.timestamp = timestamp};
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index dd8a8473c..ac01fb3b8 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -262,7 +262,7 @@ namespace armarx::armem::articulated_object
robot::RobotState robotState{.timestamp = objectPose.timestamp,
.globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues};
+ .jointMap = objectPose.objectJointValues, .proprioception=std::nullopt};
return robotState;
}
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
index 4ef29cf75..38ea8a815 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
@@ -4,6 +4,9 @@
<CodeIncludes>
<SystemInclude include="<Eigen/Core>" />
</CodeIncludes>
+ <AronIncludes>
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/Proprioception.xml" />
+ </AronIncludes>
<GenerateTypes>
<Object name="armarx::armem::arondto::RobotState">
@@ -22,6 +25,9 @@
</Dict>
</ObjectChild>
+ <ObjectChild key='proprioception'>
+ <armarx::armem::arondto::Proprioception optional="true"/>
+ </ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 2eeff803f..9caf84f24 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -10,7 +10,7 @@
#include <ArmarXCore/core/time/DateTime.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
{
@@ -60,6 +60,8 @@ namespace armarx::armem::robot
Pose globalPose;
JointMap jointMap;
+
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 8dda90015..c71d1d839 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -63,7 +63,6 @@ armarx_enable_aron_file_generation_for_target(
"${LIB_NAME}"
ARON_FILES
- aron/JointState.xml
aron/Proprioception.xml
aron/Exteroception.xml
aron/TransformHeader.xml
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
deleted file mode 100644
index 5d18e8f78..000000000
--- a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
+++ /dev/null
@@ -1,14 +0,0 @@
-<!--This class contains the data structure for ObjectPose -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <GenerateTypes>
- <Object name="armarx::armem::arondto::JointState">
- <ObjectChild key='name'>
- <String/>
- </ObjectChild>
- <ObjectChild key='position'>
- <float32 />
- </ObjectChild>
- </Object>
- </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 901d98dcb..78014ea76 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -4,7 +4,6 @@
#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/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
@@ -51,23 +50,6 @@ namespace armarx::armem
aron::fromAron(dto.timestamp, bo.timestamp);
}
- /* JointState */
-
- 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)
- {
- aron::toAron(dto.name, bo.name);
- aron::toAron(dto.position, bo.position);
- }
-
-
void
fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index f78efc200..a3e821caa 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -61,9 +61,6 @@ namespace armarx::armem
void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-
void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
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 bfb85d68f..5fb5004c9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -23,7 +23,6 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.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>
@@ -186,12 +185,15 @@ namespace armarx::armem::robot_state
RobotReader::queryState(const robot::RobotDescription& description,
const armem::Time& timestamp) const
{
- const auto jointMap = queryJointState(description, timestamp);
- if (not jointMap)
+ const auto proprioception = queryProprioception(description, timestamp);
+
+ if (not proprioception.has_value())
{
- ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query proprioception for robot '" << description.name
+ << "'.";
return std::nullopt;
}
+ const auto jointMap = proprioception->joints.position;
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
@@ -200,8 +202,10 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::RobotState{
- .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
+ return robot::RobotState{.timestamp = timestamp,
+ .globalPose = *globalPose,
+ .jointMap = jointMap,
+ .proprioception = proprioception};
}
std::optional<::armarx::armem::robot_state::Transform>
@@ -231,10 +235,9 @@ namespace armarx::armem::robot_state
// }
}
-
- std::optional<robot::RobotState::JointMap>
- RobotReader::queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const // Why timestamp?!?!
+ std::optional<armarx::armem::arondto::Proprioception>
+ RobotReader::queryProprioception(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -263,7 +266,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getRobotJointState(qResult.memory, description.name);
+ return getRobotProprioception(qResult.memory, description.name);
}
catch (...)
{
@@ -304,7 +307,6 @@ namespace armarx::armem::robot_state
return getRobotJointStates(qResult.memory, description.name);
}
-
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -415,19 +417,19 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState::JointMap>
- RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ std::optional<armarx::armem::arondto::Proprioception>
+ RobotReader::getRobotProprioception(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(constants::proprioceptionCoreSegment);
// clang-format on
- std::optional<robot::RobotState::JointMap> jointMap;
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
coreSegment.forEachEntity(
- [&jointMap](const wm::Entity& entity)
+ [&proprioception](const wm::Entity& entity)
{
if (not entity.getLatestSnapshot().hasInstance(0))
{
@@ -436,25 +438,10 @@ namespace armarx::armem::robot_state
const auto& entityInstance = entity.getLatestSnapshot().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;
-
- // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // if (not jointState)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- // return;
- // }
-
- jointMap = joints.position;
-
- // jointMap.emplace(jointState->name, jointState->position);
+ proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
});
- return jointMap;
+ return proprioception;
}
RobotReader::JointTrajectory
@@ -497,7 +484,6 @@ namespace armarx::armem::robot_state
return jointTrajectory;
}
-
// force torque for left and right
std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
RobotReader::queryForceTorque(const robot::RobotDescription& description,
@@ -563,7 +549,6 @@ namespace armarx::armem::robot_state
return getForceTorques(qResult.memory, description.name);
}
-
std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
RobotReader::queryToF(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -594,7 +579,6 @@ namespace armarx::armem::robot_state
return getToF(qResult.memory, description.name);
}
-
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -755,7 +739,6 @@ namespace armarx::armem::robot_state
return tofs;
}
-
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
const std::string& name) const
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 3a2258558..6f90a9b24 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -66,8 +66,8 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
- std::optional<robot::RobotState::JointMap>
- queryJointState(const robot::RobotDescription& description,
+ std::optional<armarx::armem::arondto::Proprioception>
+ queryProprioception(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
@@ -134,8 +134,8 @@ namespace armarx::armem::robot_state
std::vector<robot::RobotDescription>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotState::JointMap>
- getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<armarx::armem::arondto::Proprioception>
+ getRobotProprioception(const armarx::armem::wm::Memory& memory, const std::string& name) const;
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index cfa84fb3f..f971c88c3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.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/aron/TransformHeader.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index 47519748c..fc3aed1ee 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -70,7 +70,7 @@ namespace armarx::armem::robot_state
const robot::RobotState robotState{.timestamp = timestamp,
.globalPose =
robot::RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues()};
+ .jointMap = robot.getJointValues(), .proprioception=std::nullopt};
return RobotWriter::storeState(robotState,
robot.getType(),
--
GitLab