Draw a circle with a robotic arm?

edit

Pretty much all the code I'm aware of that Willow Garage has produced in the arm navigation space is focused on getting the arm from Point A to Point B, generally in order to perform a manipulation operation. We allow users to impose constraints on the path - keep the end effector upright, for instance - but the path constraints are still designed to condition the path from point A to point B, not change the purpose of the planner.

Having the end effector follow a shape is a different kind of planning problem, one where the purpose is the path, not moving from one point to another, and we really don't have much code for such things. Coverage algorithm - for instance, plan a path that would paint an irregular surface as quickly as possible - are other examples of areas Willow Garage hasn't really focused on. There may be other code in the larger ROS landscape that does such things, but I'm not aware of it.

If I were to attempt to address such problems I'd look at the literature and try a few things out, using some of the lower-level tools from arm_navigation, like collision-checking and inverse kinematics. An approach that might work ok for attempting a geometric path would be to discretize the shape into a series of end effector positions, call IK for each one, and try to interpolate between the poses, making sure that the arm didn't hit joint limits or singularities. This might not give a great path, but could get the job done.

I suspect that general code that would do for coverage/path-focused problems what arm_navigation does for Point to Point travel would be a protracted research/tech-transfer project.

I use the waypoints by following this Tutorial http://docs.ros.org/indigo/api/pr2_mo...

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

I have moved the PR2 arm in "circles", but primarily using waypointss and wayvelocities(?).