Moving the robot with MoveIt
In this session MoveIt will be used to plan and execute the movement of the Fanuc robot.
To better understand, how the moveit framework works, it is recomended to read through the concepts that are well documented at moveit.ros.org/documentation/concepts/.
In MoveIt, the simplest user interface is through the MoveGroupInterface class. It provides easy to use functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot. This interface communicates over ROS topics, services, and actions to the MoveGroup Node.
Moving into a pose
The objective of this exercise is to create a ROS node that will be able to move the gripper of the robot to a predefined position.
Preparation of the package
To start, create a package fanuc_manipulation in the fanuc_ros workspace with the following dependencies: roscpp, std_msgs, stf2_eigen, moveit_visual_tools, moveit_ros_planning, and moveit_ros_planning_interface. Then create a file move_to_pose.cpp in the new package.
Working in the terminal the process looks like this:
cd ~/rosinljubljana/fanuc_ros/src
catkin_create_pkg fanuc_manipulation roscpp std_msgs tf2_eigen moveit_visual_tools moveit_ros_planning moveit_ros_planning_interface
cd fanuc_manipulation/src
touch move_to_pose.cpp
Writing the code
To interact with the robot, the Move Group interface will be used and for visualization of trajectories and poses in Rviz the MoveIt Visual Tools library will be added. These two libraries need to be included at the beginning of the file.
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
Next, the skeleton of the code for the node is defined:
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// Place code here.
ros::shutdown();
return 0;
}
The above code handles the startup and shutdon of the node. The code for the functionality of the node will be placed into the main function.
MoveIt operates on sets of joints called “planning groups” and stores them in an object called the JointModelGroup.
In our case, we will use move_base with the fanuc_arm planning group. Thisi is the setup part of the code:
// Initialising and defining the planning group for move_base
static const std::string PLANNING_GROUP = "fanuc_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// 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);
Before moving to a pose, the target pose needs to be defined and a plan needs to be calculated.
The target pose is specified as a point in space with a position and orientation and represented with the Pose message type. We define the target pose as an arbitrary point from the reachable space of the robot:
// Defining target pose
geometry_msgs::Pose target_pose;
target_pose.position.x = 0.3;
target_pose.position.y = 0.2;
target_pose.position.z = 0.4;
target_pose.orientation.y = sqrt(2)/2;
target_pose.orientation.w = sqrt(2)/2;
move_group.setPoseTarget(target_pose);
ROS_INFO_NAMED("move_to_pose", "Setting the target position to x=%g, y=%g, z=%g",target_pose.position.x, target_pose.position.y, target_pose.position.z);
The planning is as straightforward as calling the plan method of the move_group object:
// Initialising a new plan and planning
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
Once the plan is successfully generated, it is executed by calling the move method, which is a blocking function that makes the joint controller to execute the motion plan:
// Executing the movement
move_group.move();
Building an executable and running the file
To build an executable add the neccessary declarations to the CMakeLists.txt:
add_executable(move_to_pose src/move_to_pose.cpp)
target_link_libraries(move_to_pose ${catkin_LIBRARIES})
In the terminal build the workspace:
cd ~/rosinljubljana/fanuc_ros
catkin_make
source devel/setup.bash
Open a second terminal and run the demo_gazebo.launch from the fanuc_lrmate200id_moveit_config package:
cd ~/rosinljubljana/fanuc_ros
source devel/setup.bash
roslaunch fanuc_lrmate200id_moveit_config demo_gazebo.launch
In the first terminal run the executable:
rosrun fanuc_manipulation/move_to_pose
Additional taks: create a launch file to start the whole ROS system with one file.
Using MoveIt Visual Tools
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.
We will leverage the capabilities of MoveIt Visual Tools to visualise markers and cotrol the flow of execution of our program. Use the code from move to pose example and save it to a new file in the src folder of the same package called move_to_pose_interactive.cpp. Before running the code, do not forget to edit the CMakeLists.txt file and rebuild the workspace.
To initialise the visual_tools object, clear any existing markers, and enable remote control add:
// Initialisation of Rviz Visual Tools
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
// Clear existing markers
visual_tools.deleteAllMarkers();
// Pre-load remote control
visual_tools.loadRemoteControl();
After initialisation, to and add a visual marker displaying the title of the tutorial add:
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1;
visual_tools.publishText(text_pose, "MoveGroupInterface Tutorial", rviz_visual_tools::LIME_GREEN, rviz_visual_tools::XLARGE);
visual_tools.trigger();
Insert the following code in the workflow where you want to pause the execution of the program and wait for user input to continue (e.g. before executing the movement):
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue");
To display the markers and interact with the program the following modifications of the Rviz configuration might be required:
- add the Marker Array display and set the Marker Topic to
/rviz_visual_tools - add the RvizVisualToolsGui pannel (Panels > Add New Panel > RvizVisualToolsGui)
Adjust the following settings according to personal preference:
- visualisation of the planning goal state: check/unchek Displays - MotionPlanning - Planning Request - Query Goal State
- visualisation of the trail of the planned trajectory: check/uncheck Displays - MotionPlanning - Planned Path - Show Trail
Suggestion: Save the current Rviz config to file for future reuse.
Moving the joints of the robot
MoveGroup interface enables also to move the joints directly. Below is the code for a program that creates and executes a plan for a goal in the joint-space. The goal is defined by taking current joints states of the planning group and changing the value for one of the joints.
Save the following code to a new file named move_joints.cpp in the same package and rebuild the workspace:
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// Initialising and defining the planning group for move_base
static const std::string PLANNING_GROUP = "fanuc_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// Raw pointers are frequently used to refer to the planning group for improved performance.
const moveit::core::JointModelGroup *joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// A pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
// Get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// Modifing one of the joint positions
joint_group_positions[0] = 1.0; // radians
// Pass the desired joint positions to move_group as goal for planning
move_group.setJointValueTarget(joint_group_positions);
// We lower the allowed maximum velocity and acceleration to 5% of their maximum.
// The default values are 10% (0.1).
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
ros::shutdown();
return 0;
}
Following cartesian paths
To move the robot to desired pose or joint position the pair od methods plan() - move() is used. However, to follow the cartesian path the computeCartesianpath() is used for conputing the path from the array of poses and the exectution of the movement is done by using the execute() method.
Use the follofing code for cartesian movement example:
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_cartesian");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "fanuc_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
const robot_state::JointModelGroup *joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 0.8;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
ROS_INFO_NAMED("move_cartesian", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("move_cartesian", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
//setting waypoints
std::vector<geometry_msgs::Pose> waypoints;
robot_state::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::Pose start_pose;
start_pose.orientation.y = sqrt(2) / 2;
start_pose.orientation.w = sqrt(2) / 2;
start_pose.position.x = 0.3;
start_pose.position.y = 0.15;
start_pose.position.z = 0.3;
waypoints.push_back(start_pose);
geometry_msgs::Pose target_pose = start_pose;
target_pose.position.y -= 0.15;
target_pose.position.z -= 0.15;
waypoints.push_back(target_pose); // down right
target_pose.position.y -= 0.15;
target_pose.position.z += 0.15;
waypoints.push_back(target_pose); // up right
target_pose.orientation.y = 0;
target_pose.orientation.w = 1;
target_pose.position.y += 0.15;
target_pose.position.z += 0.15;
waypoints.push_back(target_pose); // up left
target_pose.position.y += 0.15;
target_pose.position.z -= 0.15;
waypoints.push_back(target_pose); // down left
//planning
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("move_cartesian", "Visualizing plan through waypoints (%.2f%% acheived)", fraction * 100.0);
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Waypoints", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::MAGENTA, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::MEDIUM);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue with path execution");
//path execution
move_group.execute(trajectory);
visual_tools.prompt("Press 'next' to exit the program");
ros::shutdown();
return 0;