Add ForceTorqueUtility StatecharGroup

It contains states to detect Force flanks and force spikes

source/RobotAPI/statecharts/CMakeLists.txt

@@ -6,4 +6,5 @@ add_subdirectory(StatechartProfilesTestGroup)
 add_subdirectory(OrientedTactileSensorGroup)
 add_subdirectory(TrajectoryExecutionCode)
 
-add_subdirectory(SpeechObserverTestGroup)
\ No newline at end of file
+add_subdirectory(SpeechObserverTestGroup)
+add_subdirectory(ForceTorqueUtility)
\ No newline at end of file

source/RobotAPI/statecharts/ForceTorqueUtility/CMakeLists.txt

+armarx_component_set_name("ForceTorqueUtility")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+#find_package(Eigen3 QUIET)
+#find_package(Simox QUIET)
+
+#
+#armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+#armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+#
+#if (Eigen3_FOUND AND Simox_FOUND)
+#    include_directories(
+#        ${Eigen3_INCLUDE_DIR}
+#        ${Simox_INCLUDE_DIRS}
+#    )
+#endif()
+
+set(COMPONENT_LIBS
+#   ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
+    ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
+
+# Sources
+
+set(SOURCES
+ForceTorqueUtilityRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+ForceTorqueUtilityRemoteStateOfferer.h
+ForceTorqueUtility.scgxml
+)
+
+# adds all existing state headers and sources to CMake
+armarx_generate_statechart_cmake_lists()
+
+armarx_add_component("${SOURCES}" "${HEADERS}")

source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ForceTorqueUtility
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <thread>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "DetectForceFlank.h"
+
+using namespace armarx;
+using namespace ForceTorqueUtility;
+
+// DO NOT EDIT NEXT LINE
+DetectForceFlank::SubClassRegistry DetectForceFlank::Registry(DetectForceFlank::GetName(), &DetectForceFlank::CreateInstance);
+
+void DetectForceFlank::run()
+{
+    ARMARX_CHECK_EXPRESSION(in.getTriggerOnDecreasingForceVectorLength() || in.getTriggerOnIncreasingForceVectorLength());
+
+    const float forceThreshold = in.getForceVectorLengthThreshold();
+    DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
+    const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
+    const float initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
+
+    while (!isRunningTaskStopped()) // stop run function if returning true
+    {
+
+        const float force = forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
+        ARMARX_INFO << deactivateSpam(1) << VAROUT(force) << " " << VAROUT(initialForce);
+        if (
+            (
+                in.getTriggerOnDecreasingForceVectorLength() &&
+                force < initialForce &&
+                initialForce - force >= forceThreshold
+            ) ||
+            (
+                in.getTriggerOnIncreasingForceVectorLength() &&
+                force > initialForce &&
+                force - initialForce >= forceThreshold
+            )
+        )
+        {
+            emitSuccess();
+            return;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds{10});
+    }
+    emitFailure();
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr DetectForceFlank::CreateInstance(XMLStateConstructorParams stateData)
+{
+    return XMLStateFactoryBasePtr(new DetectForceFlank(stateData));
+}
+

source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ForceTorqueUtility
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.generated.h>
+
+namespace armarx
+{
+    namespace ForceTorqueUtility
+    {
+        class DetectForceFlank :
+            public DetectForceFlankGeneratedBase < DetectForceFlank >
+        {
+        public:
+            DetectForceFlank(const XMLStateConstructorParams& stateData):
+                XMLStateTemplate < DetectForceFlank > (stateData), DetectForceFlankGeneratedBase < DetectForceFlank > (stateData)
+            {
+            }
+
+            // inherited from StateBase
+            void onEnter() override {}
+            void run() override;
+            void onExit() override {}
+
+            // static functions for AbstractFactory Method
+            static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+            static SubClassRegistry Registry;
+
+            // DO NOT INSERT ANY CLASS MEMBERS,
+            // use stateparameters instead,
+            // if classmember are neccessary nonetheless, reset them in onEnter
+        };
+    }
+}
+
+

source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.xml

+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="DetectForceFlank" uuid="177EFD66-2F0B-4544-A9AD-0B89A9C9E5E3" width="800" height="600" type="Normal State">
+	<InputParameters>
+		<Parameter name="FTDatafieldName" type="::armarx::StringVariantData" docType="string" optional="no"/>
+		<Parameter name="ForceVectorLengthThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":15}}' docValue="15"/>
+		</Parameter>
+		<Parameter name="ForceWeights" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1.0,"y":1.0,"z":1.0}}}' docValue="1\n1\n1"/>
+		</Parameter>
+		<Parameter name="TriggerOnDecreasingForceVectorLength" type="::armarx::BoolVariantData" docType="bool" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+		</Parameter>
+		<Parameter name="TriggerOnIncreasingForceVectorLength" type="::armarx::BoolVariantData" docType="bool" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+		</Parameter>
+	</InputParameters>
+	<OutputParameters/>
+	<LocalParameters/>
+	<Substates/>
+	<Events>
+		<Event name="Failure">
+			<Description>Event for statechart-internal failures or optionally user-code failures</Description>
+		</Event>
+		<Event name="Success"/>
+	</Events>
+	<Transitions/>
+</State>
+

