bool srvCallback_setVel(cob_trajectory_controller::SetFloat::Request &req, cob_trajectory_controller::SetFloat::Response &res)
 {
     ROS_INFO("Setting velocity to %f", req.value.data);
     traj_generator_->SetPTPvel(req.value.data);
     res.success.data = true;
     return true;
 }