Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I use the waypoints by following this Tutorial http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html#cartesian-paths

Note that, you can specify the the interpotion resolution between two consecutive waypoints. Here is my implementation

  //2. Create Cartesian Paths
  std::vector<geometry_msgs::Pose> waypoints;
  geometry_msgs::Pose ee_point_goal; //end_effector_trajectory
  ee_point_goal.orientation.w = 1.0;
  ee_point_goal.position.z = 0.8;
      // Trajectory parameters (circle)
  double d_angle = angle_resolution*3.14/180;
  double angle= 0;

  //----------------------------------------------------------------------
  //3. Plan for the trajectory
  for (int i= 0; i< (360/angle_resolution); i++)
  {
    //discretize the trajectory
    angle+= d_angle;
    ee_point_goal.position.x = x_center + radius*cos(angle);
    ee_point_goal.position.y = y_center + radius*sin(angle);
    waypoints.push_back(ee_point_goal);
    //ROS_INFO("%d",i);
  }

  ROS_INFO("There are %d number of waypoints", waypoints.size());
  // We want cartesian path to be interpolated at a eef_resolution
  double eef_resolution = std::min(0.01,radius*angle_resolution);
  double jump_threshold = 0.0; //disable the jump threshold =0
  moveit_msgs::RobotTrajectory trajectory;
  double fraction =plan_group.computeCartesianPath(waypoints, eef_resolution,jump_threshold, trajectory);

  ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",  fraction * 100.0);