Hello,

I used Moveit to compute a path with my own robot, and now I am trying to get timestamps in order to create velocity curves.

I use the maxvelocityscaling_factor from the MotionPlanRequest msg to set the velocity, and I use the MotionPlanResponse msg to read the path. I tried several "getDuration" functions from the RobotTrajectory ()class to read the path, but I always get a correct position vector, and a time vector filled with 0.

When I run

const vector &adapter = planning_pipeline.getAdapterPluginNames();

I get an empty vector (I use this after generatePlan(), it is supposed to return the names of the planning adapters used).

I followed the Moveit tutorials about time parameterization and the planning request adapters (I run OMPL as a preprocessor or CHOMP), but it didn't make a change :

http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/planning_adapters/planning_adapters_tutorial.html http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/time_parameterization/time_parameterization_tutorial.html

I didn't install Moveit from source as specified in the tuto because CHOMP is now part of the official release and I use it, plus another person had the same problem with a source build (but didn't get an answer) : https://github.com/ros-planning/moveit/issues/1717

Thanks

Edit :

I tried to run OMPL as a preprocessor for STOMP according to the tutorial about planning adapters, but It seems that it doesn't take these settings, I still get CHOMP messages while running the code.

This is the code :

robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model
= robot_model_loader.getModel();

  static const string group = "Leg";   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;   moveit::planning_interface::MoveGroupInterface move_group(group);   const robot_state::JointModelGroup* joint_model_group =   move_group.getCurrentState()->getJointModelGroup(group); moveit::core::RobotStatePtr current_state = move_group.getCurrentState();   map<string, double> joint_goal;

  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); planning_pipeline::PlanningPipelinePtr planning_pipeline(
      new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));


    //------------------------FK--------------------------//


      moveit::core::RobotState target_state = *current_state;
      map<string, double> joint_values = move_group.getNamedTargetValues("Plie_Rent");
      vector<double> j1 = {joint_values["J1"]};
      target_state.setJointPositions("J1",j1);
      vector<double> j2 = {joint_values["J2"]};
      target_state.setJointPositions("J2",j2);
      vector<double> j3 = {joint_values["J3"]};
      target_state.setJointPositions("J3",j3);
      const Eigen::Isometry3d eef_state = target_state.getGlobalLinkTransform("Bout_pied");


      planning_interface::MotionPlanRequest req;
      planning_interface::MotionPlanResponse res;

      double deg_goal_speed = 10*360.0;
      double rad_goal_speed = deg_to_rad(deg_goal_speed);
      double speed_scale = rad_goal_speed/20;

      req.max_velocity_scaling_factor = speed_scale;
      req.group_name = "Leg";
      moveit_msgs::Constraints joint_goal1 = kinematic_constraints::constructGoalConstraints(target_state, > joint_model_group, 0.1, 0.1);
      req.goal_constraints.push_back(joint_goal1);


    //--------------------Planning--------------------//


      planning_pipeline->generatePlan(planning_scene, req, res);
      const vector<string> &adapter = planning_pipeline->getAdapterPluginNames();
      cout<<"ADAPTER_VECTOR_SIZE : "<<adapter.size()<<endl;
      /* Check that the planning was successful */
      if (res.error_code_.val != res.error_code_.SUCCESS) {
        ROS_ERROR("Could not compute plan successfully");
        return 0;
      }

      /* Visualize the trajectory */
      ROS_INFO("Visualizing the trajectory");
      size_t nb_points = res.trajectory_->getWayPointCount();
      const uint size = nb_points;
      double duree = res.trajectory_->getWayPointDurationFromStart(size);
      cout<<"TOTAL DURATION = "<<duree<<endl;
      double velocities_j1[size];
      double time_j1[size];
      double positions_j1[size];

// Method with MotionPlanRequest :
      velocities_j1[0] = 0.0;
      time_j1[0] = 0.0;
      for (int j = 1; j<=size-1; ++j){

      const robot_state::RobotState& points_ptr =  res.trajectory_->getWayPoint(j);
      time_j1[j] = time_j1[j-1] + res.trajectory_->getWayPointDurationFromPrevious(j);
      const string joint = "J1";
      const string& joint_ptr = joint;
      const double position = *points_ptr.getJointPositions(joint);
      const double vel1 = *points_ptr.getJointVelocities(joint_ptr);
      velocities_j1[j] = vel1; 
      positions_j1[j] = position;
      };
      const double duration = res.trajectory_->getDuration();
      cout<<"TOTAL DURATION (METH 2) = "<<duration<<endl;
      const std::deque< double > & time_j1 = res.trajectory_->getWayPointDurations();
      for (int l = 0; l<=size-1; ++l){
      cout<<time_j1[l]<<endl;
      };

      display_trajectory.trajectory_start = response.trajectory_start;
      display_trajectory.trajectory.push_back(response.trajectory);
      display_publisher.publish(display_trajectory);


//------------------------EXCEL-------------------------//


      ofstream myfile;
      myfile.open ("/home/edith/points_j1.csv");
      myfile << "Velocity Waypoints for J1 (rad/s<)"<<","<<"Positions for J1(rad)"<<";"<<"Time for J1(s)"<<endl;
      for (int h = 0; h<= size-1; ++h){

          myfile << velocities_j1[h] <<";"<< positions_j1[h] <<";"<<time_j1[h]<<endl;
      }
      myfile.close();

  #include <moveit/trajectory_processing/iterative_time_parameterization.h>

  trajectory_processing::IterativeParabolicTimeParameterization iptp(100, 0.05);
  iptp.computeTimeStamps(*res.trajectory_, speed_scale, 1.0);