Moveit2 - 8. Motion planning API

Sports planning API

‎ stay MoveIt in , Use the plug-in infrastructure to load the motion planner . This allows MoveIt Load the motion planner at runtime . In this example , We will run the C++ Code .‎

Run the example

‎ Open both terminals . In the first terminal , start-up RViz And wait for all contents to finish loading ：‎

ros2 launch moveit2_tutorials move_group.launch.py

In the second terminal , function launch file

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py

‎ notes ：‎‎ This tutorial USES ‎‎RvizVisualToolsGui‎‎ The panel completes the demonstration step by step . To add this panel to RViz, Please follow ‎‎ Visualization tutorial ‎‎ Operate as described in .‎

A moment later ,RViz The window should appear , And it looks similar to the window at the top of this page . To continue to complete each demonstration step , Please press the bottom of the screen RvizVisualToolsGui In the panel “ next step ” Button , Or at the top of the screen “Tools” Select... In the panel “Key Tool”, And then in RViz When active, press N key .

Expected output

stay RViz We should finally be able to see four tracks played

1. ‎ The robot arm moves to the first pose target ,‎
2. ‎ The robot moves its arm to a joint target ,‎
3. ‎ The robot arm moves back to the original pose target ,‎
4. ‎ The robot arm moves to a new pose target , At the same time, keep the end actuator horizontal .‎

Complete code

Start

‎ Setting up to start using the planner is very simple . Planner in MoveIt Set as plug-in , You can use ROS pluginlib Interface loads any planners to use . Before loading the planner , We need two objects , One is RobotModel, The other is PlanningScene. We will first instantiate a ‎‎RobotModelLoader‎‎ object , The object will be in ROS Find the robot description on the parameter server , And build a ‎‎RobotModel‎‎ For our use .‎

const std::string PLANNING_GROUP = "panda_arm";
robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_api_tutorial_node, "robot_description");
const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();
/* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model));
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

‎ Use ‎‎RobotModel‎‎, We can build a system to maintain the state of the world （ Including robots ） Of ‎‎PlanningScene‎‎.‎

planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

Configure valid robot status

planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

Now? , We will construct a loader to load the planner by name . Please note that , What we use here is ROS pluginlib library .

std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;

We will start from ROS Parameter the server gets the name of the planning plug-in to be loaded , Then load the planner to ensure that all exceptions are caught .

if (!motion_planning_api_tutorial_node->get_parameter("planning_plugin", planner_plugin_name))
  RCLCPP_FATAL(LOGGER, "Could not find planner plugin name");
try
{
    
  planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
      "moveit_core", "planning_interface::PlannerManager"));
}
catch (pluginlib::PluginlibException& ex)
{
    
  RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
}
try
{
    
  planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
  if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node,
                                    motion_planning_api_tutorial_node->get_namespace()))
    RCLCPP_FATAL(LOGGER, "Could not initialize planner instance");
  RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance->getDescription().c_str());
}
catch (pluginlib::PluginlibException& ex)
{
    
  const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
  std::stringstream ss;
  for (const auto& cls : classes)
    ss << cls << " ";
  RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(),
               ex.what(), ss.str().c_str());
}

moveit::planning_interface::MoveGroupInterface move_group(motion_planning_api_tutorial_node, PLANNING_GROUP);

visualization

MoveItVisualTools The feature pack provides many functions , Used in RViz Visualization objects in , Robots and trajectories , And debugging tools , For example, step-by-step introspection of scripts .

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(motion_planning_api_tutorial_node, "panda_link0",
                                                    "move_group_tutorial", move_group.getRobotModel());
visual_tools.enableBatchPublishing();
visual_tools.deleteAllMarkers();  // clear all old markers
visual_tools.trigger();

/* 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::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);

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

/* We can also use visual_tools to wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

Target attitude

‎ Now? , We will do for Panda Arm creates a motion planning request , Specify the desired pose of the end effector as input .‎

visual_tools.trigger();
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "panda_link0";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.4;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;

‎ Specify an error range of 0.01 m, The error range in direction is 0.01 radian ‎

std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);

‎ We will use ‎‎kinematic_constraints‎‎ The helper function provided in the package creates the request as a constraint .‎

moveit_msgs::msg::Constraints pose_goal =
    kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);

req.group_name = PLANNING_GROUP;
req.goal_constraints.push_back(pose_goal);

Now? , We build a planning context , Used to encapsulate scenes 、 Requests and responses . We use this planning context to call the planner

Visualization results

std::shared_ptr<rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisher =
    motion_planning_api_tutorial_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",
                                                                                             1);
moveit_msgs::msg::DisplayTrajectory display_trajectory;

/* Visualize the trajectory */
moveit_msgs::msg::MotionPlanResponse response;
res.getMessage(response);

display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* Set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

Display the target attitude

visual_tools.publishAxisLabeled(pose.pose, "goal_1");
visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* We can also use visual_tools to wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

Joint space posture

Now set a shutdown space target

moveit::core::RobotState goal_state(robot_model);
std::vector<double> joint_values = {
     -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::msg::Constraints joint_goal =
    kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

Evoke the planner and visualize the trajectory

/* Re-construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if (res.error_code_.val != res.error_code_.SUCCESS)
{
    
  RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
  return 0;
}
/* Visualize the trajectory */
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);

/* Now you should see two planned trajectories in series*/
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* We will add more goals. But first, set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

Display the target attitude

visual_tools.publishAxisLabeled(pose.pose, "goal_2");
visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

/* Now, we go back to the first goal to prepare for orientation constrained planning */
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);

display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* Set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

Display the target attitude

visual_tools.trigger();

/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

Add path constraint

Let's add a new target pose again . This time we will also add a path constraint to the motion

/* Let's create a new pose goal */

pose.pose.position.x = 0.32;
pose.pose.position.y = -0.25;
pose.pose.position.z = 0.65;
pose.pose.orientation.w = 1.0;
moveit_msgs::msg::Constraints pose_goal_2 =
    kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);

/* Now, let's try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);

/* But, let's impose a path constraint on the motion. Here, we are asking for the end-effector to stay level*/
geometry_msgs::msg::QuaternionStamped quaternion;
quaternion.header.frame_id = "panda_link0";
req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);

‎ Applying path constraints requires the planner to execute at the end （ Robot workspace ） Reasoning in the possible location space of , therefore , We also need to specify a boundary for the allowable planning quantity ; Be careful ： The default binding is WorkspaceBounds Request adapter （OMPL Part of the pipeline , But... Is not used in this example ） Auto fill . The boundary we use absolutely includes the accessible space of the arm . This is good , Because when planning the manipulator , No samples will be taken in this volume ; The boundary is only used to determine whether the configuration of sampling is valid .‎

req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
    req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
    req.workspace_parameters.max_corner.z = 2.0;

Invoke the planner and visualize all the plans created so far

context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher->publish(display_trajectory);

/* Set the state in the planning scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

Display the target attitude

visual_tools.publishAxisLabeled(pose.pose, "goal_3");
visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

launch file

‎ The entire startup file ‎‎ stay here ‎‎GitHub On . All the code in this tutorial can be downloaded from moveit_tutorials Package to compile and run .