source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ForceTorqueUtility
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <thread>
+
+#include "DetectForceSpike.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+using namespace armarx;
+using namespace ForceTorqueUtility;
+
+// DO NOT EDIT NEXT LINE
+DetectForceSpike::SubClassRegistry DetectForceSpike::Registry(DetectForceSpike::GetName(), &DetectForceSpike::CreateInstance);
+
+
+
+void DetectForceSpike::run()
+{
+    ARMARX_CHECK_EXPRESSION(in.getTriggerInAxisDirection() || in.getTriggerCounterAxisDirection());
+    ARMARX_CHECK_GREATER_EQUAL(in.getWindowSizeMs(), 10);
+    ARMARX_CHECK_GREATER_EQUAL(in.getWindowSizeMs(), 10);
+
+    const float forceThreshold = in.getForceThreshold();
+    ARMARX_CHECK_GREATER(forceThreshold, 0);
+
+    DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
+    const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
+    const Eigen::Vector3f axis = in.getAxis()->toEigen().normalized();
+    auto getForceAlongAxis = [&]
+    {
+        return forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).transpose() * axis;
+    };
+
+    in.getTriggerInAxisDirection();
+    in.getTriggerCounterAxisDirection();
+    std::deque<float> spikes(in.getWindowSizeMs()/10, getForceAlongAxis());
+
+    while (!isRunningTaskStopped()) // stop run function if returning true
+    {
+        spikes.push_back(getForceAlongAxis());
+        spikes.pop_front();
+
+        float refValue = spikes.at(0);
+        bool low = true;
+        bool risingEdgeDetected = false;
+        bool fallingEdgeDetected = false;
+
+        bool f2rDetected = false;
+        bool r2fDetected = false;
+
+        for(const float spike : spikes)
+        {
+            if(low)
+            {
+                if(spike < refValue)
+                {
+                    refValue = spike;
+                }
+                else if(spike > refValue + forceThreshold)
+                {
+                    low = false;
+                    risingEdgeDetected = true;
+                    f2rDetected |= fallingEdgeDetected;
+                }
+            }
+
+            if(!low)
+            {
+                if(spike > refValue)
+                {
+                    refValue = spike;
+                }
+                else if(spike < refValue - forceThreshold)
+                {
+                    low = true;
+                    fallingEdgeDetected = true;
+                    r2fDetected |= risingEdgeDetected;
+                }
+            }
+        }
+        if ((in.getTriggerInAxisDirection() && r2fDetected) || (in.getTriggerCounterAxisDirection() && f2rDetected))
+        {
+            emitSuccess();
+            return;
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds{10});
+    }
+    emitFailure();
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr DetectForceSpike::CreateInstance(XMLStateConstructorParams stateData)
+{
+    return XMLStateFactoryBasePtr(new DetectForceSpike(stateData));
+}
+

source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ForceTorqueUtility
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.generated.h>
+
+namespace armarx
+{
+    namespace ForceTorqueUtility
+    {
+        class DetectForceSpike :
+            public DetectForceSpikeGeneratedBase < DetectForceSpike >
+        {
+        public:
+            DetectForceSpike(const XMLStateConstructorParams& stateData):
+                XMLStateTemplate < DetectForceSpike > (stateData), DetectForceSpikeGeneratedBase < DetectForceSpike > (stateData)
+            {
+            }
+
+            // inherited from StateBase
+            void onEnter() override {}
+            void run() override;
+            void onExit() override {}
+
+            // static functions for AbstractFactory Method
+            static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+            static SubClassRegistry Registry;
+
+            // DO NOT INSERT ANY CLASS MEMBERS,
+            // use stateparameters instead,
+            // if classmember are neccessary nonetheless, reset them in onEnter
+        };
+    }
+}
+
+

