diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
index d85e38be8313b55a1981cecdca2c4a2f5cad1b5b..6054b8a075ccf4e9ca53e91906228b6d4ef4b284 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
@@ -1,5 +1,7 @@
#include "Armar6RobotUnitConverter.h"
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
@@ -31,10 +33,6 @@ namespace armarx::armem::server::robot_state
{
return &p.acceleration;
};
- platformPoseGetters["absolutePosition"] = [](prop::arondto::Platform & p)
- {
- return &p.absolutePosition;
- };
platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
{
return &p.relativePosition;
@@ -43,6 +41,7 @@ namespace armarx::armem::server::robot_state
{
return &p.velocity;
};
+ platformIgnored.insert("absolutePosition");
}
{
ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
@@ -65,33 +64,86 @@ namespace armarx::armem::server::robot_state
{
jointGetters["acceleration"] = [](prop::arondto::Joints & j)
{
- return &j.accellerations;
+ return &j.acceleration;
};
jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
{
- return &j.gravityTorques;
+ return &j.gravityTorque;
};
jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
{
- return &j.inertiaTorques;
+ return &j.inertiaTorque;
};
jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
{
- return &j.inverseDynamicsTorques;
+ return &j.inverseDynamicsTorque;
};
jointGetters["position"] = [](prop::arondto::Joints & j)
{
- return &j.positions;
+ return &j.position;
};
jointGetters["torque"] = [](prop::arondto::Joints & j)
{
- return &j.torques;
+ return &j.torque;
};
jointGetters["velocity"] = [](prop::arondto::Joints & j)
{
- return &j.velocities;
+ return &j.velocity;
};
}
+ {
+ jointSetters["position"] = [](prop::arondto::Joints & dto, const std::vector<std::string>& split, const Value & value)
+ {
+ dto.position[split.at(1)] = getValueAs<float>(value);
+ };
+
+
+
+#define ADD_GETTER(container, name) \
+ container [ #name ] = [](prop::arondto::Joints & j) \
+ { \
+ return &j. name; \
+ }
+ ADD_GETTER(jointGetters, position);
+ ADD_GETTER(jointGetters, velocity);
+ ADD_GETTER(jointGetters, acceleration);
+
+ ADD_GETTER(jointGetters, relativePosition);
+ ADD_GETTER(jointGetters, filteredVelocity);
+
+ ADD_GETTER(jointGetters, currentTarget);
+ ADD_GETTER(jointGetters, positionTarget);
+ ADD_GETTER(jointGetters, velocityTarget);
+
+ ADD_GETTER(jointGetters, torque);
+ ADD_GETTER(jointGetters, inertiaTorque);
+ ADD_GETTER(jointGetters, gravityTorque);
+ ADD_GETTER(jointGetters, gravityCompensatedTorque);
+ ADD_GETTER(jointGetters, inverseDynamicsTorque);
+ // ADD_GETTER(jointGetters, torqueTicks);
+
+ ADD_GETTER(jointGetters, positionTarget);
+ ADD_GETTER(jointGetters, currentTarget);
+ ADD_GETTER(jointGetters, positionTarget);
+
+ // ADD_GETTER(jointVectorGetters, orientation);
+ // ADD_GETTER(jointVectorGetters, angularVelocity);
+ // ADD_GETTER(jointVectorGetters, linearAcceleration);
+
+ // ADD_GETTER(jointGetters, temperature);
+
+ // ADD_GETTER(jointGetters, motorCurrent);
+ // ADD_GETTER(jointGetters, maxTargetCurrent);
+
+ // ADD_GETTER(jointGetters, sensorBoardUpdateRate);
+ // ADD_GETTER(jointGetters, I2CUpdateRate);
+
+ // ADD_GETTER(jointGetters, JointStatusEmergencyStop);
+ // ADD_GETTER(jointGetters, JointStatusEnabled);
+ // ADD_GETTER(jointGetters, JointStatusError);
+ // ADD_GETTER(jointGetters, JointStatusOperation);
+#undef ADD_GETTER
+ }
}
@@ -109,49 +161,44 @@ namespace armarx::armem::server::robot_state
for (const auto& [dataEntryName, dataEntry] : description.entries)
{
- switch (dataEntry.type)
- {
- case RobotUnitDataStreaming::NodeTypeFloat:
- {
- const float value = RobotUnitDataStreamingReceiver::GetAs<float>(data, dataEntry);
-
- const std::vector<std::string> split = simox::alg::split(dataEntryName, ".", false, false);
- ARMARX_CHECK_EQUAL(split.size(), 3) << split;
- const std::string& category = split.at(0);
- const std::string& name = split.at(1);
- const std::string& field = split.at(2);
-
- ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << dataEntryName;
-
- if (name == "Platform")
- {
- // Platform
- processPlatformEntry(dto.platform, field, value);
- }
- else if (simox::alg::starts_with(name, "FT"))
- {
- // Force Torque
- processForceTorqueEntry(dto.forceTorque, name, field, value);
- }
- else
- {
- // Joint
- processJointEntry(dto.joints, name, field, value);
- }
- }
- break;
-
- default:
- {
- ARMARX_DEBUG << "Ignoring non-float entry '" << dataEntryName << "'.";
- }
- break;
- }
+ process(dto, dataEntryName, {data, dataEntry});
}
return dto;
}
+ void Armar6RobotUnitConverter::process(
+ arondto::Proprioception dto,
+ const std::string& entryName,
+ const Armar6RobotUnitConverter::Value& value)
+ {
+ const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
+ ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
+
+ const std::string& category = split.at(0);
+ const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (name == "Platform")
+ {
+ // Platform
+ processPlatformEntry(dto.platform, field, value);
+ }
+ else if (simox::alg::starts_with(name, "FT"))
+ {
+ // Force Torque
+ processForceTorqueEntry(dto.forceTorque, name, field, value);
+ }
+ else
+ {
+ // Joint
+ processJointEntry(dto.joints, split, value);
+ }
+ }
+
+
+
template <class ValueT>
ValueT findByPrefix(const std::string& key,
const std::map<std::string, ValueT>& map)
@@ -165,6 +212,17 @@ namespace armarx::armem::server::robot_state
}
return nullptr;
}
+ bool findByPrefix(const std::string& key, const std::set<std::string>& set)
+ {
+ for (const auto& prefix : set)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
template <class ValueT>
ValueT findBySuffix(const std::string& key,
const std::map<std::string, ValueT>& map)
@@ -183,15 +241,19 @@ namespace armarx::armem::server::robot_state
void Armar6RobotUnitConverter::processPlatformEntry(
prop::arondto::Platform& dto,
const std::string& fieldName,
- float value)
+ const Value& value)
{
+ if (findByPrefix(fieldName, platformIgnored))
+ {
+ return;
+ }
if (auto getter = findByPrefix(fieldName, platformPoseGetters))
{
if (Eigen::Vector3f* dst = getter(dto))
{
if (auto setter = findBySuffix(fieldName, vector3fSetters))
{
- setter(*dst, value);
+ setter(*dst, getValueAs<float>(value));
}
}
}
@@ -202,7 +264,7 @@ namespace armarx::armem::server::robot_state
std::map<std::string, prop::arondto::ForceTorque>& fts,
const std::string& name,
const std::string& fieldName,
- float value)
+ const Value& value)
{
std::vector<std::string> split = simox::alg::split(name, " ", false, false);
ARMARX_CHECK_EQUAL(split.size(), 2);
@@ -219,7 +281,7 @@ namespace armarx::armem::server::robot_state
void Armar6RobotUnitConverter::processForceTorqueEntry(
prop::arondto::ForceTorque& dto,
const std::string& fieldName,
- float value)
+ const Value& value)
{
if (auto getter = findByPrefix(fieldName, ftGetters))
{
@@ -227,7 +289,7 @@ namespace armarx::armem::server::robot_state
{
if (auto setter = findBySuffix(fieldName, vector3fSetters))
{
- setter(*dst, value);
+ setter(*dst, getValueAs<float>(value));
}
}
}
@@ -236,17 +298,42 @@ namespace armarx::armem::server::robot_state
void Armar6RobotUnitConverter::processJointEntry(
prop::arondto::Joints& dto,
- const std::string& name,
- const std::string& fieldName,
- float value)
+ const std::vector<std::string>& split,
+ const Value& value)
{
- if (auto getter = findByPrefix(fieldName, jointGetters))
+ const std::string& jointName = split.at(1);
+ const std::string& fieldName = split.at(2);
+ if (split.size() == 3)
{
- if (std::map<std::string, float>* map = getter(dto))
+ // Only in simulation.
+ if (auto getter = findByPrefix(fieldName, jointGetters))
{
- (*map)[name] = value;
+ if (std::map<std::string, float>* map = getter(dto))
+ {
+ (*map)[jointName] = getValueAs<float>(value);
+ }
}
}
+ else if (split.size() == 4)
+ {
+ auto it = jointSetters.find(fieldName);
+ ARMARX_CHECK(it != jointSetters.end()) << fieldName << " | " << simox::alg::get_keys(jointSetters);
+
+ const JointSetter& setter = it->second;
+ setter(dto, split, value);
+ }
+ else
+ {
+ ARMARX_CHECK(false) << "Data entry name could not be parsed (exected 3 or 4 components between '.'): "
+ << "\n- split: '" << split << "'";
+ }
+ }
+
+
+ template<class T>
+ T Armar6RobotUnitConverter::getValueAs(const Value& value)
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
index d365edbe0b3d3b1e0fa91d34af9e9137b5b8cae9..e01d7160f220ad2ab739ff954ed0e4b69a054d30 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
@@ -13,8 +13,21 @@
namespace armarx::armem::server::robot_state
{
+ class Setter
+ {
+
+ };
+
+ class Vector3Setter
+ {
+
+ };
+
+
class Armar6RobotUnitConverter : public RobotUnitConverterInterface
{
+ struct Value;
+
public:
Armar6RobotUnitConverter();
@@ -26,38 +39,63 @@ namespace armarx::armem::server::robot_state
const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+ protected:
+
+ void process(arondto::Proprioception dto, const std::string& entryName, const Value& value);
+
+
+
private:
+ struct Value
+ {
+ const RobotUnitDataStreaming::TimeStep& data;
+ const RobotUnitDataStreaming::DataEntry& entry;
+ };
+
void processPlatformEntry(
prop::arondto::Platform& dto,
const std::string& fieldName,
- float value);
+ const Value& value);
void processForceTorqueEntry(
std::map<std::string, prop::arondto::ForceTorque>& fts,
const std::string& name,
const std::string& fieldName,
- float value);
+ const Value& value);
void processForceTorqueEntry(
prop::arondto::ForceTorque& ft,
const std::string& fieldName,
- float value);
+ const Value& value);
void processJointEntry(
prop::arondto::Joints& dto,
- const std::string& name,
- const std::string& fieldName,
- float value);
+ const std::vector<std::string>& split,
+ const Value& value);
+
+
+ private:
+
+ template <class T>
+ static T getValueAs(const Value& value);
private:
std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+ std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+
+ using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const Value& value)>;
+ std::map<std::string, JointSetter> jointSetters;
+
std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::set<std::string> platformIgnored;
+
std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
- std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+
std::map<std::string, std::string> sidePrefixMap
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index c3fb3d182e6110cc6cf8e6a5beb59ca19dbab80b..46857c8925f49da40b20c210ba14a677afe177a9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -3,51 +3,180 @@
Robot proprioception.
-->
<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
+ </CodeIncludes>
+
<GenerateTypes>
<Object name="armarx::armem::prop::arondto::Joints">
- <ObjectChild key="positions">
+ <ObjectChild key="position">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="velocities">
+ <ObjectChild key="velocity">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="accellerations">
+ <ObjectChild key="acceleration">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="relativePosition">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="filteredVelocity">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="currentTarget">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key="positionTarget">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="velocityTarget">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="torques">
+
+ <ObjectChild key="torque">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="inertiaTorques">
+ <ObjectChild key="inertiaTorque">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="gravityTorques">
+ <ObjectChild key="gravityTorque">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensatedTorque">
<Dict>
<Float />
</Dict>
</ObjectChild>
- <ObjectChild key="inverseDynamicsTorques">
+ <ObjectChild key="inverseDynamicsTorque">
<Dict>
<Float />
</Dict>
</ObjectChild>
+
+ <ObjectChild key="torqueTicks">
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="orientation">
+ <Dict>
+ <EigenMatrix rows="4" cols="1" type="float" />
+ <!--Orientation /-->
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="angularVelocity">
+ <Dict>
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="linearAcceleration">
+ <Dict>
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="temperature">
+ <Dict>
+ <Dict>
+ <Float />
+ </Dict>
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="motorCurrent">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="maxTargetCurrent">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="sensorBoardUpdateRate">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="I2CUpdateRate">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ <ObjectChild key="JointStatusEmergencyStop">
+ <Dict>
+ <Bool />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="JointStatusEnabled">
+ <Dict>
+ <Bool />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="JointStatusError">
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="JointStatusOperation">
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
</Object>
@@ -58,9 +187,10 @@ Robot proprioception.
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
- <ObjectChild key="absolutePosition">
+ <!-- Global pose is not part of proprioception. -->
+ <!--ObjectChild key="absolutePosition">
<EigenMatrix rows="3" cols="1" type="float" />
- </ObjectChild>
+ </ObjectChild-->
<ObjectChild key="velocity">
<EigenMatrix rows="3" cols="1" type="float" />