VigirGraspTrajectoryController working like JointTrajectoryController