diff --git a/config/config.yaml b/config/config.yaml
index a18ba849e3714314ed726f9e2780839dc1cae3ae..1c84f392d70e116356398ac6185f8bd6233310d0 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -1,12 +1,14 @@
 panda_mqtt_connector:
   server: "tcp://localhost:1883"
+  robot_speed_factor: .15
+  robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
   topics:
     robotConfig: "robotconfig"
     dataConfig: "dataconfig"
   zone_size: 0.5
   zones:
     - "1 1"
-    - "-1 -1 1"
+    - "0 1"
   parts:
     panda:
       Link0: "panda::panda_link0"
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 7bb35446c5d1f5bf5a827cbf46bbae61a95a2c4f..9404808bc650c3584d0014c9a77330cedca68a7a 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -24,7 +24,6 @@ namespace PlannerState {
   const double default_velocity = 1.0;
   const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
 
-  bool isInitialized = false;
   bool isLooping = true;
 
 }
@@ -176,7 +175,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     }
 
     // wake up if we have trajectory
-    while (PlannerState::raw_trajectory.size() == 0) {
+    while (PlannerState::raw_trajectory.empty()) {
       if (newTrajectoryAvailable) {
         updateRawTrajectory(node_handle, &group);
         i = 0;
@@ -185,7 +184,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     }
 
     double velocity = 0.0;
-    std::string planning_mode = "";
+    std::string planning_mode;
 
     if (!node_handle.getParam("robot_speed_factor", velocity)) {
       velocity = PlannerState::default_velocity;
@@ -198,7 +197,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
     ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity);
 
     moveit::planning_interface::MoveGroupInterface::Plan plan;
-    PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, velocity);
+    PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity);
     group.execute(plan);
 
     // make sure the robot moves in an infinite circle
diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp
index c3423a9b0a6ff2be7c8bbcf40aea1c152ed7e713..88c70d7600fb4d8a1f08ec36883f2fc80f1a012b 100644
--- a/src/util/TrajectoryUtil.cpp
+++ b/src/util/TrajectoryUtil.cpp
@@ -8,118 +8,104 @@ const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
 const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";
 
 bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
-                       moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
-                       std::string pathType, double velocity)
-{
-    if(velocity < 0.1 || velocity > 1.0){
-        ROS_ERROR("Invalid velocity configured. ");
-        return false;
-    }
-
-    if (pathType.compare(CARTESIAN_PATH) == 0)
-    {
-        if(!computeCartesianPath(group, plan, targetPose))
-        {
-            return false;
-        }else{
-            manipulateVelocity(velocity, group, plan, CARTESIAN_PATH);
-            return true;
-        }
-    }
-
-    if(pathType.compare(FLUID_PATH) == 0)
-    {
-        group.setStartStateToCurrentState();
-        group.setPlanningTime(10.0);
-
-        group.setPoseTarget(targetPose);
-        manipulateVelocity(velocity, group, plan, FLUID_PATH);
-        return (group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    }
-
-    ROS_ERROR("Invalid type of path provided.");
-
+                                       moveit::planning_interface::MoveGroupInterface::Plan &plan,
+                                       geometry_msgs::Pose targetPose,
+                                       const std::string& pathType, double maxVelocityFactor,
+                                       double maxAccelerationFactor, double motionSpeedFactor) {
+  if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) {
+    ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1].");
     return false;
-}
-
-bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose)
-{
-    std::vector<double> x_values;
-    std::vector<double> y_values;
-    std::vector<double> z_values;
-
-    // check if no trajectory is configured
-    if(!node_handle.getParam("trajectory_pos_x", x_values) ||
-       !node_handle.getParam("trajectory_pos_y", y_values) ||
-       !node_handle.getParam("trajectory_pos_z", z_values) )
-    {
-        return false;
-    }else{
-        for(int i = 0; i < x_values.size(); i++)
-        {
-            geometry_msgs::Pose pose = initialPose;
-            pose.position.x = x_values.at(i);
-            pose.position.y = y_values.at(i);
-            pose.position.z = z_values.at(i);
-            waypoints.push_back(pose);
-        }
-
-        node_handle.setParam("new_trajectory_available", false);
-        return true;
-    }
-}
+  }
+  if (maxAccelerationFactor <= 0 || maxAccelerationFactor > 1.0) {
+    ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1].");
+    return false;
+  }
+  if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
+    ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
+    return false;
+  }
 
-bool TrajectoryUtil::computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
-                                          moveit::planning_interface::MoveGroupInterface::Plan &plan,
-                                          const geometry_msgs::Pose &targetPose) {
+  group.setStartStateToCurrentState();
+  group.setPlanningTime(10.0);
+  group.setPoseTarget(targetPose);
+  group.setMaxVelocityScalingFactor(maxVelocityFactor);
+  group.setMaxAccelerationScalingFactor(maxAccelerationFactor);
 
+  if (pathType == CARTESIAN_PATH) {
     std::vector<geometry_msgs::Pose> single_traj{targetPose};
-    group.setStartStateToCurrentState();
-    group.setPlanningTime(10.0);
 
     moveit_msgs::RobotTrajectory trajectory_msg;
-    double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, false);
+    double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, true);
+    ROS_INFO("Planned cartesian plan (%.2f%% achieved)", fraction * 100.0);
 
     robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
     rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
