rotate_end_effector in circular

What are singularities in a six-axis robot arm?
If you intend to use a six-axis robot arm, such as Mecademic’s Meca500, the example featured in this tutorial, you will probably need to do more than just position and orient the robot end-effector in various poses. You will likely also need the end-effector to follow prescribed paths as in gluing, or when inserting a pin. If this is the case, then you must learn about robot singularities, because these special configurations will often impede the Cartesian movements of your robot end-effector. You must therefore know how to keep away from robot singularities by properly designing your robot cell.

An industrial robot can be controlled in two spaces: joint space and Cartesian space. Hence, there are two sets of position-mode motion commands that make an industrial robot move. For joint-space motion commands (sometimes incorrectly called point-to-point commands), you simply specify — directly or indirectly — a desired set of joint positions, and the robot moves by translating or rotating each joint to the desired joint position, simultaneously and in a linear fashion. For Cartesian-space motion commands, you specify a desired pose for the end-effector AND a desired Cartesian path (linear or circular). To find the necessary joint positions along the desired Cartesian path, the robot controller must calculate the inverse position and velocity kinematics of the robot. Singularities arise when this calculation fails (for example, when you have division by zero) and must therefore be avoided.

Try jogging a six-axis robot arm in joint space, and the only time the robot will stop is when a joint hits a limit or when there is a mechanical interference. In contrast, try jogging the same robot in Cartesian space and the robot will frequently stop and refuse to go in certain directions, although it seems to be far from what you think is the workspace boundary. A robot singularity is a configuration in which the robot end-effector becomes blocked in certain directions.

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "rotate_end_effector");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // Setup
  static const std::string PLANNING_GROUP = "xarm7";
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to add and remove collision objects in our "virtual world" scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);


// Visualization
  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
  visual_tools.deleteAllMarkers();

  // Remote control is an introspection tool that allows users to step through a high level script
  // via buttons and keyboard shortcuts in RViz
  visual_tools.loadRemoteControl();

  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "rotate_end_effector", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  visual_tools.trigger();

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // Start the demo
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");


  geometry_msgs::Pose target_pose = move_group.getCurrentPose().pose;
  //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;
  ee_point_goal.position.z = target_pose.position.z;
  // add the start position in path list 
  waypoints.push_back(target_pose);
  // Trajectory parameters (circle)
  double angle_resolution = 36;
  double d_angle = angle_resolution*3.14/180;
  double angle= 0;
  double radius = 0.2;
  double centerA = target_pose.position.x;
  double centerB = target_pose.position.y - radius;
  //----------------------------------------------------------------------
  //3. Plan for the trajectory
  for (int i= 0; i< (360/angle_resolution); i++)
  {
    //discretize the trajectory
    angle += i * d_angle;
    ee_point_goal.position.x = centerA + radius * cos(angle);
    ee_point_goal.position.y = centerB + radius * sin(angle);
    waypoints.push_back(ee_point_goal);
  }

  ROS_INFO("There are %d number of waypoints", waypoints.size());
  // We want cartesian path to be interpolated at a eef_resolution
  double eef_resolution = 0.01;
  moveit_msgs::RobotTrajectory trajectory;
  double jump_threshold = 0.0;
  double fraction = move_group.computeCartesianPath(waypoints, eef_resolution, jump_threshold, trajectory);

  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  my_plan.trajectory_ = trajectory;
  sleep(5.0);
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
    for (std::size_t i = 0; i < waypoints.size(); ++i){
         visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  }
  visual_tools.trigger();
    move_group.execute(my_plan);
}