diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
index 2f1b3c3669b8365dbd0c7cd7bdf52479bf4b6cf7..49acfeefb9fec34ea497c6209d8a0b1c3c846c9b 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
@@ -77,6 +77,8 @@ module armarx
         bool isFinished();
         void runDMP(Ice::DoubleSeq leftGoals, Ice::DoubleSeq rightGoals);
         double getCanVal();
+
+        void setViaPoints(string whichDMP, double canVal, Ice::DoubleSeq viaPoint);
     };
 };
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
index 1aa129cb5870f45316ac5e70d4c5bdda0ecf3eb4..eeb9ebe8f80df2f0775ad22d8b1156e913dc9d3e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
@@ -1,4 +1,6 @@
 #include "NJointBimanualForceMPController.h"
+#include <random>
+
 
 namespace armarx
 {
@@ -158,18 +160,22 @@ namespace armarx
         Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
         Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
 
-        float forceOnHands = (leftForce + rightForce)(2);
+        float forceOnHands = (leftForce + rightForce)(2) ;
+
+        xvel = cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
 
-        xvel = cfg->forceP * (targetSupportForce - forceOnHands) + I_decay * cfg->forceI * forceIterm;
         forceIterm += targetSupportForce - forceOnHands;
 
         canVal = canVal + forceSign * xvel * deltaT;
+        //        canVal = cfg->timeDuration + forceSign * xvel;
+
 
         if (canVal > cfg->timeDuration)
         {
             canVal = cfg->timeDuration;
         }
 
+
         leftDMPController->canVal = canVal;
         rightDMPController->canVal = canVal;
 
@@ -254,7 +260,6 @@ namespace armarx
         }
         Eigen::Vector6f rightTwist = rightJacobi * qvel;
 
-
         leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) + cfg->filterCoeff * leftFilteredValue;
         rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) + cfg->filterCoeff * rightFilteredValue;
 
@@ -432,6 +437,18 @@ namespace armarx
 
     }
 
+    void NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP, double u, const Ice::DoubleSeq& viaPoint, const Ice::Current&)
+    {
+        if (whichDMP == "Left")
+        {
+            leftDMPController->setViaPose(u, viaPoint);
+        }
+        if (whichDMP == "Right")
+        {
+            rightDMPController->setViaPose(u, viaPoint);
+        }
+    }
+
     void NJointBimanualForceMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
     {
         std::string debugName = cfg->debugName;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
index f276fc1f371e42fae6c8b8ba7c2b6b48bc5e06e8..c59c12e2f0993ff22cd250e9f6a4ffa4294eabc5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
@@ -63,6 +63,8 @@ namespace armarx
             return canVal;
         }
 
+        void setViaPoints(const string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&);
+
     protected:
         virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);