Moveit time parameterization

edit

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 max_velocity_scaling_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<string> &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/movei... http://docs.ros.org/melodic/api/movei...

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/movei...

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 ...