Skip to content
Snippets Groups Projects
Commit 65976bbc authored by Fabian Tërnava's avatar Fabian Tërnava
Browse files

added default armar6 robotstatememory config

parent 1943e207
No related branches found
No related tags found
1 merge request!138updates to robot state memory
sens.Index L 1 Joint.acceleration => HandL
sens.Index L 1 Joint.gravityTorque => HandL
sens.Index L 1 Joint.inertiaTorque => HandL
sens.Index L 1 Joint.inverseDynamicsTorque => HandL
sens.Index L 1 Joint.position => HandL
sens.Index L 1 Joint.torque => HandL
sens.Index L 1 Joint.velocity => HandL
sens.Index L 2 Joint.acceleration => HandL
sens.Index L 2 Joint.gravityTorque => HandL
sens.Index L 2 Joint.inertiaTorque => HandL
sens.Index L 2 Joint.inverseDynamicsTorque => HandL
sens.Index L 2 Joint.position => HandL
sens.Index L 2 Joint.torque => HandL
sens.Index L 2 Joint.velocity => HandL
sens.Index L 3 Joint.acceleration => HandL
sens.Index L 3 Joint.gravityTorque => HandL
sens.Index L 3 Joint.inertiaTorque => HandL
sens.Index L 3 Joint.inverseDynamicsTorque => HandL
sens.Index L 3 Joint.position => HandL
sens.Index L 3 Joint.torque => HandL
sens.Index L 3 Joint.velocity => HandL
sens.Index R 1 Joint.acceleration => HandR
sens.Index R 1 Joint.gravityTorque => HandR
sens.Index R 1 Joint.inertiaTorque => HandR
sens.Index R 1 Joint.inverseDynamicsTorque => HandR
sens.Index R 1 Joint.position => HandR
sens.Index R 1 Joint.torque => HandR
sens.Index R 1 Joint.velocity => HandR
sens.Index R 2 Joint.acceleration => HandR
sens.Index R 2 Joint.gravityTorque => HandR
sens.Index R 2 Joint.inertiaTorque => HandR
sens.Index R 2 Joint.inverseDynamicsTorque => HandR
sens.Index R 2 Joint.position => HandR
sens.Index R 2 Joint.torque => HandR
sens.Index R 2 Joint.velocity => HandR
sens.Index R 3 Joint.acceleration => HandR
sens.Index R 3 Joint.gravityTorque => HandR
sens.Index R 3 Joint.inertiaTorque => HandR
sens.Index R 3 Joint.inverseDynamicsTorque => HandR
sens.Index R 3 Joint.position => HandR
sens.Index R 3 Joint.torque => HandR
sens.Index R 3 Joint.velocity => HandR
sens.Middle L 1 Joint.acceleration => HandL
sens.Middle L 1 Joint.gravityTorque => HandL
sens.Middle L 1 Joint.inertiaTorque => HandL
sens.Middle L 1 Joint.inverseDynamicsTorque => HandL
sens.Middle L 1 Joint.position => HandL
sens.Middle L 1 Joint.torque => HandL
sens.Middle L 1 Joint.velocity => HandL
sens.Middle L 2 Joint.acceleration => HandL
sens.Middle L 2 Joint.gravityTorque => HandL
sens.Middle L 2 Joint.inertiaTorque => HandL
sens.Middle L 2 Joint.inverseDynamicsTorque => HandL
sens.Middle L 2 Joint.position => HandL
sens.Middle L 2 Joint.torque => HandL
sens.Middle L 2 Joint.velocity => HandL
sens.Middle L 3 Joint.acceleration => HandL
sens.Middle L 3 Joint.gravityTorque => HandL
sens.Middle L 3 Joint.inertiaTorque => HandL
sens.Middle L 3 Joint.inverseDynamicsTorque => HandL
sens.Middle L 3 Joint.position => HandL
sens.Middle L 3 Joint.torque => HandL
sens.Middle L 3 Joint.velocity => HandL
sens.Middle R 1 Joint.acceleration => HandR
sens.Middle R 1 Joint.gravityTorque => HandR
sens.Middle R 1 Joint.inertiaTorque => HandR
sens.Middle R 1 Joint.inverseDynamicsTorque => HandR
sens.Middle R 1 Joint.position => HandR
sens.Middle R 1 Joint.torque => HandR
sens.Middle R 1 Joint.velocity => HandR
sens.Middle R 2 Joint.acceleration => HandR
sens.Middle R 2 Joint.gravityTorque => HandR
sens.Middle R 2 Joint.inertiaTorque => HandR
sens.Middle R 2 Joint.inverseDynamicsTorque => HandR
sens.Middle R 2 Joint.position => HandR
sens.Middle R 2 Joint.torque => HandR
sens.Middle R 2 Joint.velocity => HandR
sens.Middle R 3 Joint.acceleration => HandR
sens.Middle R 3 Joint.gravityTorque => HandR
sens.Middle R 3 Joint.inertiaTorque => HandR
sens.Middle R 3 Joint.inverseDynamicsTorque => HandR
sens.Middle R 3 Joint.position => HandR
sens.Middle R 3 Joint.torque => HandR
sens.Middle R 3 Joint.velocity => HandR
sens.Pinky L 1 Joint.acceleration => HandL
sens.Pinky L 1 Joint.gravityTorque => HandL
sens.Pinky L 1 Joint.inertiaTorque => HandL
sens.Pinky L 1 Joint.inverseDynamicsTorque => HandL
sens.Pinky L 1 Joint.position => HandL
sens.Pinky L 1 Joint.torque => HandL
sens.Pinky L 1 Joint.velocity => HandL
sens.Pinky L 2 Joint.acceleration => HandL
sens.Pinky L 2 Joint.gravityTorque => HandL
sens.Pinky L 2 Joint.inertiaTorque => HandL
sens.Pinky L 2 Joint.inverseDynamicsTorque => HandL
sens.Pinky L 2 Joint.position => HandL
sens.Pinky L 2 Joint.torque => HandL
sens.Pinky L 2 Joint.velocity => HandL
sens.Pinky L 3 Joint.acceleration => HandL
sens.Pinky L 3 Joint.gravityTorque => HandL
sens.Pinky L 3 Joint.inertiaTorque => HandL
sens.Pinky L 3 Joint.inverseDynamicsTorque => HandL
sens.Pinky L 3 Joint.position => HandL
sens.Pinky L 3 Joint.torque => HandL
sens.Pinky L 3 Joint.velocity => HandL
sens.Pinky R 1 Joint.acceleration => HandR
sens.Pinky R 1 Joint.gravityTorque => HandR
sens.Pinky R 1 Joint.inertiaTorque => HandR
sens.Pinky R 1 Joint.inverseDynamicsTorque => HandR
sens.Pinky R 1 Joint.position => HandR
sens.Pinky R 1 Joint.torque => HandR
sens.Pinky R 1 Joint.velocity => HandR
sens.Pinky R 2 Joint.acceleration => HandR
sens.Pinky R 2 Joint.gravityTorque => HandR
sens.Pinky R 2 Joint.inertiaTorque => HandR
sens.Pinky R 2 Joint.inverseDynamicsTorque
sens.Pinky R 2 Joint.position => HandR
sens.Pinky R 2 Joint.torque => HandR
sens.Pinky R 2 Joint.velocity => HandR
sens.Pinky R 3 Joint.acceleration => HandR
sens.Pinky R 3 Joint.gravityTorque => HandR
sens.Pinky R 3 Joint.inertiaTorque => HandR
sens.Pinky R 3 Joint.inverseDynamicsTorque => HandR
sens.Pinky R 3 Joint.position => HandR
sens.Pinky R 3 Joint.torque => HandR
sens.Pinky R 3 Joint.velocity => HandR
sens.Ring L 1 Joint.acceleration
sens.Ring L 1 Joint.gravityTorque
sens.Ring L 1 Joint.inertiaTorque
sens.Ring L 1 Joint.inverseDynamicsTorque
sens.Ring L 1 Joint.position
sens.Ring L 1 Joint.torque
sens.Ring L 1 Joint.velocity
sens.Ring L 2 Joint.acceleration
sens.Ring L 2 Joint.gravityTorque
sens.Ring L 2 Joint.inertiaTorque
sens.Ring L 2 Joint.inverseDynamicsTorque
sens.Ring L 2 Joint.position
sens.Ring L 2 Joint.torque
sens.Ring L 2 Joint.velocity
sens.Ring L 3 Joint.acceleration
sens.Ring L 3 Joint.gravityTorque
sens.Ring L 3 Joint.inertiaTorque
sens.Ring L 3 Joint.inverseDynamicsTorque
sens.Ring L 3 Joint.position
sens.Ring L 3 Joint.torque
sens.Ring L 3 Joint.velocity
sens.Ring R 1 Joint.acceleration
sens.Ring R 1 Joint.gravityTorque
sens.Ring R 1 Joint.inertiaTorque
sens.Ring R 1 Joint.inverseDynamicsTorque
sens.Ring R 1 Joint.position
sens.Ring R 1 Joint.torque
sens.Ring R 1 Joint.velocity
sens.Ring R 2 Joint.acceleration
sens.Ring R 2 Joint.gravityTorque
sens.Ring R 2 Joint.inertiaTorque
sens.Ring R 2 Joint.inverseDynamicsTorque
sens.Ring R 2 Joint.position
sens.Ring R 2 Joint.torque
sens.Ring R 2 Joint.velocity
sens.Ring R 3 Joint.acceleration
sens.Ring R 3 Joint.gravityTorque
sens.Ring R 3 Joint.inertiaTorque
sens.Ring R 3 Joint.inverseDynamicsTorque
sens.Ring R 3 Joint.position
sens.Ring R 3 Joint.torque
sens.Ring R 3 Joint.velocity
sens.Thumb L 1 Joint.acceleration
sens.Thumb L 1 Joint.gravityTorque
sens.Thumb L 1 Joint.inertiaTorque
sens.Thumb L 1 Joint.inverseDynamicsTorque
sens.Thumb L 1 Joint.position
sens.Thumb L 1 Joint.torque
sens.Thumb L 1 Joint.velocity
sens.Thumb L 2 Joint.acceleration
sens.Thumb L 2 Joint.gravityTorque
sens.Thumb L 2 Joint.inertiaTorque
sens.Thumb L 2 Joint.inverseDynamicsTorque
sens.Thumb L 2 Joint.position
sens.Thumb L 2 Joint.torque
sens.Thumb L 2 Joint.velocity
sens.Thumb R 1 Joint.acceleration
sens.Thumb R 1 Joint.gravityTorque
sens.Thumb R 1 Joint.inertiaTorque
sens.Thumb R 1 Joint.inverseDynamicsTorque
sens.Thumb R 1 Joint.position
sens.Thumb R 1 Joint.torque
sens.Thumb R 1 Joint.velocity
sens.Thumb R 2 Joint.acceleration
sens.Thumb R 2 Joint.gravityTorque
sens.Thumb R 2 Joint.inertiaTorque
sens.Thumb R 2 Joint.inverseDynamicsTorque
sens.Thumb R 2 Joint.position
sens.Thumb R 2 Joint.torque
sens.Thumb R 2 Joint.velocity
sens.TorsoJoint.acceleration
sens.TorsoJoint.gravityTorque
sens.TorsoJoint.inertiaTorque
sens.TorsoJoint.inverseDynamicsTorque
sens.TorsoJoint.position
sens.TorsoJoint.torque
sens.TorsoJoint.velocity
# memory servers
add_subdirectory(server/ExampleMemory)
add_subdirectory(server/GeneralPurposeMemory)
add_subdirectory(server/RobotSensorMemory)
add_subdirectory(server/RobotStateMemory)
add_subdirectory(server/SkillsMemory)
# memory server addons
......
armarx_component_set_name("RobotSensorMemory")
armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
......@@ -9,10 +9,10 @@ set(COMPONENT_LIBS
)
set(SOURCES
RobotSensorMemory.cpp
RobotStateMemory.cpp
)
set(HEADERS
RobotSensorMemory.h
RobotStateMemory.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
......
......@@ -22,9 +22,11 @@
// STD
#include <algorithm>
#include <iostream>
#include <fstream>
// Header
#include "RobotSensorMemory.h"
#include "RobotStateMemory.h"
// Simox
#include <SimoxUtility/algorithm/string.h>
......@@ -39,31 +41,33 @@
namespace armarx
{
RobotSensorMemory::RobotSensorMemory()
RobotStateMemory::RobotStateMemory()
{
}
armarx::PropertyDefinitionsPtr RobotSensorMemory::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->optional(robotUnitSensorPrefix, "SensorValuePrefix", "Prefix of all sensor values");
defs->optional(robotUnitMemoryBatchSize, "RobotUnitMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
defs->optional(robotUnitPollFrequency, "RobotUnitUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
defs->optional(robotUnitConfigPath, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
defs->optional(robotStateComponentMemoryBatchSize, "RobotStateComponentMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
defs->optional(robotStateComponentPollFrequency, "RobotStateComponentUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_STATE_COMPONENT_MAXIMUM_FREQUENCY));
return defs;
}
std::string RobotSensorMemory::getDefaultName() const
std::string RobotStateMemory::getDefaultName() const
{
return "RobotSensorMemory";
}
void RobotSensorMemory::onInitComponent()
void RobotStateMemory::onInitComponent()
{
robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
......@@ -75,7 +79,7 @@ namespace armarx
}
void RobotSensorMemory::onConnectComponent()
void RobotStateMemory::onConnectComponent()
{
setupRobotUnitSegment();
setupRobotStateComponentSegment();
......@@ -84,25 +88,25 @@ namespace armarx
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
keys = handler->getDataDescription();
robotUnitConversionTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
robotUnitStoringTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
robotUnitConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
robotUnitStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
robotStateComponentConversionTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::convertRobotStateComponentDataToAron, (1000 / robotStateComponentPollFrequency));
robotStateComponentStoringTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::storeConvertedRobotStateComponentDataInMemory, (1000 / robotStateComponentPollFrequency));
robotStateComponentConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotStateComponentDataToAron, (1000 / robotStateComponentPollFrequency));
robotStateComponentStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotStateComponentDataInMemory, (1000 / robotStateComponentPollFrequency));
startRobotUnitStream();
startRobotStateComponentPoll();
}
void RobotSensorMemory::onDisconnectComponent()
void RobotStateMemory::onDisconnectComponent()
{
stopRobotUnitStream();
startRobotStateComponentPoll();
}
void RobotSensorMemory::onExitComponent()
void RobotStateMemory::onExitComponent()
{
stopRobotUnitStream();
stopRobotStateComponentPoll();
......@@ -111,7 +115,7 @@ namespace armarx
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
void RobotSensorMemory::setupRobotUnitSegment()
void RobotStateMemory::setupRobotUnitSegment()
{
ARMARX_INFO << "Adding core segment " << robotUnitCoreSegmentName;
memory.addCoreSegments({robotUnitCoreSegmentName});
......@@ -141,7 +145,7 @@ namespace armarx
robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
}
void RobotSensorMemory::convertRobotUnitStreamingDataToAron()
void RobotStateMemory::convertRobotUnitStreamingDataToAron()
{
auto& data = handler->getDataBuffer();
if (data.size() == 0)
......@@ -156,14 +160,22 @@ namespace armarx
RobotUnitData convertedAndGroupedData;
for (const auto& [name, dataEntry] : keys.entries)
{
size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
if (dot_pos == std::string::npos)
std::string groupName = "";
if (configSensorMapping.find(name) != configSensorMapping.end())
{
ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
continue;
groupName = configSensorMapping.at(name);
}
else
{
size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
if (dot_pos == std::string::npos)
{
ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
continue;
}
groupName = name.substr(0, dot_pos);
}
std::string groupName = name.substr(0, dot_pos);
RobotUnitData::RobotUnitDataGroup e;
if (auto it = convertedAndGroupedData.groups.find(groupName); it == convertedAndGroupedData.groups.end())
{
......@@ -251,7 +263,7 @@ namespace armarx
ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
}
void RobotSensorMemory::storeConvertedRobotUnitDataInMemory()
void RobotStateMemory::storeConvertedRobotUnitDataInMemory()
{
unsigned int size = 0;
{
......@@ -301,14 +313,14 @@ namespace armarx
}
}
void RobotSensorMemory::stopRobotUnitStream()
void RobotStateMemory::stopRobotUnitStream()
{
std::lock_guard g{startStopMutex};
robotUnitConversionTask->stop();
robotUnitStoringTask->stop();
}
void RobotSensorMemory::startRobotUnitStream()
void RobotStateMemory::startRobotUnitStream()
{
std::lock_guard g{startStopMutex};
if (robotUnitConversionTask->isRunning() || robotUnitStoringTask->isRunning())
......@@ -368,6 +380,41 @@ namespace armarx
}
ARMARX_INFO << ss.str();
// parse config
std::filesystem::path configPath(robotUnitConfigPath);
configSensorMapping.clear();
if(std::filesystem::is_regular_file(configPath))
{
// a simple self-made parser for the config file. Extend it if you need to
std::ifstream in(configPath);
std::string line;
while (std::getline(in, line))
{
if(line.size() > 0)
{
if(simox::alg::starts_with(line, "//"))
{
continue;
}
std::vector<std::string> mappings = simox::alg::split(line, ",");
for(const auto& mapping : mappings)
{
std::vector<std::string> split = simox::alg::split(mapping, "=>");
if(split.size() < 2)
{
ARMARX_WARNING << "A mapping in the RobotUnitConfig file is not valid. Ignoring it.";
continue;
}
for (unsigned int i = 0; i < split.size() - 1; ++i)
{
configSensorMapping.insert({split[i], split[split.size()-1]});
}
}
}
}
}
robotUnitConversionTask->start();
robotUnitStoringTask->start();
}
......@@ -376,7 +423,7 @@ namespace armarx
/*************************************************************/
// RobotStateComponent Polling functions
/*************************************************************/
void RobotSensorMemory::setupRobotStateComponentSegment()
void RobotStateMemory::setupRobotStateComponentSegment()
{
ARMARX_INFO << "Adding core segment " << robotStateComponentCoreSegmentName;
memory.addCoreSegments({robotStateComponentCoreSegmentName});
......@@ -400,7 +447,7 @@ namespace armarx
robotStateComponentProviderID.providerSegmentName = robotStateComponentProviderSegmentName;
}
void RobotSensorMemory::convertRobotStateComponentDataToAron()
void RobotStateMemory::convertRobotStateComponentDataToAron()
{
ARMARX_DEBUG << "RobotStateComponentData: Generating new aron map for current timestep";
auto start = std::chrono::high_resolution_clock::now();
......@@ -445,7 +492,7 @@ namespace armarx
ARMARX_DEBUG << "RobotStateComponentData: The total time needed to convert the data to aron is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
}
void RobotSensorMemory::storeConvertedRobotStateComponentDataInMemory()
void RobotStateMemory::storeConvertedRobotStateComponentDataInMemory()
{
unsigned int size = 0;
{
......@@ -491,14 +538,14 @@ namespace armarx
}
}
void RobotSensorMemory::stopRobotStateComponentPoll()
void RobotStateMemory::stopRobotStateComponentPoll()
{
std::lock_guard g{startStopMutex};
robotStateComponentConversionTask->stop();
robotStateComponentStoringTask->stop();
}
void RobotSensorMemory::startRobotStateComponentPoll()
void RobotStateMemory::startRobotStateComponentPoll()
{
std::lock_guard g{startStopMutex};
if (robotStateComponentConversionTask->isRunning() || robotStateComponentStoringTask->isRunning())
......
......@@ -55,7 +55,7 @@ namespace armarx
*
* Detailed description of class RobotSensorMemory.
*/
class RobotSensorMemory :
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
virtual public RobotUnitComponentPluginUser,
......@@ -63,7 +63,7 @@ namespace armarx
{
public:
RobotSensorMemory();
RobotStateMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
......@@ -141,14 +141,18 @@ namespace armarx
int robotUnitPollFrequency = 50;
std::string robotUnitSensorPrefix = "sens.*";
unsigned int robotUnitMemoryBatchSize = 50;
std::string robotUnitConfigPath = "NO CONFIG SET";
// RobotUnit stuff
std::map<std::string, std::string> configSensorMapping;
// queue
std::queue<RobotUnitData> robotUnitDataQueue;
mutable std::mutex robotUnitDataMutex;
// running tasks
armarx::PeriodicTask<RobotSensorMemory>::pointer_type robotUnitConversionTask;
armarx::PeriodicTask<RobotSensorMemory>::pointer_type robotUnitStoringTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
// RobotStateComponent stuff
struct RobotStateComponentData
......@@ -167,7 +171,7 @@ namespace armarx
mutable std::mutex robotStateComponentDataMutex;
// running tasks
armarx::PeriodicTask<RobotSensorMemory>::pointer_type robotStateComponentConversionTask;
armarx::PeriodicTask<RobotSensorMemory>::pointer_type robotStateComponentStoringTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotStateComponentConversionTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotStateComponentStoringTask;
};
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment