diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index b0a1a80cab68dc9f10e178cc00da1e1b65b49c94..12b46048fadfc49819e2bcf46d6bc8f18d916bb5 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -6,3 +6,5 @@ add_subdirectory(RobotAPIVariantWidget)
 add_subdirectory(RobotAPINJointControllers)
 
 add_subdirectory(DMPController)
+
+add_subdirectory(RobotStatechartHelpers)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a7d4a2e9e15e6ab016dc614dd2c04b381fb5bcd6
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
@@ -0,0 +1,41 @@
+set(LIB_NAME       RobotStatechartHelpers)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+    include_directories(
+        ${Eigen3_INCLUDE_DIR}
+        ${Simox_INCLUDE_DIRS}
+    )
+endif()
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
+    ArmarXCore ArmarXCoreObservers
+)
+
+set(LIB_FILES
+#./RobotStatechartHelpers.cpp
+VelocityControllerHelper.cpp
+PositionControllerHelper.cpp
+ForceTorqueHelper.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+#./RobotStatechartHelpers.h
+VelocityControllerHelper.h
+PositionControllerHelper.h
+ForceTorqueHelper.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..26156ee74a83b61aba5dd57bc6ac3dd38bd9128d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
@@ -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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "ForceTorqueHelper.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+using namespace armarx;
+
+ForceTorqueHelper::ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx &forceTorqueObserver, const std::string &FTDatefieldName)
+{
+    forceDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
+    torqueDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
+    setZero();
+}
+
+Eigen::Vector3f ForceTorqueHelper::getForce()
+{
+    return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
+}
+
+Eigen::Vector3f ForceTorqueHelper::getTorque()
+{
+    return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
+}
+
+void ForceTorqueHelper::setZero()
+{
+    initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
+    initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e3a2d9e065d0ab90c0f0c2f9dca1e2fd66b5f40
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
@@ -0,0 +1,53 @@
+/*
+ * 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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class DatafieldRef;
+    typedef IceInternal::Handle<DatafieldRef> DatafieldRefPtr;
+
+    class ForceTorqueHelper;
+    typedef boost::shared_ptr<ForceTorqueHelper> ForceTorqueHelperPtr;
+
+    class ForceTorqueHelper
+    {
+    public:
+        ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx&  forceTorqueObserver, const std::string& FTDatefieldName);
+
+        Eigen::Vector3f getForce();
+        Eigen::Vector3f getTorque();
+        void setZero();
+
+        DatafieldRefPtr forceDf;
+        DatafieldRefPtr torqueDf;
+        Eigen::Vector3f initialForce;
+        Eigen::Vector3f initialTorque;
+    };
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..229077ce661f1c3d49341d1ec296eb2e5b3d5134
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -0,0 +1,113 @@
+/*
+ * 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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "PositionControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr &tcp, const VelocityControllerHelperPtr &velocityControllerHelper, const Eigen::Matrix4f &target)
+    : posController(tcp), velocityControllerHelper(velocityControllerHelper)
+{
+    waypoints.push_back(target);
+    currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::update()
+{
+    if(!isLastWaypoint() && isTargetNear())
+    {
+        currentWaypointIndex++;
+    }
+    Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
+    velocityControllerHelper->setTargetVelocity(cv);
+}
+
+void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f> &waypoints)
+{
+    this->waypoints = waypoints;
+    currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr> &waypoints)
+{
+    this->waypoints.clear();
+    for(const PosePtr& pose : waypoints)
+    {
+        this->waypoints.push_back(pose->toEigen());
+    }
+    currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f &waypoint)
+{
+    this->waypoints.push_back(waypoint);
+}
+
+void PositionControllerHelper::setTarget(const Eigen::Matrix4f &target)
+{
+    waypoints.clear();
+    waypoints.push_back(target);
+    currentWaypointIndex = 0;
+}
+
+float PositionControllerHelper::getPositionError() const
+{
+    return posController.getPositionError(getCurrentTarget());
+}
+
+float PositionControllerHelper::getOrientationError() const
+{
+    return posController.getOrientationError(getCurrentTarget());
+}
+
+bool PositionControllerHelper::isTargetReached() const
+{
+    return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+}
+
+bool PositionControllerHelper::isTargetNear() const
+{
+    return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+}
+
+bool PositionControllerHelper::isFinalTargetReached() const
+{
+    return isLastWaypoint() && isTargetReached();
+}
+
+bool PositionControllerHelper::isFinalTargetNear() const
+{
+    return isLastWaypoint() && isTargetNear();
+}
+
+bool PositionControllerHelper::isLastWaypoint() const
+{
+    return currentWaypointIndex + 1 >= waypoints.size();
+}
+
+const Eigen::Matrix4f &PositionControllerHelper::getCurrentTarget() const
+{
+    return waypoints.at(currentWaypointIndex);
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..cedc9b5405773a6712fa0fc6aed4e099cd3be2ce
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -0,0 +1,78 @@
+/*
+ * 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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <Eigen/Dense>
+
+#include "VelocityControllerHelper.h"
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+    class PositionControllerHelper;
+    typedef boost::shared_ptr<PositionControllerHelper> PositionControllerHelperPtr;
+
+    class PositionControllerHelper
+    {
+    public:
+        PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target);
+
+        void update();
+
+        void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+        void setNewWaypoints(const std::vector<PosePtr>& waypoints);
+        void addWaypoint(const Eigen::Matrix4f& waypoint);
+        void setTarget(const Eigen::Matrix4f& target);
+
+        float getPositionError() const;
+
+        float getOrientationError() const;
+
+        bool isTargetReached() const;
+        bool isTargetNear() const;
+        bool isFinalTargetReached() const;
+        bool isFinalTargetNear() const;
+
+        bool isLastWaypoint() const;
+
+        const Eigen::Matrix4f& getCurrentTarget() const;
+
+        CartesianPositionController posController;
+        VelocityControllerHelperPtr velocityControllerHelper;
+
+        std::vector<Eigen::Matrix4f> waypoints;
+        size_t currentWaypointIndex = 0;
+
+        float thresholdPositionReached = 0;
+        float thresholdOrientationReached = 0;
+        float thresholdPositionNear = 0;
+        float thresholdOrientationNear = 0;
+    };
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..720addebc816a025b5ca6fef106dedb2df718f6f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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    RobotAPI::ArmarXObjects::RobotStatechartHelpers
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "RobotStatechartHelpers.h"
+
+
+using namespace armarx;
+
+
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..c476160c84e8ea782ff55c10f4ca890560abe550
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
@@ -0,0 +1,45 @@
+/*
+ * 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    RobotAPI::ArmarXObjects::RobotStatechartHelpers
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-RobotStatechartHelpers RobotStatechartHelpers
+    * @ingroup RobotAPI
+    * A description of the library RobotStatechartHelpers.
+    *
+    * @class RobotStatechartHelpers
+    * @ingroup Library-RobotStatechartHelpers
+    * @brief Brief description of class RobotStatechartHelpers.
+    *
+    * Detailed description of class RobotStatechartHelpers.
+    */
+    class RobotStatechartHelpers
+    {
+    public:
+
+    };
+
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..22d7b732fcb966d797cf93184aad3b862e22abe1
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -0,0 +1,77 @@
+/*
+ * 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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "VelocityControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx &robotUnit, const std::string &nodeSetName, const std::string &controllerName)
+    : robotUnit(robotUnit), controllerName(controllerName)
+{
+    config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
+    init();
+}
+
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx &robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string &controllerName)
+    : robotUnit(robotUnit), controllerName(controllerName)
+{
+    this->config = config;
+    init();
+}
+
+void VelocityControllerHelper::init()
+{
+    NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+    if (ctrl)
+    {
+        controllerCreated = false;
+    }
+    else
+    {
+        ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+        controllerCreated = true;
+    }
+    controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);
+    controller->activateController();
+}
+
+void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf &cv)
+{
+    controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
+}
+
+void VelocityControllerHelper::cleanup()
+{
+    controller->deactivateController();
+    if (controllerCreated)
+    {
+        while (controller->isControllerActive())
+        {
+            TimeUtil::SleepMS(1);
+        }
+        controller->deleteController();
+    }
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..07876699976dd9445d4405a3efe559cbdb2b33bc
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
@@ -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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class VelocityControllerHelper;
+    typedef boost::shared_ptr<VelocityControllerHelper> VelocityControllerHelperPtr;
+
+    class VelocityControllerHelper
+    {
+    public:
+        VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName);
+        VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName);
+
+        void init();
+
+        void setTargetVelocity(const Eigen::VectorXf& cv);
+
+        void cleanup();
+
+        NJointCartesianVelocityControllerWithRampConfigPtr config;
+        NJointCartesianVelocityControllerWithRampInterfacePrx controller;
+        RobotUnitInterfacePrx robotUnit;
+        std::string controllerName;
+        bool controllerCreated = false;
+    };
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 1c49e462dee3383b6ac73cdeca27d5f16a4388bc..3ffd160461bef2710fc8041f66cf3d8588819fd4 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -32,7 +32,7 @@ CartesianPositionController::CartesianPositionController(const VirtualRobot::Rob
 {
 }
 
-Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
 {
     int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
     int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
@@ -68,14 +68,14 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
     return cartesianVel;
 }
 
-float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose)
+float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
     Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
     return errPos.norm();
 }
 
-float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose)
+float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
     Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
@@ -84,14 +84,14 @@ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& ta
     return aa.angle();
 }
 
-Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose)
+Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
     return targetPos - tcp->getPositionInRootFrame();
 
 }
 
-Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose)
+Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
     Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 9d6843006f86c38460153a67ad44b7d4055cc15d..e0c4ec59905a5672a93e19bb11f4dc0eb7cd4338 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -40,12 +40,12 @@ namespace armarx
     public:
         CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp);
 
-        Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode);
+        Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
 
-        float getPositionError(const Eigen::Matrix4f& targetPose);
-        float getOrientationError(const Eigen::Matrix4f& targetPose);
-        Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose);
-        Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose);
+        float getPositionError(const Eigen::Matrix4f& targetPose) const;
+        float getOrientationError(const Eigen::Matrix4f& targetPose) const;
+        Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
+        Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
 
         //CartesianVelocityController velocityController;
         float KpPos = 1;