To plan a single goal, we literally only need to provide MoveIt! with the goal itself. A goal is expressed by a Pose message from the geometry_msgs package. We need to specify both the orientation and the pose. For this particular example, this goal was obtained by performing manual planning and checking the state of the arm. In a real situation, goals will probably be set depending on the purpose of the robotic arm:

geometry_msgs::Pose goal;
goal.orientation.x = -0.000764819;
goal.orientation.y = 0.0366097;
goal.orientation.z = 0.00918912;
goal.orientation.w = 0.999287;
goal.position.x = 0.775884;
goal.position.y = 0.43172;
goal.position.z = 2.71809;

For this particular goal, we can also set the tolerance. We are aware that our PID is not incredibly accurate, which could lead to MoveIt! believing that the goal hasn't been achieved. Changing the goal tolerance makes the system achieve the waypoints with a higher margin of error in order to account for inaccuracies in the control:

plan_group.setGoalTolerance(0.2);

Finally, we just need to set the planning group target pose, which will then be planned and executed by the snippet of code shown at the beginning of this section:

plan_group.setPoseTarget(goal);

We can run this snippet of code with the following command; the arm should position itself without any issues:

$ rosrun rosbook_arm_snippets move_group_plan_single_target

Planning a random target can be effectively performed in two steps: first of all, we need to create the random target itself and then check its validity. If the validity is confirmed, then we can proceed by requesting the goal as usual; otherwise, we will cancel (although we could retry until we find a valid random target). In order to verify the validity of the target, we need to perform a service call to a service provided by MoveIt! for this specific purpose. As usual, to perform a service call, we need a service client:

ros::ServiceClient validity_srv =  nh.serviceClient<moveit_msgs::GetStateValidity>("/check_state_validity");

Once the service client is set up, we need to create the random target. To do so, we need to create a robot state object containing the random positions, but to simplify the process, we can start by acquiring the current robot state object:

robot_state::RobotState current_state = *plan_group.getCurrentState();

We will then set the current robot state object to random positions, but to do so, we need to provide the joint model group for this robot state. The joint model group can be obtained using the already created robot state object as follows:

current_state.setToRandomPositions(current_state.getJointModelGroup("arm"));

Up to this point, we have a service client waiting to be used as well as a random robot state object, which we want to validate. We will create a pair of messages: one for the request and another for the response. Fill in the request message with the random robot state using one of the API conversion functions, and request the service call:

moveit_msgs::GetStateValidity::Request validity_request;
moveit_msgs::GetStateValidity::Response validity_response;

robot_state::robotStateToRobotStateMsg(current_state, validity_request.robot_state);
validity_request.group_name = "arm";

validity_srv.call(validity_request, validity_response);

Once the service call is complete, we can check the response message. If the state appears to be invalid, we would simply stop running the module; otherwise, we will continue. As explained earlier, at this point, we could retry until we get a valid random state; this can be an easy exercise for the reader:

if (!validity_response.valid)
{
  ROS_INFO("Random state is not valid");
  ros::shutdown();
  return 1;
}

Finally, we will set the robot state we just created as the goal using the planning group interface, which will then be planned and executed as usual by MoveIt!:

plan_group.setJointValueTarget(current_state);

We can run this snippet of code with the following command, which should lead to the arm repositioning itself on a random configuration:

$ rosrun rosbook_arm_snippets move_group_plan_random_target

As we commented during the configuration generation step, when initially integrating our robotic arm, MoveIt! provides the concept of predefined group states, which can later be used to position the robot with a predefined pose. Accessing predefined group states requires creating a robot state object as a target; in order to do so, the best approach is to start by obtaining the current state of the robotic arm from the planning group interface:

robot_state::RobotState current_state = *plan_group.getCurrentState();

Once we have obtained the current state, we can modify it by setting it to the predefined group state, with the following call, which takes the model group that needs to be modified and the name of the predefined group state:

current_state.setToDefaultValues(current_state.getJointModelGroup("arm"), "home");

Finally, we will use the new robot state of the robotic arm as our new goal and let MoveIt! take care of planning and execution as usual:

plan_group.setJointValueTarget(current_state);

We can run this snippet of code with the following command, which should lead to the arm repositioning itself to achieve the predefined group state:

$ rosrun rosbook_arm_snippets move_group_plan_group_state

MoveIt! provides a set of messages that can be used to communicate visualization information, essentially providing it with the planned path in order to get a nice visualization of how the arm is going to move to achieve its goal. As usual, communication is performed through a topic, which needs to be advertised:

ros::Publisher display_pub = nh.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);

The message we need to publish requires the start state of the trajectory and the trajectory itself. In order to obtain such information, we always need to perform planning using the planning group interface first, and using the created plan, we can proceed to fill in the message:

moveit_msgs::DisplayTrajectory display_msg;
display_msg.trajectory_start = goal_plan.start_state_;
display_msg.trajectory.push_back(goal_plan.trajectory_);
display_pub.publish(display_msg);

Once the message has been filled in, publishing it to the correct topic will cause the RViz visualization to show the trajectory that the arm is about to perform. It is important to take into account that, when performing a call to plan, it will also show the same type of visualization, so you shouldn't be confused if the trajectory is displayed twice.