Merge branch '26-add-stop-on-force-threshold-behavior-to-taskspacedmpcontroller' into 'main'

Resolve "Add stop-on-force-threshold behavior to TaskSpaceDMPController"

Closes #26

See merge request !45

source/armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.ice

@@ -337,6 +337,13 @@ module armarx
 
         float pos_filter;
         float vel_filter;
+
+        string forceSensorName;
+        float waitTimeForCalibration;
+        float forceFilter;
+        float forceDeadZone;
+        Eigen::Vector3f forceThreshold;
+        string forceFrameName = "ArmR8_Wri2";
     };
 
 
@@ -357,6 +364,7 @@ module armarx
         void stopDMP();
         void pauseDMP();
         void resumeDMP();
+        bool isDMPRunning();
 
         void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
         void setTorqueKp(StringFloatDictionary torqueKp);
@@ -372,6 +380,9 @@ module armarx
         void setAngularVelocityKd(float kd);
         void setAngularVelocityKp(float kp);
 
+        void enableForceStop();
+        void disableForceStop();
+        void setForceThreshold(Eigen::Vector3f forceThreshold);
     };
 
     class DeprecatedNJointTaskSpaceImpedanceDMPControllerConfig extends NJointControllerConfig

source/armarx/control/deprecated_njoint_mp_controller/task_space/DeprecatedNJointTSDMPController.h

@@ -4,6 +4,7 @@
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h"
 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
@@ -96,6 +97,10 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
 
         void resetDMP(const Ice::Current&) override;
         void stopDMP(const Ice::Current&) override;
+        bool isDMPRunning(const Ice::Current&) override
+        {
+            return started;
+        }
 
         double getCanVal(const Ice::Current&) override
         {
@@ -116,6 +121,20 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
         void setAngularVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
         void setAngularVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) override;
 
+        void enableForceStop(const Ice::Current&) override
+        {
+            useForceStop = true;
+        }
+        void disableForceStop(const Ice::Current&) override
+        {
+            useForceStop = false;
+        }
+
+        void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
+        {
+            forceThreshold.getWriteBuffer() = f;
+            forceThreshold.commitWrite();
+        }
     protected:
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
@@ -214,7 +233,15 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
         Eigen::VectorXf jointHighLimits;
         Eigen::VectorXf jointLowLimits;
 
+        Eigen::Vector3f filteredForce;
+        Eigen::Vector3f forceOffset;
+        Eigen::Vector3f filteredForceInRoot;
+        WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
+        std::atomic<bool> useForceStop;
+        const SensorValueForceTorque* forceSensor;
 
+        std::atomic<float> timeForCalibration;
+        std::atomic_bool stopped = false;
     };
 
 } // namespace armarx