ForceTorque Helper: added method getFramedForce

source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp

@@ -40,6 +40,13 @@ namespace armarx
         return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
     }
 
+    FramedDirection ForceTorqueHelper::getFramedForce()
+    {
+        const auto force = forceDf->getDataField()->get<FramedDirection>();
+
+        return FramedDirection(force->toEigen() - initialForce, force->getFrame(), force->agent);
+    }
+
     Eigen::Vector3f ForceTorqueHelper::getTorque()
     {
         return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;

source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h

@@ -24,9 +24,13 @@
 #pragma once
 
 #include <memory>
-#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+
 #include <Eigen/Dense>
 
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+
+
 namespace armarx
 {
     class DatafieldRef;
@@ -41,7 +45,10 @@ namespace armarx
         ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx&  forceTorqueObserver, const std::string& FTDatefieldName);
 
         Eigen::Vector3f getForce();
+        FramedDirection getFramedForce();
+
         Eigen::Vector3f getTorque();
+
         void setZero();
 
         DatafieldRefPtr forceDf;