modified njointtaskspacedmp

source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice

@@ -50,7 +50,8 @@ module armarx
 
         double phaseL = 10;
         double phaseK = 10;
-        double phaseDist0 = 0.3;
+        double phaseDist0 = 50;
+        double phaseDist1 = 10;
         double phaseKp = 1;
 
         double timeDuration = 10;

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h

@@ -46,13 +46,6 @@ namespace armarx
         // NJointController interface
         void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
 
-        //        static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
-        //        NJointCartesianVelocityController(
-        //            NJointControllerDescriptionProviderInterfacePtr prov,
-        //            NJointCartesianVelocityControllerConfigPtr config,
-        //            const boost::shared_ptr<VirtualRobot::Robot>& r);
-
-
         //
         bool isFinished(const Ice::Current&)
         {

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp

@@ -61,9 +61,12 @@ namespace armarx
         initData.isStart = false;
         reinitTripleBuffer(initData);
 
+
+        isDisturbance = false;
         phaseL = cfg->phaseL;
         phaseK = cfg->phaseK;
         phaseDist0 = cfg->phaseDist0;
+        phaseDist1 = cfg->phaseDist1;
         phaseKp = cfg->phaseKp;
 
         // initialize tcp position and orientation
@@ -138,9 +141,31 @@ namespace armarx
             }
 
             error = sqrt(error);
-            phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
+
+            double phaseDist;
+            if (isDisturbance)
+            {
+                phaseDist = phaseDist1;
+            }
+            else
+            {
+                phaseDist = phaseDist0;
+            }
+
+            phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
             double mpcFactor = 1 - (phaseStop / phaseL);
 
+            if (mpcFactor < 0.1)
+            {
+                isDisturbance = true;
+            }
+
+            if (mpcFactor > 0.9)
+            {
+                isDisturbance = false;
+            }
+
+
             double tau = dmpPtr->getTemporalFactor();
             canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
             currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
@@ -164,6 +189,7 @@ namespace armarx
                     vel0 = currentState[i].vel / timeDuration;
                     vel1 = phaseKp * (targetState[i] - tcpPosition[i]);
                     x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+
                 }
                 for (size_t i = 0; i < 3; i++)
                 {
@@ -171,6 +197,17 @@ namespace armarx
                     vel1 = phaseKp * (targetState[i + 3] - tcpOrientation[i]);
                     x(i + 3) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                 }
+
+                debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = x(0);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(1);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = x(2);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["r_vel"] = x(3);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["p_vel"] = x(4);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(5);
+                debugOutputData.getWriteBuffer().currentCanVal = canVal;
+                debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
+                debugOutputData.commitWrite();
+
             }
             else if (mode == VirtualRobot::IKSolver::Position)
             {
@@ -182,6 +219,13 @@ namespace armarx
                     vel1 = phaseKp * (targetState[i] - tcpPosition[i]);
                     x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                 }
+
+                debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = x(0);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(1);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = x(2);
+                debugOutputData.getWriteBuffer().currentCanVal = canVal;
+                debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
+                debugOutputData.commitWrite();
             }
             else if (mode == VirtualRobot::IKSolver::Orientation)
             {
@@ -193,6 +237,13 @@ namespace armarx
                     vel1 = phaseKp * (targetState[i + 3] - tcpOrientation[i]);
                     x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                 }
+
+                debugOutputData.getWriteBuffer().latestTargetVelocities["r_vel"] = x(3);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["p_vel"] = x(4);
+                debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(5);
+                debugOutputData.getWriteBuffer().currentCanVal = canVal;
+                debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
+                debugOutputData.commitWrite();
             }
             else
             {
@@ -303,6 +354,20 @@ namespace armarx
         return (VirtualRobot::IKSolver::CartesianSelection)0;
     }
 
+    void NJointTaskSpaceDMPController::onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &debugObs)
+    {
+        StringVariantBaseMap datafields;
+        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+        for (auto& pair : values)
+        {
+            datafields[pair.first] = new Variant(pair.second);
+        }
+
+        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+        debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
+    }
+
 
     void NJointTaskSpaceDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
     {

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h

@@ -85,8 +85,19 @@ namespace armarx
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
         VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
     private:
+        struct DebugBufferData
+        {
+            StringFloatDictionary latestTargetVelocities;
+            double currentCanVal;
+            double mpcFactor;
+        };
+
+        TripleBuffer<DebugBufferData> debugOutputData;
+
+
         std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
         std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
         std::vector<ControlTarget1DoFActuatorVelocity*> targets;
@@ -108,9 +119,11 @@ namespace armarx
         double phaseL;
         double phaseK;
         double phaseDist0;
+        double phaseDist1;
         double phaseKp;
 
 
+        bool isDisturbance;
         NJointTaskSpaceDMPControllerConfigPtr cfg;
         VirtualRobot::RobotNodePtr tcp;
         Eigen::Vector3f tcpPosition;