diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h
index 2cb2650..6eaf957 100644
--- a/include/ur_modern_driver/ur_driver.h
+++ b/include/ur_modern_driver/ur_driver.h
@@ -31,7 +31,7 @@ public:
 	UrCommunication* sec_interface_;
 
 	UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
-			unsigned int safety_count_max = 12, double max_time_step = 0.08,
+			unsigned int safety_count_max = 12, double max_time_step = 0.24,
 			double min_payload = 0., double max_payload = 1.);
 	void start();
 	void halt();
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index a524b4d..e126fa7 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -57,10 +57,10 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 		}
 	}
 	//make sure we come to a smooth stop
-	while (timestamps.back() < inp_timestamps.back()) {
+	/*while (timestamps.back() < inp_timestamps.back()) {
 		timestamps.push_back(timestamps.back() + 0.008);
 	}
-	timestamps.pop_back();
+	timestamps.pop_back();*/
 
 	unsigned int j = 0;
 	for (unsigned int i = 0; i < timestamps.size(); i++) {
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index 5cd23ad..82da922 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -271,7 +271,8 @@ private:
 						traj.points[i].positions[mapping[j]]);
 				new_point.velocities.push_back(
 						traj.points[i].velocities[mapping[j]]);
-				new_point.accelerations.push_back(
+				if (traj.points[i].accelerations.size() != 0)
+					new_point.accelerations.push_back(
 						traj.points[i].accelerations[mapping[j]]);
 			}
 			new_point.time_from_start = traj.points[i].time_from_start;