Added time parametrization and execution on FollowJointTrajectoryController