Improved visualization of the force torque sensor

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.
                     }
                 }
 

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;