source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.xml

+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="DetectForceSpike" uuid="DCB1C3B2-FBBB-49ED-B0D0-70C35011E5E3" width="800" height="600" type="Normal State">
+	<InputParameters>
+		<Parameter name="Axis" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":0.0,"y":0.0,"z":-1.0}}}' docValue="0\n0\n-1"/>
+		</Parameter>
+		<Parameter name="FTDatafieldName" type="::armarx::StringVariantData" docType="string" optional="no"/>
+		<Parameter name="ForceThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}' docValue="20"/>
+		</Parameter>
+		<Parameter name="ForceWeights" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1.0,"y":1.0,"z":1.0}}}' docValue="1\n1\n1"/>
+		</Parameter>
+		<Parameter name="MinDeltaThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}}' docValue="1"/>
+		</Parameter>
+		<Parameter name="TriggerCounterAxisDirection" type="::armarx::BoolVariantData" docType="bool" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+		</Parameter>
+		<Parameter name="TriggerInAxisDirection" type="::armarx::BoolVariantData" docType="bool" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+		</Parameter>
+		<Parameter name="WindowSizeMs" type="::armarx::IntVariantData" docType="int" optional="no">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::IntVariantData","value":300}}' docValue="300"/>
+		</Parameter>
+	</InputParameters>
+	<OutputParameters/>
+	<LocalParameters/>
+	<Substates/>
+	<Events>
+		<Event name="Failure">
+			<Description>Event for statechart-internal failures or optionally user-code failures</Description>
+		</Event>
+		<Event name="Success"/>
+	</Events>
+	<Transitions/>
+</State>
+

source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtility.scgxml

+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="ForceTorqueUtility" package="RobotAPI" generateContext="true">
+	<Proxies>
+		<Proxy value="RobotAPIInterfaces.forceTorqueObserver"/>
+	</Proxies>
+	<Configurations>
+		<Configuration profileName="Armar6Real">
+ArmarX.ForceTorqueUtilityRemoteStateOfferer.ForceTorqueUnitObserverName = Armar6ForceTorqueObserver
+</Configuration>
+	</Configurations>
+	<State filename="DetectForceFlank.xml" visibility="public"/>
+	<State filename="DetectForceSpike.xml" visibility="public"/>
+</StatechartGroup>
+

source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ForceTorqueUtility::ForceTorqueUtilityRemoteStateOfferer
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "ForceTorqueUtilityRemoteStateOfferer.h"
+
+using namespace armarx;
+using namespace ForceTorqueUtility;
+
+// DO NOT EDIT NEXT LINE
+ForceTorqueUtilityRemoteStateOfferer::SubClassRegistry ForceTorqueUtilityRemoteStateOfferer::Registry(ForceTorqueUtilityRemoteStateOfferer::GetName(), &ForceTorqueUtilityRemoteStateOfferer::CreateInstance);
+
+
+
+ForceTorqueUtilityRemoteStateOfferer::ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+    XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > (reader)
+{
+}
+
+void ForceTorqueUtilityRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+{
+
+}
+
+void ForceTorqueUtilityRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+{
+
+}
+
+void ForceTorqueUtilityRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+{
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string ForceTorqueUtilityRemoteStateOfferer::GetName()
+{
+    return "ForceTorqueUtilityRemoteStateOfferer";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateOffererFactoryBasePtr ForceTorqueUtilityRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+{
+    return XMLStateOffererFactoryBasePtr(new ForceTorqueUtilityRemoteStateOfferer(reader));
+}
+
+
+

source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ForceTorqueUtility
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "ForceTorqueUtilityStatechartContext.generated.h"
+
+namespace armarx
+{
+    namespace ForceTorqueUtility
+    {
+        class ForceTorqueUtilityRemoteStateOfferer :
+            virtual public XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+        {
+        public:
+            ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+            // inherited from RemoteStateOfferer
+            void onInitXMLRemoteStateOfferer() override;
+            void onConnectXMLRemoteStateOfferer() override;
+            void onExitXMLRemoteStateOfferer() override;
+
+            // static functions for AbstractFactory Method
+            static std::string GetName();
+            static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+            static SubClassRegistry Registry;
+
+
+        };
+    }
+}
+