Robotics StackExchange | Archived questions

Moveit time parameterization

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();

Asked by edote on 2020-03-27 14:07:45 UTC

Comments

Do you have issues if you don't use CHOMP? Are there large jumps in your trajectory? How are you calling the retiming functions and what is returned?

Asked by fvd on 2020-03-30 05:31:32 UTC

Thank you for your answer fvd, 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 console output :

<[ INFO] [1585590007.623463814]: Ready to take commands for planning group Leg. [ INFO] [1585590009.486245997]: Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using 'chomp_interface/CHOMPPlanner' for now. [ INFO] [1585590009.498345015]: Using planning interface 'CHOMP' [ INFO] [1585590013.784286201]: The timeout for planning must be positive (0.000000 specified). Assuming one second instead. [ERROR] [1585590013.784547245]: Found empty JointState message [ INFO] [1585590013.785235278]: CHOMP trajectory initialized using method: quintic-spline [ INFO] [1585590013.785840342]: The following collision detectors are available in the planning scene>

Asked by edote on 2020-03-30 12:56:49 UTC

What do you mean by "large jumps" ? I set a joint goal and use the function generatePlan(), it generates a path with 100 points for each joint. This is the code that calls the retiming functions :

planning_pipeline->generatePlan(planning_scene, req, res); size_t nb_points = res.trajectory_->getWayPointCount(); const uint size = nb_points; double velocities_j1[size]; double time_j1[size]; for (int j = 0; j<=size-1; ++j){

const robot_state::RobotState& points_ptr = res.trajectory_->getWayPoint(j); time_j1[j] = 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; } for (int l = 0; l<=size-1; ++l){ cout<<time_j1[l]<<endl; };

I also tried with getWayPointDurationFromStart() and getDuration(), but it also gave vectors filled with zeros.

Asked by edote on 2020-03-30 13:37:29 UTC

Please edit your question with the extra information. In your code, I don't actually see where you are using the time parametrization. It looks like you are just reading the trajectory, but it's hard to read as is.

By "large jumps" I mean large differences between the waypoints of the trajectory. If you graph it or print it out, does it look continuous?

Asked by fvd on 2020-03-31 08:39:41 UTC

There is no large jump, it is continuous. I set the velocity with this method (http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/time_parameterization/time_parameterization_tutorial.html#during-runtime) :

planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; double deg_goal_speed = 10*360.0; // °/s double rad_goal_speed = deg_to_rad(deg_goal_speed);// rad/s double speed_scale = rad_goal_speed/20; req.max_velocity_scaling_factor = speed_scale; 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_pipeline->generatePlan(planning_scene, req, res);

Asked by edote on 2020-03-31 09:55:29 UTC

I edited the ompl_planning_pipeline.launch file and some other files as specified there to use AddTimeParameterization : http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/planning_adapters/planning_adapters_tutorial.html

Do you mean that there is a way to call the adapter programmatically ? Isn't it called automatically with the planner, if the files are configured ?

Asked by edote on 2020-03-31 09:58:54 UTC

I edited your question. Please add new information to it yourself. Yes, you can call the time parametrization directly with a RobotTrajectory of your own. Look here for function of the IPTP algorithm. The others are analogous.

Asked by fvd on 2020-03-31 10:04:48 UTC

I'm confused I edited it, what did you edit ?

I feel stupid, I didn't use a IterativeParabolicTimeParameterization to compute the stamps before using getWayPointDuration(), it works now ! Thank you a lot !

Asked by edote on 2020-03-31 13:34:10 UTC

You can look at the edit revisions here. Thanks for posting an answer.

Asked by fvd on 2020-04-01 08:28:15 UTC

Answers

I just needed to compute the timestamps before reading them. I added these three lines and it works fine :

  #include <moveit/trajectory_processing/iterative_time_parameterization.h>

then, after generating the plan :

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

Asked by edote on 2020-03-31 13:37:49 UTC

Comments