diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index cea227464abe29898488605e6dcb38f5e50fb4a5..90c68c71c1fd0ca963f8f4eb1bc05c1d926be1f7 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -1971,6 +1971,36 @@ namespace armarx
         return getDeriv(dim, 0);
     }
 
+    Eigen::VectorXf Trajectory::TrajData::getPositionsAsVectorXf() const
+    {
+        if (!trajectory)
+        {
+            throw LocalException("Ptr to trajectory is NULL");
+        }
+        size_t numDim = trajectory->dim();
+        Eigen::VectorXf result(numDim);
+        for (std::size_t i = 0; i < numDim; ++i)
+        {
+            result(i) = getPosition(i);
+        }
+        return result;
+    }
+
+    Eigen::VectorXd Trajectory::TrajData::getPositionsAsVectorXd() const
+    {
+        if (!trajectory)
+        {
+            throw LocalException("Ptr to trajectory is NULL");
+        }
+        size_t numDim = trajectory->dim();
+        Eigen::VectorXd result(numDim);
+        for (std::size_t i = 0; i < numDim; ++i)
+        {
+            result(i) = getPosition(i);
+        }
+        return result;
+    }
+
     double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
     {
         if (!trajectory)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 318c66bef78228a14e7b77c4defe2e768728890d..38b353a038752b4426c957a332b614c5fda51dfc 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -128,6 +128,9 @@ namespace armarx
             {
                 return getPositionsAs<double>();
             }
+            Eigen::VectorXf getPositionsAsVectorXf() const;
+            Eigen::VectorXd getPositionsAsVectorXd() const;
+
 
             template<class T>
             void writePositionsToNameValueMap(std::map<std::string, T>& map) const
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
index 5539a9afbf07f9c4b845bb52485434592295d1c9..6d76464e94326b8e864ace29ec294d10aff65402 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -392,7 +392,7 @@ void GraspTrajectory::writeToFile(const std::string& filename)
 
 GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
 {
-    std::string packageName = cnd->executionHints->graspTrajectoryPackage;
+    std::string packageName = "Armar6Skills";// cnd->executionHints->graspTrajectoryPackage;
     armarx::CMakePackageFinder finder(packageName);
     std::string dataDir = finder.getDataDir() + "/" + packageName;
     return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml");
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index ea98170c468c82a599e5dd1f22a73f2a76ffaaa5..70d3d998b90b042d9354893609e01f1e24881ef0 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -31,6 +31,7 @@
 using namespace armarx;
 
 
+
 CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
     : _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
 {
@@ -85,7 +86,10 @@ void CartesianNaturalPositionControllerProxy::init()
         _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
         _controllerCreated = true;
     }
-    _controller->activateController();
+    if (_activateControllerOnInit)
+    {
+        _controller->activateController();
+    }
 }
 
 bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight)
@@ -347,6 +351,11 @@ std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::
     return result;
 }
 
+void CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value)
+{
+    _activateControllerOnInit = value;
+}
+
 void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
 {
     VelocityBaseSettings& v = _velocityBaseSettings;
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index d87c65c4bf5136cf2ea3812059242ed6710c87b5..c5b502dd1406c25398fefb78d6a2bd70fb746126 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -113,7 +113,6 @@ namespace armarx
             VirtualRobot::RobotNodeSetPtr rns;
         };
 
-
         CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
         static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
 
@@ -174,6 +173,8 @@ namespace armarx
         DynamicKp getDynamicKp();
         static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale);
 
+        void setActivateControllerOnInit(bool value);
+
     private:
         void updateDynamicKp();
         void updateNullspaceTargets();
@@ -207,5 +208,6 @@ namespace armarx
         std::map<std::string, WaypointConfig> _defaultWaypointConfigs;
         bool _waypointChanged = false;
         FTSensorValue _ftOffset;
+        bool _activateControllerOnInit = false;
     };
 }