Bootstrap

MoveIt2——8.运动规划API

运动规划API

在这里插入图片描述

‎在 MoveIt 中，使用插件基础结构加载运动规划器。这允许 MoveIt 在运行时加载运动规划器。在此示例中，我们将运行执行此操作所需的C++代码。‎

运行示例

‎打开两个终端。在第一个终端中，启动 RViz 并等待所有内容完成加载：‎

ros2 launch moveit2_tutorials move_group.launch.py

在第二个终端中，运行launch文件

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py

‎注：‎‎本教程使用 ‎‎RvizVisualToolsGui‎‎ 面板逐步完成演示。要将此面板添加到 RViz，请按照‎‎可视化教程‎‎中的说明进行操作。‎

片刻之后，RViz 窗口应会出现，并且看起来与此页顶部的窗口类似。要继续完成每个演示步骤，请按屏幕底部 RvizVisualToolsGui 面板中的“下一步”按钮，或在屏幕顶部的“Tools”面板中选择“Key Tool”，然后在 RViz 处于活动状态时按键盘上的 N 键。

期望输出

在RViz中我们最终应该能看到四条轨迹被播放

  1. ‎机械臂移动到第一个姿势目标，‎在这里插入图片描述
  2. ‎机器人将其手臂移动到关节目标，‎在这里插入图片描述
  3. ‎机械臂移回原始姿势目标，‎
  4. ‎机械臂移动到新的姿势目标，同时保持末端执行器水平。‎在这里插入图片描述

完整代码

开始

‎设置以开始使用规划器非常简单。规划器在MoveIt中设置为插件，您可以使用ROS pluginlib接口加载要使用的任何规划器。在加载规划器之前，我们需要两个对象，一个是 RobotModel，另一个是 PlanningScene。我们将首先实例化一个‎‎RobotModelLoader‎‎对象，该对象将在ROS参数服务器上查找机器人描述，并构建一个‎‎RobotModel‎‎供我们使用。‎

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

‎使用‎‎RobotModel‎‎，我们可以构建一个维护世界状态（包括机器人）的‎‎PlanningScene‎‎。‎

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

配置有效的机器人状态

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

现在，我们将构造一个加载程序来按名称加载规划器。请注意，我们在这里使用的是 ROS pluginlib 库。

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

我们将从ROS参数服务器获取要加载的规划插件的名称，然后加载规划器以确保捕获所有异常。

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

可视化

MoveItVisualTools功能包提供了许多功能，用于在RViz中可视化对象，机器人和轨迹，以及调试工具，例如脚本的分步内省。

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

目标姿态

‎现在，我们将为 Panda 臂创建一个运动规划请求，指定末端执行器的所需姿势作为输入。‎

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;

‎在位置上指定误差范围为 0.01 m，在方向上误差范围为 0.01 弧度‎

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

‎我们将使用‎‎kinematic_constraints‎‎包中提供的帮助器函数将请求创建为约束。‎

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

现在，我们构建一个规划上下文，用于封装场景、请求和响应。我们使用此规划上下文调用规划器

可视化结果

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

显示目标姿态

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

关节空间姿态

现在设置一个关机空间目标

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

唤起规划器并可视化轨迹

/* 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());

显示目标姿态

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

显示目标姿态

visual_tools.trigger();

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

添加路径约束

让我们再次添加一个新的目标姿态。这次我们也将为运动添加一个路径约束

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

‎施加路径约束要求规划器在末端执行器（机器人的工作空间）的可能位置空间中进行推理，因此，我们还需要为允许的计划量指定一个边界;注意：默认绑定由 WorkspaceBounds 请求适配器（OMPL 管道的一部分，但在此示例中未使用）自动填充。我们使用的边界绝对包括手臂的可触及空间。这很好，因为在规划机械臂时，不会在此体积中进行采样;边界仅用于确定采样的配置是否有效。‎

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;

唤起规划器并可视化目前为止创建的所有的规划

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

显示目标姿态

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文件

‎整个启动文件‎‎在此处‎‎GitHub上。本教程中的所有代码都可以从moveit_tutorials包进行编译和运行。

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
ES6中js的运算符（ ?. ?? ）
论文阅读——Wireless Identification and Sensing Platform Version 6.0
书上秋千图和相关的思考
优橙国企招聘会—5G网络优化工程师
Kotlin语言基础入门：Kotlin简介
C++炸弹小游戏
2020人脸识别报告：上万家企业入局，八大技术六个趋势一文看尽
基于Azure Kinect DK相机的安装配置，获取并保存RGB、Depth、IR图、点云，点云融合（Windows）
webpack配置完全指南
MySql系列之设置Mysql允许其他电脑访问
;