From 341ac45b151ff3c8d0e7e52796efe1fea3855600 Mon Sep 17 00:00:00 2001
From: ARMAR-7a User <armar7a@h2t>
Date: Fri, 3 Nov 2023 18:06:07 +0100
Subject: [PATCH] Improved visualization of the force torque sensor
---
.../armem_robot_state/server/common/Visu.cpp | 28 ++++++++++++-------
.../armem_robot_state/server/common/Visu.h | 11 ++++++--
2 files changed, 27 insertions(+), 12 deletions(-)
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 e0e1721cd..be19db635 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -8,6 +8,7 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
+#include <SimoxUtility/math/rescale.h>
#include <VirtualRobot/XML/RobotIO.h>
@@ -46,10 +47,12 @@ namespace armarx::armem::server::robot_state
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
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",
+ defs->optional(p.framesEnabled, prefix + "framesEnabled", "Enable or disable visualization of frames.");
+ defs->optional(p.forceTorque.enabled, prefix + "forceTorque.enabled",
"Enable or disable visualization of force torque sensors.");
+ defs->optional(p.forceTorque.forceScale, prefix + "forceTorque.forceScale",
+ "Scaling of force arrows.");
}
@@ -242,7 +245,7 @@ namespace armarx::armem::server::robot_state
layers.push_back(layerFrames);
}
- if (p.visualizeForceTorqueSensor)
+ if (p.forceTorque.enabled)
{
viz::Layer layerFrames = arviz.layer("ForceTorque");
for (const robot::Robot& robot : robots)
@@ -263,9 +266,6 @@ namespace armarx::armem::server::robot_state
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;
@@ -284,11 +284,19 @@ namespace armarx::armem::server::robot_state
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);
+ // Force arrows.
+ const float length = p.forceTorque.forceScale * ft.force(i);
+ const float width = std::min(
+ simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F),
+ 10.F);
+
+ const Eigen::Vector3f to = from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
+
+ std::stringstream key;
+ key << side << " Force " << xyz.at(i);
+ layerFrames.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
- layerFrames.add(viz::Arrow(side + " Force " + std::to_string(xyz.at(i)))
- .fromTo(from, to).color(color).width(100));
+ // Torque circle arrows.
}
}
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 39f0af9eb..265f08e2a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -95,9 +95,16 @@ namespace armarx::armem::server::robot_state
struct Properties
{
bool enabled = true;
+ float frequencyHz = 25.f;
+
bool framesEnabled = false;
- bool visualizeForceTorqueSensor = false;
- float frequencyHz = 25;
+
+ struct ForceTorque
+ {
+ bool enabled = true;
+ float forceScale = 1.F;
+ };
+ ForceTorque forceTorque;
} p;
--
GitLab