diff --git a/abb_egm_driver/src/abb_egm_robot_interface.cpp b/abb_egm_driver/src/abb_egm_robot_interface.cpp
index 98c32ad1466eb48a0c5cd56827ff443175142d7a..5bfcf95e8a91c3ddd0d681d187fe49a1c098ca29 100644
--- a/abb_egm_driver/src/abb_egm_robot_interface.cpp
+++ b/abb_egm_driver/src/abb_egm_robot_interface.cpp
@@ -173,11 +173,11 @@ bool AbbEgmRobotInterface::jointTrajectoryCall(const trajectory_msgs::JointTraje
         ROS_INFO("Stop current static goal to follow new trajectory");
       else
         ROS_WARN("Could not stop the static goal even though the interface switched from static goal mode to trajectory mode.");
-      ros::Duration(0.1).sleep();
   }
+
   if(interface_mode != INTERFACE_TRAJECTORY){
     interface_mode = INTERFACE_TRAJECTORY;
-    ros::Duration(0.5).sleep();
+    ros::Duration(1.5).sleep();
   }
 
   // An empty ROS trajectory means that the robot should stop