diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index fdf5ecb3e856b0758b5245197d0d79b81f940d4b..c5ca4ead4deb70d4d507186a0a16d43dfb81ac51 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -119,7 +119,7 @@ namespace armarx
             svi.addBaseClass<SensorValue1DoFActuatorVelocity>();
             svi.addBaseClass<SensorValue1DoFActuatorAcceleration>();
             svi.addBaseClass<SensorValue1DoFActuatorTorque>();
-            //svi.addBaseClass<SensorValue1DoFGravityTorque>();
+            svi.addBaseClass<SensorValue1DoFGravityTorque>();
             return svi;
         }
     };
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 6cb44a0a8fe002de49ca26a26488972c3aad5299..0408ca66ffcaeb1d906183d1ecca85a1a22bf31d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -54,7 +54,7 @@ void PositionControllerHelper::update()
         currentWaypointIndex++;
     }
     Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
-    velocityControllerHelper->setTargetVelocity(cv);
+    velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
 }
 
 void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
@@ -85,6 +85,22 @@ void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
     currentWaypointIndex = 0;
 }
 
+void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+{
+    this->feedForwardVelocity = feedForwardVelocity;
+}
+
+void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+{
+    feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
+    feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
+}
+
+void PositionControllerHelper::clearFeedForwardVelocity()
+{
+    feedForwardVelocity = Eigen::Vector6f::Zero();
+}
+
 float PositionControllerHelper::getPositionError() const
 {
     return posController.getPositionError(getCurrentTarget());
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 24df9457501f4cb56cb42c7975471cae47fed5e6..04c92fc6db1d2041ef05af5f6ce15bb89ef609b3 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -34,8 +34,15 @@
 #include <RobotAPI/libraries/core/CartesianPositionController.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+namespace Eigen
+{
+    typedef Matrix<float, 6, 1> Vector6f;
+}
+
+
 namespace armarx
 {
+
     class PositionControllerHelper;
     typedef boost::shared_ptr<PositionControllerHelper> PositionControllerHelperPtr;
 
@@ -52,6 +59,9 @@ namespace armarx
         void setNewWaypoints(const std::vector<PosePtr>& waypoints);
         void addWaypoint(const Eigen::Matrix4f& waypoint);
         void setTarget(const Eigen::Matrix4f& target);
+        void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
+        void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+        void clearFeedForwardVelocity();
 
         float getPositionError() const;
 
@@ -82,5 +92,6 @@ namespace armarx
         float thresholdOrientationReached = 0;
         float thresholdPositionNear = 0;
         float thresholdOrientationNear = 0;
+        Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
     };
 }