Check our paper Jerk-bounded Position Controller with Real-Time Task Modification for Interactive Industrial Robots.