From 38a3eb2b5283ab7a612df913e5fb9350c313835a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 1 Jun 2021 08:19:32 +0200
Subject: [PATCH] ForceTorque Helper: added method getFramedForce
---
.../RobotStatechartHelpers/ForceTorqueHelper.cpp | 7 +++++++
.../libraries/RobotStatechartHelpers/ForceTorqueHelper.h | 9 ++++++++-
2 files changed, 15 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
index b6cf85960..de446c057 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
index c7e635b64..57f437a4c 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
+++ b/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;
--
GitLab