From b63797e092e6fc2f78fd4d3f2f24c691181d8fb7 Mon Sep 17 00:00:00 2001
From: Meixner <andre.meixner@kit.edu>
Date: Fri, 3 Nov 2023 16:42:44 +0100
Subject: [PATCH] Added visualization of force torque sensor (arviz) in the
robot state memory
---
.../armem_robot_state/server/CMakeLists.txt | 4 +
.../armem_robot_state/server/common/Visu.cpp | 73 +++++++++++++--
.../armem_robot_state/server/common/Visu.h | 7 ++
.../server/common/combine.cpp | 10 +-
.../armem_robot_state/server/common/combine.h | 2 +-
.../server/forward_declarations.h | 7 +-
.../server/proprioception/Segment.cpp | 91 +++++++++++++++----
.../server/proprioception/Segment.h | 8 +-
.../server/proprioception/SensorValues.cpp | 2 +
.../server/proprioception/SensorValues.h | 50 ++++++++++
10 files changed, 218 insertions(+), 36 deletions(-)
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 3ad1d5068..54d582387 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -20,10 +20,12 @@ armarx_add_library(
# This package
RobotAPICore
RobotAPIInterfaces
+ RobotStatechartHelpers
RobotAPI::armem_server
RobotAPI::armem_robot
RobotAPI::armem_robot_state
aroncommon
+ aroneigenconverter
# System / External
Eigen3::Eigen
@@ -38,6 +40,7 @@ armarx_add_library(
localization/Segment.h
proprioception/Segment.h
+ proprioception/SensorValues.h
proprioception/aron_conversions.h
proprioception/RobotStateWriter.h
proprioception/RobotUnitData.h
@@ -63,6 +66,7 @@ armarx_add_library(
localization/Segment.cpp
proprioception/Segment.cpp
+ proprioception/SensorValues.cpp
proprioception/aron_conversions.cpp
proprioception/RobotStateWriter.cpp
proprioception/RobotUnitData.cpp
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 41eaf6e2e..e0e1721cd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -9,6 +9,8 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -21,6 +23,7 @@
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
#include "combine.h"
@@ -45,6 +48,8 @@ namespace armarx::armem::server::robot_state
defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.framesEnabled, prefix + "famesEnabled", "Enable or disable visualization of frames.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
+ defs->optional(p.visualizeForceTorqueSensor, prefix + "visualizeForceTorqueSensor",
+ "Enable or disable visualization of force torque sensors.");
}
@@ -191,11 +196,11 @@ namespace armarx::armem::server::robot_state
const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
- TIMING_START(tJointPositions);
- const auto jointPositions =
- proprioceptionSegment.getRobotJointPositionsLocking(
+ TIMING_START(tSensorValues);
+ const auto sensorValues =
+ proprioceptionSegment.getSensorValuesLocking(
timestamp, debugObserver ? &*debugObserver : nullptr);
- TIMING_END_STREAM(tJointPositions, ARMARX_DEBUG);
+ TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
@@ -211,10 +216,10 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Combining robot ..."
<< "\n- " << robotDescriptions.size() << " descriptions"
<< "\n- " << globalPoses.size() << " global poses"
- << "\n- " << jointPositions.size() << " joint positions";
+ << "\n- " << sensorValues.size() << " joint positions";
const robot::Robots robots =
- combine(robotDescriptions, globalPoses, jointPositions, timestamp);
+ combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
viz::Layer layer = arviz.layer("Robots");
@@ -237,6 +242,60 @@ namespace armarx::armem::server::robot_state
layers.push_back(layerFrames);
}
+ if (p.visualizeForceTorqueSensor)
+ {
+ viz::Layer layerFrames = arviz.layer("ForceTorque");
+ for (const robot::Robot& robot : robots)
+ {
+ const std::string& name = robot.description.name;
+ if (robotNameHelper.find(name) == robotNameHelper.end())
+ {
+ const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
+ robotNameHelper[name] = RobotNameHelper::Create(robotPath);
+ robotModels[name]
+ = VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure);
+ }
+
+ auto model = robotModels.at(name);
+ model->setJointValues(robot.config.jointMap);
+ model->setGlobalPose(robot.config.globalPose.matrix());
+
+ const proprioception::ForceTorqueValuesMap& forceTorques
+ = sensorValues.at(name).forceTorqueValuesMap;
+
+ // TODO: Make parameterizable.
+ const float scaling = 1;
+
+ for (const auto& [side, ft] : forceTorques)
+ {
+ ARMARX_CHECK(side == RobotNameHelper::LocationLeft or side == RobotNameHelper::LocationRight) << side;
+
+ const std::string forceTorqueSensorName =
+ robotNameHelper.at(name)->getArm(side).getForceTorqueSensor();
+
+ const Eigen::Matrix4f forceTorqueSensorPose
+ = model->getSensor(forceTorqueSensorName)->getGlobalPose();
+
+ const std::string xyz = "XYZ";
+
+ const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
+ for (int i = 0; i < 3; ++i)
+ {
+ simox::Color color = simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
+ color.a = 128;
+
+ Eigen::Vector3f to = from + scaling * ft.force(i) *
+ simox::math::orientation(forceTorqueSensorPose).col(i);
+
+ layerFrames.add(viz::Arrow(side + " Force " + std::to_string(xyz.at(i)))
+ .fromTo(from, to).color(color).width(100));
+ }
+ }
+
+ }
+ layers.push_back(layerFrames);
+ }
+
// Commit layers.
@@ -255,7 +314,7 @@ namespace armarx::armem::server::robot_state
debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)", tGlobalPoses.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.4 Joint Positions (ms)", tJointPositions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)", tSensorValues.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 99321b656..39f0af9eb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -33,6 +33,8 @@
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
+
namespace armarx::armem::server::robot_state
{
@@ -86,10 +88,15 @@ namespace armarx::armem::server::robot_state
const proprioception::Segment& proprioceptionSegment;
const localization::Segment& localizationSegment;
+ std::map<std::string, RobotNameHelperPtr> robotNameHelper;
+ std::map<std::string, VirtualRobot::RobotPtr> robotModels;
+
+
struct Properties
{
bool enabled = true;
bool framesEnabled = false;
+ bool visualizeForceTorqueSensor = false;
float frequencyHz = 25;
} p;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
index abe3f0bbb..997253f25 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
@@ -2,6 +2,7 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -15,7 +16,7 @@ namespace armarx::armem::server
robot_state::combine(
const description::RobotDescriptionMap& robotDescriptions,
const localization::RobotPoseMap& globalPoses,
- const proprioception::RobotJointPositionMap& jointPositions,
+ const proprioception::SensorValuesMap& sensorValues,
const armem::Time& timestamp)
{
std::stringstream logs;
@@ -42,9 +43,12 @@ namespace armarx::armem::server
{
logs << "\nNo global pose for robot '" << robotName << "'.";
}
- if (auto it = jointPositions.find(robotName); it != jointPositions.end())
+ if (auto it = sensorValues.find(robotName); it != sensorValues.end())
{
- robot.config.jointMap = it->second;
+ for (const auto& [name, values] : it->second.jointValueMap)
+ {
+ robot.config.jointMap.emplace(name, values.position);
+ }
}
else
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
index 811599b69..b96ca240c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
@@ -32,7 +32,7 @@ namespace armarx::armem::server::robot_state
combine(
const description::RobotDescriptionMap& robotDescriptions,
const localization::RobotPoseMap& globalPoses,
- const proprioception::RobotJointPositionMap& jointPositions,
+ const proprioception::SensorValuesMap& sensorValues,
const armem::Time& timestamp);
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
index e730e29f1..1b9c39543 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
@@ -74,7 +74,12 @@ namespace armarx::armem::server::robot_state::localization
namespace armarx::armem::server::robot_state::proprioception
{
- using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
+ struct JointValues;
+ struct ForceTorqueValues;
+ struct SensorValues;
+ using JointValuesMap = std::unordered_map<std::string, JointValues>;
+ using ForceTorqueValuesMap = std::unordered_map<std::string, ForceTorqueValues>;
+ using SensorValuesMap = std::unordered_map<std::string, SensorValues>;
class Segment;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 99c330e76..d7136b1ef 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -15,6 +15,9 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include "SensorValues.h"
+
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
namespace armarx::armem::server::robot_state::proprioception
{
@@ -70,13 +73,13 @@ namespace armarx::armem::server::robot_state::proprioception
- RobotJointPositionMap Segment::getRobotJointPositionsLocking(
+ SensorValuesMap Segment::getSensorValuesLocking(
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
return doLocked([this, ×tamp, &debugObserver]()
{
- return getRobotJointPositions(timestamp, debugObserver);
+ return getSensorValues(timestamp, debugObserver);
});
}
@@ -93,18 +96,18 @@ namespace armarx::armem::server::robot_state::proprioception
}
- RobotJointPositionMap
- Segment::getRobotJointPositions(
+ SensorValuesMap
+ Segment::getSensorValues(
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
namespace adn = aron::data;
ARMARX_CHECK_NOT_NULL(segmentPtr);
- RobotJointPositionMap jointMap;
+ SensorValuesMap sensorValues;
int i = 0;
- Duration tFindData = Duration::MilliSeconds(0), tReadJointPositions = Duration::MilliSeconds(0);
+ Duration tFindData = Duration::MilliSeconds(0), tReadSensorValues = Duration::MilliSeconds(0);
TIMING_START(tProcessEntities)
segmentPtr->forEachEntity([&](const wm::Entity & entity)
{
@@ -128,12 +131,12 @@ namespace armarx::armem::server::robot_state::proprioception
}
if (data)
{
- TIMING_START(_tReadJointPositions)
+ TIMING_START(_tReadSensorValues)
- jointMap.emplace(entity.id().providerSegmentName, readJointPositions(*data));
+ sensorValues.emplace(entity.id().providerSegmentName, readSensorValues(*data));
- TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
- tReadJointPositions += Duration::MicroSeconds(_tReadJointPositions.toMicroSeconds());
+ TIMING_END_COMMENT_STREAM(_tReadSensorValues, "tReadSensorValues " + std::to_string(i), ARMARX_DEBUG)
+ tReadSensorValues += Duration::MicroSeconds(_tReadSensorValues.toMicroSeconds());
}
++i;
});
@@ -143,10 +146,10 @@ namespace armarx::armem::server::robot_state::proprioception
{
debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)", tFindData.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadJointPositions (ms)", tReadJointPositions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadSensorValues (ms)", tReadSensorValues.toMilliSecondsDouble());
}
- return jointMap;
+ return sensorValues;
}
@@ -277,25 +280,73 @@ namespace armarx::armem::server::robot_state::proprioception
}
- std::map<std::string, float>
- Segment::readJointPositions(const wm::EntityInstanceData& data)
+ SensorValues
+ Segment::readSensorValues(const wm::EntityInstanceData& data)
{
namespace adn = aron::data;
// Just get what we need without casting the whole data.
- std::map<std::string, float> jointPositions;
+ SensorValues sensorValues;
+ auto checkJVM = [&sensorValues](const std::string& name)
+ {
+ if (sensorValues.jointValueMap.find(name) == sensorValues.jointValueMap.end())
+ {
+ sensorValues.jointValueMap[name] = JointValues();
+ }
+ };
if (adn::DictPtr joints = getDictElement(data, "joints"))
{
- if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
+ if (adn::DictPtr values = getDictElement(*joints, "position"))
+ {
+ for (const auto& [name, value] : values->getElements())
+ {
+ checkJVM(name);
+ sensorValues.jointValueMap[name].position
+ = adn::Float::DynamicCastAndCheck(value)->getValue();
+ }
+ }
+ if (adn::DictPtr values = getDictElement(*joints, "velocity"))
+ {
+ for (const auto& [name, value] : values->getElements())
+ {
+ checkJVM(name);
+ sensorValues.jointValueMap[name].velocity
+ = adn::Float::DynamicCastAndCheck(value)->getValue();
+ }
+ }
+ if (adn::DictPtr values = getDictElement(*joints, "torque"))
+ {
+ for (const auto& [name, value] : values->getElements())
+ {
+ checkJVM(name);
+ sensorValues.jointValueMap[name].velocity
+ = adn::Float::DynamicCastAndCheck(value)->getValue();
+ }
+ }
+ }
+ if (adn::DictPtr forceTorqueMap = getDictElement(data, "forceTorque"))
+ {
+ for (const auto& [name, value] : forceTorqueMap->getElements())
{
- for (const auto& [name, value] : jointsPosition->getElements())
+ if (adn::DictPtr forceTorqueValues = aron::data::Dict::DynamicCastAndCheck(value))
{
- const float jointPosition = adn::Float::DynamicCastAndCheck(value)->getValue();
- jointPositions[name] = jointPosition;
+ const Eigen::Vector3f torque
+ = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
+ adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("torque")));
+
+ const Eigen::Vector3f force
+ = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
+ adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("force")));
+
+ sensorValues.forceTorqueValuesMap[name] = ForceTorqueValues
+ {
+ .force = force,
+ .torque = torque
+ };
}
}
}
- return jointPositions;
+ return sensorValues;
}
} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 8123aea55..fbce59a99 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -60,9 +60,9 @@ namespace armarx::armem::server::robot_state::proprioception
void onConnect(RobotUnitInterfacePrx robotUnitPrx);
- RobotJointPositionMap getRobotJointPositions(
+ SensorValuesMap getSensorValues(
const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
- RobotJointPositionMap getRobotJointPositionsLocking(
+ SensorValuesMap getSensorValuesLocking(
const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
const armem::MemoryID& getRobotUnitProviderID() const;
@@ -73,8 +73,8 @@ namespace armarx::armem::server::robot_state::proprioception
private:
static
- std::map<std::string, float>
- readJointPositions(const wm::EntityInstanceData& data);
+ SensorValues
+ readSensorValues(const wm::EntityInstanceData& data);
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
new file mode 100644
index 000000000..ee25d654e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
@@ -0,0 +1,2 @@
+#include "SensorValues.h"
+
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
new file mode 100644
index 000000000..2beb7608a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
@@ -0,0 +1,50 @@
+/*
+ * 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 Andre Meixner ( andre dot meixner at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+struct JointValues
+{
+ double position = 0.0;
+ double velocity = 0.0;
+ double torque = 0.0;
+ //double torqueTicks = 0.0;
+};
+
+struct ForceTorqueValues
+{
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f torque = Eigen::Vector3f::Zero();
+};
+
+struct SensorValues
+{
+ JointValuesMap jointValueMap;
+ ForceTorqueValuesMap forceTorqueValuesMap;
+};
+
+}
+
--
GitLab