Revision history [back]

Hi! i don't known if it helps but here my way of planning:

First, i do a planning by joints, in other words i do the "Planning to a joint-space goal" from the move_group_interface_tutorial.cpp

Second i do a reading of eef position with this code:

  geometry_msgs::PoseStamped robot_pose;
  robot_pose = group.getCurrentPose();

  geometry_msgs::Pose current_position;
  current_position = robot_pose.pose;

  /*Retrive position and orientation */
  geometry_msgs::Point exact_pose = current_position.position;
  geometry_msgs::Quaternion exact_orientation = current_position.orientation;

  ROS_INFO("Reference frame : %s",group.getPlanningFrame().c_str());
  ROS_INFO("Reference frame : %s",group.getEndEffectorLink().c_str());

  std::cout<<"Robot position : "<<exact_pose.x<<"\t"<<exact_pose.y<<"\t"<<exact_pose.z<<std::endl;
  std::cout<<"Robot Orientation : "<<exact_orientation.x<<"\t"<<exact_orientation.y<<"\t"<<exact_orientation.z<<"\t"<<exact_orientation.w<<std::endl;

Third with the coordinates that i read on the step 2, i do the planning by coordinates, in other words, the "Planning to a Pose goal" from move_group_interface_tutorial.cpp

And thats about it, that way my coordinates are always valid

Hope it helped