首页 > 其他分享 >ROS2- Moveit2 - 运动规划API(Motion Planning API)

ROS2- Moveit2 - 运动规划API(Motion Planning API)

时间：2024-09-02 14:16:22浏览次数：4  
标签：planning req trajectory pose Motion visual Planning tools API

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

入门

如果您还没有这样做，请确保您已经完成入门指南中的步骤。

 

运行演示

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

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py

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

片刻之后，RViz 窗口应会出现，其外观与本页顶部的窗口类似。要完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的“下一步”按钮，或选择屏幕顶部“工具”面板中的“ Key Tool”，然后在 RViz 处于焦点时按键盘上的N 键。

 

预期输出

在 RViz 中，我们最终应该能够看到四条轨迹被重播：

机器人将手臂移动到第一个姿势目标，

一个

 

机器人将手臂移动到关节目标，

 3.  机器人将手臂移回原来的姿势目标

 4. 机器人将其手臂移动到新的姿势目标，同时保持末端执行器的水平。

完整代码

整个代码可以在 moveit_tutorials GitHub 项目中看到。

开始

设置开始使用规划器非常简单。规划器在 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::vector<std::string> planner_plugin_names;
我们将从 ROS 参数服务器获取想要加载的规划插件的名称，然后加载规划器，确保捕获所有异常。  
if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names))
  RCLCPP_FATAL(LOGGER, "Could not find planner plugin names");
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());
}

if (planner_plugin_names.empty())
{
  RCLCPP_ERROR(LOGGER,
               "No planner plugins defined. Please make sure that the planning_plugins parameter is not empty.");
  return -1;
}

const auto& planner_name = planner_plugin_names.at(0);
try
{
  planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_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_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");

姿势目标

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

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 米，方向公差为 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);

定义工作空间界限

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

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

planning_interface::PlanningContextPtr context =
    planner_instance->getPlanningContext(planning_scene, req, res.error_code);

if (!context)
{
  RCLCPP_ERROR(LOGGER, "Failed to create planning context");
  return -1;
}
context->solve(res);
if (res.error_code.val != res.error_code.SUCCESS)
{
  RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
  return -1;
}

可视化结果

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

启动文件

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

标签：planning,req,trajectory,pose,Motion,visual,Planning,tools,API
From： https://www.cnblogs.com/ai-ldj/p/18392619

相关文章

  • 一个练习项目，好玩的bbs-python-fastapi
    代码：fromfastapiimportFastAPI,Response,Cookie,Dependsfromfastapi.responsesimportJSONResponsefromfastapi.responsesimportHTMLResponseimportos.pathimportMySQLdbimportjsonimporthashlibimportrandomimportmathimportosfromdatetimeim......
  • 如何使用 Python 调用 DPAPI ？
    在Windows环境下，DPAPI（DataProtectionAPI）是一种用于加密和解密数据的API，可以保护数据，使其只能由当前用户或计算机访问。在Python中，可以通过Cryptography或pywin32等库来使用DPAPI进行数据加密和解密。以下是我我做项目时使用Python调用DPAPI进行数据加密和解密的示......
  • ROS2- Moveit2 -Planning Scene ROS API (规划场景 ROS API)
    在本教程中，我们将研究如何使用规划场景差异来执行两项操作：在世界中添加和移除物体将物体安装到机器人上或从机器人上卸下入门如果您还没有这样做，请确保您已经完成入门指南中的步骤。运行代码打开两个shell。在第一个shell中启动RViz并等待所有内容完成加载：ros......
  • Ros2-Moveit2-PlanningSceneMonitor(规划场景监控)
    PlanningSceneMonitor是维护最新规划场景的推荐接口。RobotState、CurrentStateMonitor、PlanningScene、PlanningSceneMonitor和PlanningSceneInterface之间的关系一开始可能非常令人困惑。本教程旨在阐明这些关键概念。机器人状态RobotState是机器人的快照。它包含RobotMod......
  • 电影票小程序API接口对接方式注册和申请接入流程
    电影票API接口对接方式注册和申请接入：注册成为合作伙伴或开发者：在API提供商处注册账号，并按照其指引填写相关信息，申请API密钥和相关文档 。了解使用条款和限制：明确API提供商的使用条款，包括请求频率限制、数据使用权限、隐私政策、版权信息、计费规则等，确保在使用过......
  • 鸿蒙跨端实践-ArkTS和CAPI的混合开发实现
    一、背景在动态化-鸿蒙跨端方案文章中，讲述了动态化适配鸿蒙的方案实现，当在鸿蒙系统进行UI渲染的时候，我们使用了系统的组件进行递归渲染。在iOS和Android也是借助各自系统组件进行的渲染，但是在鸿蒙系统会存在以下4个严重问题：1.UI层级过多以金融APP理财频道页中的一个乐高楼层中的“......
  • 教会小白如何使用淘宝API接口获取商品数据
    随着互联网的普及，越来越多的人开始接触网络购物，而淘宝作为中国最大的电商平台之一，成为了众多消费者首选的购物平台。然而，对于一些小白用户来说，如何通过淘宝API接口获取商品数据可能是一个难题。本文将详细介绍如何教会小白使用淘宝API接口获取商品数据。一、了解淘宝API接口淘宝......
  • .Net 5.0 WebAPI 发布至 Linux 系统
    阅读目录〇、前言一、Linux环境准备1.1CentOS7.x在线安装.net5.01.2CentOS8.x在线安装.net5.01.3查看是否安装成功二、示例项目创建和发布2.1创建一个测试项目2.2发布步骤三、服务开启和配置自启动3.1服务开启3.2将服务配置为系统自启动......
  • 如何利用淘宝商品评论API返回值进行竞品分析
    利用淘宝商品评论API的返回值进行竞品分析是一个涉及数据处理、文本分析和商业洞察的过程。由于淘宝不直接提供公开的商品评论API给普通开发者使用，这里我们假设你通过某种方式（如合作伙伴关系、数据服务提供商或合法爬虫技术但遵守相关法律法规和淘宝的服务条款）获取了商品评论数据。......
  • 2024最新股票实时交易数据API接口已经全部验证可用
    经过多次实测，已经验证下面的数据接口均可使用，我已经做好了超链接，直接点击就能获取相应数据，可以马上验证接口的有效性，现在分享给大家，希望可以能够帮助到做量化分析的朋友们。实时交易数据使用说明：1、下方所有API接口连接均可直接点击打开，在浏览器中即可获取返回的数据；2、......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99