diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index a8f71316e4e8ffc6748ab2f7eef6498f589aee2a..e60347e7a58fb6914282dfbb659a7737cc08854c 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -26,6 +26,7 @@
 #include "VectorHelpers.h"
 #include <ArmarXCore/observers/AbstractObjectSerializer.h>
 #include "math/MathUtils.h"
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
 
 namespace armarx
 {
@@ -1726,6 +1727,71 @@ namespace armarx
         return newTraj;
     }
 
+    TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, const IceUtil::Time& timestep)
+    {
+        Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(dim(), maxVelocity);
+        Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(dim(), maxAcceleration);
+        return calculateTimeOptimalTrajectory(maxVelocities, maxAccelerations, maxDeviation, timestep);
+    }
+
+    TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep)
+    {
+
+        auto timestepValue = timestep.toSecondsDouble();
+        std::list<Eigen::VectorXd> waypointList;
+        auto dimensions = dim();
+        for (auto& waypoint : *this)
+        {
+            auto positions = waypoint.getPositions();
+            waypointList.push_back(Eigen::Map<Eigen::VectorXd>(positions.data(), dimensions));
+        }
+        VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(VirtualRobot::Path(waypointList, maxDeviation),
+                maxVelocities,
+                maxAccelerations, timestepValue);
+        ARMARX_CHECK_EXPRESSION(timeOptimalTraj.isValid());
+
+
+        TrajectoryPtr newTraj = new Trajectory();
+
+        Ice::DoubleSeq newTimestamps;
+        double duration = timeOptimalTraj.getDuration();
+        newTimestamps.reserve(duration / timestepValue + 1);
+        for (double t = 0.0; t < duration; t += timestepValue)
+        {
+            newTimestamps.push_back(t);
+        }
+        newTimestamps.push_back(duration);
+
+        for (size_t d = 0; d < dimensionNames.size(); d++)
+        {
+            Ice::DoubleSeq position;
+            position.reserve(newTimestamps.size());
+            for (double t = 0.0; t < duration; t += timestepValue)
+            {
+                position.push_back(timeOptimalTraj.getPosition(t)[d]);
+            }
+            position.push_back(timeOptimalTraj.getPosition(duration)[d]);
+            newTraj->addDimension(position, newTimestamps, dimensionNames.at(d));
+
+            Ice::DoubleSeq derivs;
+            derivs.reserve(newTimestamps.size());
+
+            for (double t = 0.0; t < duration; t += timestepValue)
+            {
+                derivs.clear();
+                derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
+                derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
+                newTraj->addDerivationsToDimension(d, t, derivs);
+            }
+            derivs.clear();
+            derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
+            derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
+            newTraj->addDerivationsToDimension(d, duration, derivs);
+        }
+        newTraj->setLimitless(limitless);
+        return newTraj;
+    }
+
 
 
 
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 03ddd3972f399c02b13c00b140ef2c423633796a..5f7613bdca048ee60f3efdcc43b53e7d9f79ca62 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -380,6 +380,8 @@ namespace armarx
 
         static Trajectory NormalizeTimestamps(const Trajectory& traj, const double startTime = 0.0,  const double endTime = 1.0);
         TrajectoryPtr normalize(const double startTime = 0.0,  const double endTime = 1.0);
+        TrajectoryPtr calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, IceUtil::Time const& timestep);
+        TrajectoryPtr calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep);
         static Ice::DoubleSeq NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime = 0.0,  const double endTime = 1.0);
         static Ice::DoubleSeq GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
         template <typename T>