-    trajectory_processing::IterativeParabolicTimeParameterization iptp;
+    trajectory_processing::IterativeParabolicTimeParameterization iptp{};
 
-    bool success = iptp.computeTimeStamps(rt);
-    ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
+    ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s");
 
-    if(!success){
-        return false;
+    if (iptp.computeTimeStamps(rt)) {
+      ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s");
     }
 
-    rt.getRobotTrajectoryMsg(trajectory_msg);
-    ROS_INFO("Planned cartesian plan (%.2f%% acheived)", fraction * 100.0);
-    plan.trajectory_ = trajectory_msg;
+    if (iptp.computeTimeStamps(rt, maxVelocityFactor)) {
+      ROS_INFO("Computed time stamps.");
+      rt.getRobotTrajectoryMsg(trajectory_msg);
+      plan.trajectory_ = trajectory_msg;
 
+      ROS_ERROR_STREAM("After second time parameterization, the trajectory takes " << rt.getDuration() << "s");
+    } else {
+      ROS_ERROR("Computation of cartesian path has failed.");
+      return false;
+    }
+  } else if (pathType == FLUID_PATH) {
+    if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
+      ROS_ERROR("Computation of fluid path has failed.");
+    }
+  } else {
+    ROS_ERROR("Invalid type of path provided.");
+    return false;
+  }
+
+  manipulateVelocity(plan, motionSpeedFactor);
+}
+
+bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
+                                   geometry_msgs::Pose &initialPose) {
+  std::vector<double> x_values;
+  std::vector<double> y_values;
+  std::vector<double> z_values;
+
+  // check if no trajectory is configured
+  if (!node_handle.getParam("trajectory_pos_x", x_values) ||
+      !node_handle.getParam("trajectory_pos_y", y_values) ||
+      !node_handle.getParam("trajectory_pos_z", z_values)) {
+    return false;
+  } else {
+    for (int i = 0; i < x_values.size(); i++) {
+      geometry_msgs::Pose pose = initialPose;
+      pose.position.x = x_values.at(i);
+      pose.position.y = y_values.at(i);
+      pose.position.z = z_values.at(i);
+      waypoints.push_back(pose);
+    }
+
+    node_handle.setParam("new_trajectory_available", false);
     return true;
+  }
 }
 
-void TrajectoryUtil::manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group,
-                                        moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType)
-{
-    if(pathType.compare(FLUID_PATH))
-    {
-        group.setMaxVelocityScalingFactor(velocity);
+void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) {
+  ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << "joints");
+  for (auto &point : plan.trajectory_.joint_trajectory.points) {
+    point.time_from_start *= 1 / velocity;
+    for (double &v : point.velocities) {
+      v *= velocity;
     }
 
-    if(pathType.compare(CARTESIAN_PATH))
-    {
-        for (int i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++) {
-            plan.trajectory_.joint_trajectory.points[i].time_from_start.operator*=((1 / velocity));
-            for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).velocities.size(); j++) {
-                if (plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
-                    plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
-                }
-            }
-
-            for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).accelerations.size(); j++) {
-                plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j) =
-                        sqrt(std::abs(plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j)));
-            }
-        }
+    for (double &acceleration : point.accelerations) {
+      acceleration = sqrt(std::abs(acceleration));
     }
+  }
+
 }
diff --git a/src/util/TrajectoryUtil.h b/src/util/TrajectoryUtil.h
index 0569c2200ee077eb6395ce614a22f4fa19a989a6..15ea8fbf601612887990c8d730f2acfce030615f 100644
--- a/src/util/TrajectoryUtil.h
+++ b/src/util/TrajectoryUtil.h
@@ -18,23 +18,21 @@ class TrajectoryUtil {
 
 public:
 
-    static const std::string CARTESIAN_PATH;
-    static const std::string FLUID_PATH;
+  static const std::string CARTESIAN_PATH;
+  static const std::string FLUID_PATH;
 
-    bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
-                           moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
-                           std::string pathType, double velocity);
+  bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
+                         moveit::planning_interface::MoveGroupInterface::Plan &plan,
+                         geometry_msgs::Pose targetPose,
+                         const std::string &pathType, double maxVelocityFactor,
+                         double maxAccelerationFactor, double motionSpeedFactor);
 
-    bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose);
+  bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
+                     geometry_msgs::Pose &initialPose);
 
 private:
 
-    bool computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
-                              moveit::planning_interface::MoveGroupInterface::Plan &plan,
-                              const geometry_msgs::Pose &targetPose);
-
-    void manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group,
-                            moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType);
+  void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
 };
 
 #endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file