Ask Your Question
0

Moveit time parameterization

asked 2020-03-27 14:07:45 -0500

edote gravatar image

updated 2020-03-31 10:32:52 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

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?

fvd gravatar image fvd  ( 2020-03-30 05:31:32 -0500 )edit

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.

edote gravatar image edote  ( 2020-03-30 12:56:49 -0500 )edit

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]&lt;<endl; };<="" p="">

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

edote gravatar image edote  ( 2020-03-30 13:37:29 -0500 )edit

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?

fvd gravatar image fvd  ( 2020-03-31 08:39:41 -0500 )edit

There is no large jump, it is continuous. I set the velocity with this method (http://docs.ros.org/melodic/api/movei...) :

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

edote gravatar image edote  ( 2020-03-31 09:55:29 -0500 )edit

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

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 ?

edote gravatar image edote  ( 2020-03-31 09:58:54 -0500 )edit

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.

fvd gravatar image fvd  ( 2020-03-31 10:04:48 -0500 )edit

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 !

edote gravatar image edote  ( 2020-03-31 13:34:10 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2020-03-31 13:37:49 -0500

edote gravatar image

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);
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-03-27 14:07:45 -0500

Seen: 48 times

Last updated: Mar 31