Fix FT check and always convert force to root frame

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp

@@ -43,8 +43,7 @@ namespace armarx
     NJointCartesianWaypointController::NJointCartesianWaypointController(
         RobotUnitPtr,
         const NJointCartesianWaypointControllerConfigPtr& config,
-        const VirtualRobot::RobotPtr&
-    )
+        const VirtualRobot::RobotPtr&)
     {
         ARMARX_CHECK_NOT_NULL(config);
 
@@ -60,12 +59,7 @@ namespace armarx
             ARMARX_CHECK_NOT_NULL(_rtTcp)
                     << "No tcp in rns " << config->rns;
 
-            if (!config->ftName.empty())
-            {
-                _rtFT = _rtRobot->getSensor(config->ftName);
-                ARMARX_CHECK_NOT_NULL(_rtFT)
-                        << "No sensor node in robot of name " << config->ftName;
-            }
+            _rtRobotRoot = _rtRobot->getRootNode();
         }
 
         //sensor
@@ -77,6 +71,18 @@ namespace armarx
                         << "No sensor value of name " << config->ftName;
                 _rtForceSensor = &(svalFT->force);
                 _rtTorqueSensor = &(svalFT->force);
+
+                const auto sdev = peekSensorDevice(config->ftName);
+                ARMARX_CHECK_NOT_NULL(sdev);
+
+                const auto reportFrameName = sdev->getReportingFrame();
+                ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
+                        << VAROUT(sdev->getReportingFrame());
+
+                _rtFT = _rtRobot->getRobotNode(reportFrameName);
+                ARMARX_CHECK_NOT_NULL(_rtFT)
+                        << "No sensor report frame '" << reportFrameName
+                        << "' in robot";
             }
         }
         //ctrl
@@ -174,16 +180,19 @@ namespace armarx
 
         if (_rtForceSensor)
         {
-            rt2nonrtBuf.ft.force = *_rtForceSensor;
+            const Eigen::Vector3f ftInRoot =
+                _rtFT->getTransformationTo(_rtRobotRoot)
+                .topLeftCorner<3, 3>()
+                .transpose() *
+                (*_rtForceSensor);
+            rt2nonrtBuf.ft.force = ftInRoot;
             rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
             rt2nonrtBuf.ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
-            rt2nonrtBuf.ftInRoot = _rtFT->getRobotNode()->getPoseInRootFrame() *
-                                   _rtFT->getRobotNodeToSensorTransformation();
+            rt2nonrtBuf.ftInRoot = _rtFT->getPoseInRootFrame();
 
-            Eigen::Vector3f ftVal = *_rtForceSensor;
+            Eigen::Vector3f ftVal = ftInRoot;
             if (_rtForceThresholdInRobotRootZ)
             {
-                ftVal = rt2nonrtBuf.ftInRoot.topLeftCorner<3, 3>() * (*_rtForceSensor);
                 ftVal(0) = 0;
                 ftVal(1) = 0;
             }
@@ -435,25 +444,15 @@ namespace armarx
             if (_rtForceSensor)
             {
                 const Eigen::Matrix4f gft = gp * buf.ftInRoot;
-                {
-                    const Eigen::Vector3f fInRoot = gft.topLeftCorner<3, 3>() * buf.ft.force;
-                    drawer->setArrowVisu(
-                        getName(), "force",
-                        new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}},
-                    new Vector3{fInRoot},
-                    {1, 1, 0, 1}, fInRoot.norm() * 10, 2.5);
-                }
-                {
-                    const Eigen::Vector3f fInRoot =
-                        _publishForceThresholdInRobotRootZ ?
-                        buf.ftUsed :
-                        gft.topLeftCorner<3, 3>() * buf.ftUsed;
-                    drawer->setArrowVisu(
-                        getName(), "forceUsed",
-                        new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}},
-                    new Vector3{fInRoot},
-                    {1, 0.5, 0, 1}, fInRoot.norm() * 10, 2.5);
-                }
+                const Vector3Ptr pos = new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}};
+                drawer->setArrowVisu(
+                    getName(), "force", pos,
+                new Vector3{buf.ft.force.normalized()},
+                {1, 1, 0, 1}, buf.ft.force.norm(), 2.5);
+                drawer->setArrowVisu(
+                    getName(), "forceUsed", pos,
+                new Vector3{buf.ftUsed.normalized()},
+                {1, 0.5, 0, 1}, buf.ftUsed.norm(), 2.5);
             }
         }
     }

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h

@@ -99,7 +99,8 @@ namespace armarx
         VirtualRobot::RobotPtr          _rtRobot;
         VirtualRobot::RobotNodeSetPtr   _rtRns;
         VirtualRobot::RobotNodePtr      _rtTcp;
-        VirtualRobot::SensorPtr         _rtFT;
+        VirtualRobot::RobotNodePtr      _rtFT;
+        VirtualRobot::RobotNodePtr      _rtRobotRoot;
 
         std::unique_ptr<Ctrl>           _rtWpController;
         Eigen::VectorXf                 _rtJointVelocityFeedbackBuffer;