diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index dc91c6a504edfc82e05c44ed64af3367e00ade53..96c5d52571fe27f17d69b24e4206541f929ad7d5 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -41,6 +41,9 @@ set(LIB_FILES
     math/TimeSeriesUtils.cpp
     CartesianVelocityController.cpp
     CartesianPositionController.cpp
+    CartesianVelocityRamp.cpp
+    JointVelocityRamp.cpp
+    CartesianVelocityControllerWithRamp.cpp
 )
 
 set(LIB_HEADERS
@@ -77,6 +80,9 @@ set(LIB_HEADERS
     math/TimeSeriesUtils.h
     CartesianVelocityController.h
     CartesianPositionController.h
+    CartesianVelocityRamp.h
+    JointVelocityRamp.h
+    CartesianVelocityControllerWithRamp.h
 )
 
 
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index e150487d6ae0e520c59b3613a3644cc84012e9c4..d873e26c9078764965ba7f49f3f95de7ee63bcb9 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -60,11 +60,11 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
     //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
     //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
-    Eigen::MatrixXf kernel = lu_decomp.kernel();
-    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(kernel.rows());
-    for (int i = 0; i < kernel.cols(); i++)
+    Eigen::MatrixXf nullspace = lu_decomp.kernel();
+    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+    for (int i = 0; i < nullspace.cols(); i++)
     {
-        nsv += kernel.col(i) * kernel.col(i).dot(nullspaceVel) / kernel.col(i).squaredNorm();
+        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
     }
 
     //nsv /= kernel.cols();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index a87ac4acc1b1c5e5b296b1fc4b4922caf116c01a..fa1fb482f6c8b6c1e4b6169f1b721ae6def9ab4f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -51,10 +51,10 @@ namespace armarx
         Eigen::MatrixXf jacobi;
         Eigen::MatrixXf inv;
         VirtualRobot::RobotNodeSetPtr rns;
+        VirtualRobot::DifferentialIKPtr ik;
 
     private:
         Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
-        VirtualRobot::DifferentialIKPtr ik;
         VirtualRobot::RobotNodePtr tcp;
 
         float cartesianMMRegularization;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cbeb7e53a742703565114b2ecbbae6162845d78d
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -0,0 +1,49 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Sonja Marahrens (sonja dot marahrens)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "CartesianVelocityControllerWithRamp.h"
+
+using namespace armarx;
+
+CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
+        float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const VirtualRobot::RobotNodePtr& tcp)
+    : controller(rns, tcp), mode(mode)
+{
+    Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(tcp, mode);
+
+    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+    Eigen::MatrixXf nullspace = lu_decomp.kernel();
+    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+    for (int i = 0; i < nullspace.cols(); i++)
+    {
+        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
+    }
+
+    cartesianVelocity.reset(new CartesianVelocityRamp(jacobi * currentJointVelocity, maxPositionAcceleration, maxOrientationAcceleration, mode));
+    nullSpaceVelocity.reset(new JointVelocityRamp(nsv, maxNullspaceAcceleration));
+}
+
+Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
+{
+    return controller.calculate(cartesianVelocity->update(cartesianVel, dt), nullSpaceVelocity->update(nullspaceVel, dt), mode);
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
new file mode 100644
index 0000000000000000000000000000000000000000..55f9ad770278cfdab2831177511a2ee4dd75e614
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -0,0 +1,60 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Sonja Marahrens (sonja dot marahrens)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "CartesianVelocityController.h"
+#include "CartesianVelocityRamp.h"
+#include "JointVelocityRamp.h"
+
+#include <boost/shared_ptr.hpp>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <Eigen/Dense>
+
+
+namespace armarx
+{
+    class CartesianVelocityControllerWithRamp;
+    typedef boost::shared_ptr<CartesianVelocityControllerWithRamp> CartesianVelocityControllerWithRampPtr;
+
+    class CartesianVelocityControllerWithRamp
+    {
+    public:
+        CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
+                                            float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration,
+                                            const VirtualRobot::RobotNodePtr& tcp = nullptr);
+
+        Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);
+
+    private:
+        CartesianVelocityController controller;
+        VirtualRobot::IKSolver::CartesianSelection mode;
+        CartesianVelocityRampPtr cartesianVelocity;
+        JointVelocityRampPtr nullSpaceVelocity;
+
+    };
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7fe3d7a19dd405bccb264da83bd4b722925134f
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Sonja Marahrens (sonja dot marahrens)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "CartesianVelocityRamp.h"
+
+using namespace armarx;
+
+CartesianVelocityRamp::CartesianVelocityRamp(const Eigen::VectorXf& state, float maxPositiopnAcceleration, float maxOrientationAcceleration, VirtualRobot::IKSolver::CartesianSelection mode)
+    : state(state), maxPositionAcceleration(maxPositiopnAcceleration), maxOrientationAcceleration(maxOrientationAcceleration), mode(mode)
+{
+}
+
+Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, float dt)
+{
+    Eigen::VectorXf delta = target - state;
+    int i = 0;
+    float factor = 1;
+
+    if (mode & VirtualRobot::IKSolver::Position)
+    {
+        Eigen::Vector3f posDelta = delta.block<3, 1>(i, 0);
+        float posFactor = posDelta.norm() / maxPositionAcceleration;
+        factor = std::max(factor, posFactor);
+        i += 3;
+    }
+
+    if (mode & VirtualRobot::IKSolver::Orientation)
+    {
+        Eigen::Vector3f oriDelta = delta.block<3, 1>(i, 0);
+        float oriFactor = oriDelta.norm() / maxOrientationAcceleration;
+        factor = std::max(factor, oriFactor);
+    }
+
+    state += delta / factor * dt;
+    return state;
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
new file mode 100644
index 0000000000000000000000000000000000000000..006b11e21f60a3619fdf542779776492a912cfc9
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Sonja Marahrens (sonja dot marahrens)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class CartesianVelocityRamp;
+    typedef boost::shared_ptr<CartesianVelocityRamp> CartesianVelocityRampPtr;
+
+    class CartesianVelocityRamp
+    {
+    public:
+        CartesianVelocityRamp(const Eigen::VectorXf& state, float maxPositionAcceleration, float maxOrientationAcceleration, VirtualRobot::IKSolver::CartesianSelection mode);
+
+        Eigen::VectorXf update(const Eigen::VectorXf& target, float dt);
+
+    private:
+        float maxPositionAcceleration;
+        float maxOrientationAcceleration;
+
+        Eigen::VectorXf state;
+        VirtualRobot::IKSolver::CartesianSelection mode;
+
+
+    };
+}
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94382e88be5af9492ec91cf5665a9330e266ee21
--- /dev/null
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Sonja Marahrens (sonja dot marahrens)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "JointVelocityRamp.h"
+
+using namespace armarx;
+
+
+JointVelocityRamp::JointVelocityRamp(const Eigen::VectorXf& state, float maxAcceleration)
+    : state(state), maxAcceleration(maxAcceleration)
+{
+
+}
+
+Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
+{
+
+    Eigen::VectorXf delta = target - state;
+
+    float factor = delta.norm() / maxAcceleration;
+    factor = std::max(factor, 1.f);
+
+    state += delta / factor * dt;
+    return state;
+
+}
+
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.h b/source/RobotAPI/libraries/core/JointVelocityRamp.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ad10d51e054aa20cc131010425e1f524bcdd2e4
--- /dev/null
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Sonja Marahrens (sonja dot marahrens)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class JointVelocityRamp;
+    typedef boost::shared_ptr<JointVelocityRamp> JointVelocityRampPtr;
+
+    class JointVelocityRamp
+    {
+    public:
+        JointVelocityRamp(const Eigen::VectorXf& state, float maxAcceleration);
+
+        Eigen::VectorXf update(const Eigen::VectorXf& target, float dt);
+
+    private:
+
+        Eigen::VectorXf state;
+        float maxAcceleration;
+
+    };
+}
diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
index 38dff717efe933335499c980fc31a8efd8efd9cc..de4e6dee8c5c1e738dd78d555f61e2a9b0abcc34 100644
--- a/source/RobotAPI/libraries/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -5,3 +5,5 @@ armarx_add_test(PIDControllerTest PIDControllerTest.cpp "${LIBS}")
 
 armarx_add_test(MathUtilsTest MathUtilsTest.cpp "${LIBS}")
 armarx_add_test(CartesianVelocityControllerTest CartesianVelocityControllerTest.cpp "${LIBS}")
+
+armarx_add_test(CartesianVelocityRampTest CartesianVelocityRampTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b334b6378b823062b8614ddf21a572da7eded17
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
@@ -0,0 +1,54 @@
+/*
+* 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    ArmarX::RobotAPI::Test
+* @author     Nils Adermann (naderman at naderman dot de)
+* @date       2010
+* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+*             GNU General Public License
+*/
+
+#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityRamp::Test
+#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include "../CartesianVelocityRamp.h"
+
+using namespace armarx;
+
+
+
+BOOST_AUTO_TEST_CASE(testBoth)
+{
+    Eigen::VectorXf state(6);
+    state << 0, 0, 0, 0, 0, 0;
+    float dt = 0.1f;
+
+    CartesianVelocityRampPtr c(new CartesianVelocityRamp(state, 100, 1, VirtualRobot::IKSolver::CartesianSelection::All));
+
+    Eigen::VectorXf target(6);
+    target << 100, 0, 0, 0, 0, 0;
+
+    state = c->update(target, dt);
+    Eigen::VectorXf expected(6);
+    expected << 10, 0, 0, 0, 0, 0;
+
+
+    BOOST_CHECK_LE((state - expected).norm(), 0.01f);
+
+}