diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index aa88f0c..7fe6cd3 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -55,18 +55,18 @@ 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();
-	 */
+
 	unsigned int j = 0;
 	for (unsigned int i = 0; i < timestamps.size(); i++) {
 		while (inp_timestamps[j] <= timestamps[i]) {
 			j += 1;
 		}
 		positions.push_back(
-				UrDriver::interp_cubic(timestamps[i], inp_timestamps[j],
+				UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j-1], inp_timestamps[j] - inp_timestamps[j-1],
 						inp_positions[j - 1], inp_positions[j],
 						inp_velocities[j - 1], inp_velocities[j]));
 	}
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index 3fd745d..a6a8b5c 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -30,15 +30,14 @@ class URTrajectoryFollower {
 protected:
 	ros::NodeHandle nh_;
 	actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
-	std::string action_name_;
 	actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal_;
 	control_msgs::FollowJointTrajectoryFeedback feedback_;
 	control_msgs::FollowJointTrajectoryResult result_;
 	ros::Subscriber sub_;
 	UrDriver* robot_;
 public:
-	URTrajectoryFollower(UrDriver* robot, std::string name) :
-			as_(nh_, "follow_joint_trajectory", false), action_name_(name) {
+	URTrajectoryFollower(UrDriver* robot) :
+			as_(nh_, "follow_joint_trajectory", false) {
 		robot_ = robot;
 		//register the goal and feeback callbacks
 		as_.registerGoalCallback(
@@ -88,7 +87,7 @@ public:
 	}
 
 	void preemptCB() {
-		ROS_INFO("%s: Preempted", action_name_.c_str());
+		ROS_INFO("on_cancel");
 		// set the action state to preempted
 		robot_->stopTraj();
 		as_.setPreempted();
@@ -216,7 +215,7 @@ int main(int argc, char **argv) {
 	robot.start();
 
 	std::thread rt_publish_thread(publishRTMsg, robot);
-	URTrajectoryFollower action_server(&robot, ros::this_node::getName());
+	URTrajectoryFollower action_server(&robot);
 
 	ros::spin();
 	robot.halt();