爱折腾-创客智造实验室 - ros2与moveit2入门教程
说明： 介绍ROS2与MoveIt2的目录 目录： ROS2与MoveIt2入门教程-MoveIt2简介 ROS2与MoveIt2入门教程-MoveIt2软件安装 ROS2与MoveIt2入门教程-MoveIt2基本概念 ROS2与MoveIt2入门教程-MoveIt2中可用的规划器 ROS2与MoveIt2入门教程-插件接口 ROS2与MoveIt2入门教程-在RViz中快速上手MoveIt2 ROS2与MoveIt2入门教程-Move Group C++接口 ROS2与MoveIt2入门教程-机器人模型和机器人状态 ROS2与MoveIt2入门教程-规划场景 ROS2与MoveIt2入门教程-规划场景监视器 ROS2与MoveIt2入门教程-规划场景ROS API ROS2与MoveIt2入门教程-MoveItCpp教程 ROS2与MoveIt2入门教程-URDF与SRDF ROS2与MoveIt2入门教程-实时机械臂伺服
说明： 介绍MoveIt2 MoveIt2简介 MoveIt2是专门为ROS 2从MoveIt迁移过来、主要用于机器人机械手或机械臂的运动规划软件框架，有助于将机器人带入未来。该框架综合了运动规划、机械（手）臂操纵、3D感知、运动学、控制和导航等方面的最新成果 MoveIt2的主要功能 该软件框架的主要功能包括： 运动规划 生成可顺利通过杂乱环境的高自由度轨迹 生成的轨迹或者路径可以避免（逃离）局部最小值 机械臂操纵 通过抓取生成（grasp generation）对机器人环境进行分析，并与该环境进行交互 逆运动学（Inverse Kinematics） 求解给定位姿的关节位置，甚至可用于过度制动的机械臂 控制 通过通用接口，对底层硬件控制器执行时间参数化的关节轨迹 3D感知 通过Octomaps，可以连接深度传感器和点云 碰撞检测 使用几何图元、网格或点云避开障碍物 MoveIt2的主要特征 该软件框架的主要特征有： 拥有3D交互式可视化工具Rviz，功能十分强大 该工具开箱即用的视觉展示功能允许新用户针对障碍物尝试各种避障规划算法。而在物理硬件上执行这些算法只需单击一下即可。 可充分利用仿真工具Gazebo，技术处于前沿 MoveIt通过充分利用基于物理的仿真工具，可以加速开发和测试周期。组合Gazebo、ROS Control和MoveIt，可以打造强大的机器人开发平台。 具有向导式配置助手，使用非常容易 通过使用逐步配置向导或者使用普遍的预配置设置，可以快速设置任何机器人以使用MoveIt。还可以配置Gazebo和ROS Control。 具有任务构造器，技术十分先进 可以一种灵活透明的方式来定义和规划由多个相互依赖的子任务组成的动作（actions）。 利用抓取生成技术，具有智能化特征 拥有基于几何和机器学习的抓取生成库，可用于MoveIt的机器人拾取和放置任务管道。 为什么要使用MoveIt2？ MoveIt是使用最广泛的机械臂操纵软件，已在150多种机器人上使用。该软件框架是根据 BSD 许可条款发布的，因此可以免费用于工业、商业和研究用途。 通过综合运动规划、机械臂操纵、3D感知、运动学、控制和导航方面的最新进展，MoveIt是最先进的移动机械臂操纵软件。 当前版本 截至目前（2021年10月），MoveIt2的版本为2.2，包含三个发行版或者分支，即适用于ROS 2 Foxy的Foxy 2.2 LST版本（长期支持版）、适用于ROS 2 Galactic的Galactic 2.2版本（推荐版本）、适用于ROS 2 Rolling的Rolling 2.2版本（开发版）。另外，目前维护的MoveIt版本为适用于ROS 1 Noetic的Noetic 1.1 LST版本 引用MoveIt相关论文 如果在研究中使用了MoveIt软件框架，请在您的出版物中包含以下引文之一： （1）David Coleman, Ioan A. Șucan, Sachin Chitta, Nikolaus Correll, Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study, Journal of Software Engineering for Robotics, 5(1):3–16, May 2014. doi: 10.6092/JOSER_2014_05_01_p3. （2）Ioan A. Sucan and Sachin Chitta, "MoveIt", [Online] Available at moveit.ros.org. （3）M. Görner, R. Haschke, H. Ritter, and J. Zhang, “Moveit! task constructor for task-level motion planning,” in IEEE Intl. Conf. on Robotics and Automation, pp. 190–196, May 2019. doi: 10.1109/ICRA.2019.8793898. 英语原文网址：https://moveit.ros.org/
说明： 介绍如何通过三种方式进行安装：二进制安装、从源代码编译安装、使用MoveIt2 LXD容器进行安装。 步骤： 二进制安装 MoveIt2的二进制安装方法最为简单，但目前仅适用于Ubuntu 20.04上的ROS 2 Foxy、ROS 2 Galactic 和 ROS 2 Rolling这三个发行版。如果机器上已经安装好了ROS 2的上述三个发行版之一，则MoveIt2的二进制安装命令为： sudo apt install ros-foxy-moveit 或者 sudo apt install ros-galactic-moveit 或者 sudo apt install ros-rolling-moveit 这样就可以完成MoveIt2二进制软件包的安装。 从源代码编译安装 目前主要支持在Linux操作系统（具体来说应该是Ubuntu）上从源代码编译安装MoveIt2软件框架 使用MoveIt2 LXD容器进行安装 对许多ROS用户来说，试用ROS 2的最大障碍之一就是需要付出很多努力来设置一切。MoveIt2 Foxy工作空间要求在Ubuntu 20.04操作系统上安装ros-foxy-desktop，而许多用户恰好没有时间或者能力进行这项工作。LXD镜像moveit2-foxy在Ubuntu 20.04环境中提供了一个预编译好的MoveIt2 Foxy工作空间，允许用户立即开始测试和调试。为此，用户需要做的就是下载该LXD镜像，并使用这个一步一步教你如何做的教程所述的gui配置文件启动该镜像。该镜像下载地址为：https://moveit-lxd-images.s3.us-east-2.amazonaws.com/moveit2-foxy.tar.gz 安装并初始化LXD sudo snap install lxd sudo lxd init 下载MoveIt2镜像并使用命令行客户端lxc将其导入到您的LXD寄存器中 lxc image import moveit2-foxy.tar.gz --alias moveit2-foxy 验证所有导入的镜像 lxc image list 应用 LXD GUI 配置文件。为了从LXD容器内部运行RViz或任何其他GUI应用程序，需要通过应用LXD启动配置文件来访问主机X11会话。为此，需要使用以下内容创建一个新文件gui.txt： config: environment.DISPLAY: :0 raw.idmap: both 1000 1000 user.user-data: | #cloud-config runcmd: - 'sed -i "s/; enable-shm = yes/enable-shm = no/g" /etc/pulse/client.conf' - 'echo export PULSE_SERVER=unix:/tmp/.pulse-native | tee --append /home/ubuntu/.profile' packages: - x11-apps - mesa-utils - pulseaudio description: GUI LXD profile devices: PASocket: path: /tmp/.pulse-native source: /run/user/1000/pulse/native type: disk X0: path: /tmp/.X11-unix/X0 source: /tmp/.X11-unix/X0 type: disk mygpu: type: gpu name: gui used_by: 创建一个新的名为gui的容器配置文件： lxc profile create gui cat gui.txt | lxc profile edit gui 启动LXD容器。使用默认配置文件和gui将镜像moveit2-foxy作为容器 moveit2-foxy启动： lxc launch --profile default --profile gui moveit2-foxy moveit2-foxy 具有sudo权限的用户ubuntu在正在运行的容器实例中打开一个bash shell： lxc exec moveit2-foxy -- sudo --user ubuntu --login 通过运行glxgears来验证GUI是否正常工作。如果glxgears不可用或命令失败，则请参阅下面的“GUI故障排除”部分 登录后，就可以在~/ws_ros2中找到预编译好的和源ROS2工作空间目录。现在已经准备就绪，可以开始运行MoveIt2的演示了（MoveItCpp、MoveGroup、MoveIt Servo），例如： ros2 launch run_moveit_cpp run_moveit_cpp.launch.py GUI故障排除 以下是GUI支持不起作用的最可能原因。对于所有修复，可能需要重新启动容器实例，甚至重新应用gui配置文件。 错误的显示设备 验证主机和容器的$DISPLAY是否设置为相同的值。默认值为“:0”，如果主机使用不同的显示器，则需要将相同的值应用到您的gui 配置文件中。 缺少相关软件包 在某些情况下，cloud-init无法安装必要的软件包mesa-utils、x11-apps和pulseaudio。可以简单地从容器shell内手动添加它们： sudo apt install mesa-utils x11-apps pulseaudio 英伟达故障排除 在某些 Nvidia 系统上，容器中可能无法访问OpenGL。在这种情况下，最简单的解决方案是简单地安装与主机系统上安装的NVidia 版本相同的库，如下所示： 在您的主机系统中，检查当前安装的驱动程序版本： apt list --installed nvidia-driver-* -> nvidia-driver-440/... 在您的容器实例中安装GL库： sudo apt install libnvidia-gl-440 也可以将gl库添加到config/user.user-data/packages下的lxc gui配置文件中，只需要确保始终使用与您的主机系统相匹配的版本。 英语原文地址：https://moveit.ros.org/
说明： 介绍MoveIt2基本概念 基本概念： 本节教程是MoveIt2工作原理的概述。有关更具体的文档和详细信息，请参阅MoveIt2教程或面向开发人员的MoveIt2概念。 MoveIt2系统架构 MoveIt2快速高层图解** Move_Group节点 上图显示了MoveIt2提供的名为move_group的主要节点的高级系统架构。该节点在此充当了集成器：将所有单个组件聚集到一起，以提供一组ROS动作和服务供用户使用 用户接口（User Interface） 用户可以通过以下三种方式访问由move_group节点提供的各个动作和服务： 在C++中—使用move_group_interface软件包，该软件包为move_group提供了一个易于设置的C++接口 在Python中—使用moveit_commander软件包 通过GUI—使用Rviz（ROS可视化工具）的Motion Planning插件 节点move_group可以使用ROS参数服务器进行配置，而且该节点还会从ROS参数服务器中获取机器人的URDF和SRDF。 配置（Configuration） move_group是一个ROS节点。它使用ROS参数服务器来获取以下三类信息： URDF信息—节点move_group会在ROS参数服务器上查找robots_description参数以获取机器人的URDF。 SRDF信息—节点move_group会在ROS参数服务器上查找robots_description_semantic参数以获取机器人的SRDF。SRDF通常是由用户使用MoveIt设置助手（Setup Assistant）一次性创建的。 MoveIt其它配置信息—节点move_group会在ROS参数服务器上查找特定于MoveIt的其它配置信息，包括关节限制、运动学、运动规划、感知和其它信息。这些组件的配置文件由MoveIt设置助手自动生成，并存储在机器人对应的MoveIt配置软件包的config目录中。 机器人接口（Robot Interface） 节点move_group通过ROS话题和动作与机器人进行对话。该节点与机器人通信以获取机器人的当前状态信息（关节位置等），从机器人传感器上获取点云和其他传感器数据，并与机器人上的控制器进行对话。 关节状态信息 节点move_group侦听/joint_states话题以确定当前状态信息，即确定机器人的每个关节位于何处。节点move_group能够同时侦听多个该话题发布者发布的信息，即使这些发布者只发布有关机器人状态的部分信息（例如，单独的发布者可能用于发布机器人的手臂和移动底座的状态信息）。请注意，节点move_group不会设置自己的关节状态发布者—这是必须在每个机器人上实现的任务。 坐标变换信息 节点move_group使用ROS的TF库监控坐标变换信息。这允许节点获取有关机器人位姿的全局信息（以及其它信息）。例如，ROS 导航堆栈会将机器人的地图坐标系和底座坐标系之间的坐标变换发布到TF上。节点move_group就可以使用TF来计算内部使用的这种坐标变换。注意节点move_group只侦听TF。要从您的机器人发布TF信息，您将需要在您的机器人上运行一个robots_state_publisher节点。 控制器接口 节点move_group使用FollowJointTrajectoryAction接口与机器人上的控制器进行对话。这是一个ROS 动作接口。机器人上的服务器需要为这个动作提供服务——这个服务器不是由move_group节点本身提供的。节点move_group只会实例化一个客户端来与你机器人上的这个控制器动作服务器进行对话。 规划场景（Planning Scene） 节点move_group使用规划场景监视器（Planning Scene Monitor）来维护一个规划场景，规划场景是机器人世界和机器人当前状态的表示。机器人状态可以包括连接到机器人（由其携带）的任何物体，这些物体被认为是刚性连接到机器人上的。在下面的规划场景部分将概述有关用于维护和更新规划场景的体系结构的更多详细信息。 可扩展的功能 节点move_group的结构易于扩展—诸如拾取和放置、运动学、运动规划等单个功能实际上是作为具有一个公共基类的单独插件实现的。这些插件可以使用ROS通过一组ROS yaml参数和通过使用ROS pluginlib库进行配置。大多数用户不必配置move_group插件，因为这些插件是在MoveIt设置助手生成的启动文件中自动配置的。 动规划 运动规划插件 MoveIt2通过插件界面与运动规划器协同工作。这允许MoveIt2与来自多个库的不同运动规划器进行通信并使用这些运动规划器，从而使MoveIt2易于扩展。通过ROS动作或服务（由move_group节点提供）与运动规划器建立接口。节点move_group的默认运动规划器是由MoveIt设置助手使用OMPL及MoveIt与OMPL的接口进行配置的。默认情况下可用的其他规划器是Pilz工业运动规划器和CHOMP。 运动规划请求 运动规划请求指定您希望运动规划器执行什么操作。通常，您会要求运动规划器将手臂移动到不同的位置（在关节空间中）或将末端执行器移动到一个新的位姿。默认情况下会进行碰撞检测（包括自碰撞和连接物体碰撞检测）。您还可以通过Planning_pipeline和planner_id参数指定运动规划器以及运动规划器要检查的约束—由MoveIt2提供的下列内置约束都是运动学约束： 位置约束—将某个链接的位置限制在一个空间区域内 方位约束—将某个链接的方位限制在指定的横滚角、俯仰角或偏航角极限范围内 可见性约束—将某个链接上的一个点限制在位于特定传感器的可见性视锥内 关节约束—将某个关节限制在两个值之间 用户指定的约束—您还可以使用用户定义的回调函数指定自己的约束。 运动规划结果 节点move_group将根据您的运动规划请求生成一条期望的轨迹。该轨迹会将手臂（或任何关节组）移动到期望的位置。请注意，来自move_group节点的运动规划结果是一条轨迹，而不仅仅是一条路径—_move_group*会使用期望的最大速度和加速度（如果指定了的话）来生成一条服从关节级别的速度和加速度约束的轨迹。 运动规划管道（Pipeline）：运动规划器和规划请求适配器 完整的运动规划管道将运动规划器与称为规划请求适配器的其它组件链接在一起。规划请求适配器允许预处理运动规划请求和后处理运动规划响应。预处理在多种情况下很有用，例如当机器人的启动状态稍微超出机器人的指定关节限制时。其他几个操作则需要后处理，例如将为机器人生成的路径转换为时间参数化的轨迹。MoveIt2提供了一组默认的运动规划适配器，每个适配器执行一个很具体的功能 FixStartStateBounds适配器 修复起始状态边界适配器可以将起始状态修复到URDF中指定的关节极限范围内。在未正确配置物理机器人的关节极限的情况下会需要这个适配器。这样机器人的配置可能最终会导致其中一个或多个关节略微超出其关节极限。在这种情况下，运动规划器将无法进行规划，因为它会认为起始状态超出了关节极限。“FixStartStateBounds”规划请求适配器会通过将其移动到关节极限范围内来“修复”该起始状态。但是，这显然不是每次都正确的解决方案—例如关节实际上大大超出了其关节极限。该适配器的一个参数用于指定“可修复”关节可以超出其关节极限的程度。 FixWorkspaceBounds适配器 修复工作空间边界适配器会为规划指定一个默认的工作空间：一个尺寸为10m×10m×10m的立方体。仅当向规划器提出的规划请求未填写这些字段时，才会指定此工作空间。 FixStartStateCollision适配器 修复起始状态碰撞适配器会通过少量扰动关节值来尝试在指定配置（处于碰撞中）附近对新的无碰撞配置进行采样。该适配器扰动的值大小由“jiggle_fraction”参数指定，该参数将扰动控制为关节总运动范围的百分比。此适配器的另一个参数指定该适配器在放弃之前将采样多少随机扰动。 FixStartStatePathConstraints适配器 当运动规划的起始状态不遵守指定的路径约束时，将会应用此适配器。该适配器会尝试在机器人的当前配置和遵守路径约束的新位置之间规划一条路径。新位置将作为规划的起始状态。 AddTimeParameterization适配器 运动规划器通常会生成“运动学路径”，即不服从任何速度或加速度约束并且没有时间参数化的路径。此适配器会通过应用速度和加速度约束来“时间参数化”运动规划。 ResolveConstraintFrames适配器 可以使用子坐标系设置目标约束（例如，杯/手柄cup/handle坐标系中的某个位姿目标，其中手柄handle是物体杯子cup的子坐标系）。此适配器会将约束坐标系更改为某个物体或机器人坐标系（例如杯子）。 开源运动规划库OMPL OMPL（Open Motion Planning Library）是一个开源运动规划库，该库主要实现了随机运动规划器。MoveIt2直接与OMPL集成，并使用该库中的各个运动规划器作为其主要/默认规划器集。OMPL中的规划器是抽象的；即OMPL没有机器人的概念。而MoveIt2会对OMPL进行配置，并为OMPL提供后端以处理机器人问题 规划场景（Planning Scene） 规划场景（Planning Scene）用于表示机器人周边的世界，也存储机器人本身的状态。它由move_group节点内的规划场景监视器（Planning Scene Monitor）维护。规划场景监视器会侦听以下消息： 状态信息：在joint_states话题上侦听此消息 传感器信息：使用下面描述的世界几何监视器侦听此消息 世界几何信息：在planning_scene话题上的用户输入（作为规划场景的一个差分）。 世界几何监视器（World Geometry Monitor） 世界几何监视器使用来自机器人传感器和用户输入的信息构建世界几何体。它使用下面描述的占用地图监视器（occupancy map monitor）来构建机器人周边环境的3D表示，并使用旨在添加对象信息的planning_scene话题上的信息来增强该3D表示。 3D感知 MoveIt2中的3D感知是由占用地图监视器来处理的。占用地图监视器使用插件架构来处理不同类型的传感器输入，如上图所示。具体来说，MoveIt2内置支持以下两种输入数据的处理： 点云：由点云占用地图更新器（point cloud occupancy map updater）插件处理 深度图像：由深度图像占用地图更新器（depth image occupancy map updater）插件处理 请注意，用户可以将自己类型的更新器作为插件添加到占用地图监视器中。 Octomap 占用地图监视器使用Octomap来维护环境的占用地图。Octomap 实际上可以编码有关单个单元格的概率信息，尽管此信息当前还未在MoveIt2中使用。Octomap可以直接传递到MoveIt2使用的碰撞检测库FCL中。 深度图像占用地图更新器 深度图像占用地图更新器包括它自己的自过滤器self-filter，即它会从深度图像地图中删除机器人的可见部分。它使用有关机器人的当前信息（机器人状态）来执行此操作。 运动学（Kinematics） 运动学插件 MoveIt2使用插件基础设施，其特别目标在于允许用户编写自己的逆运动学算法。正运动学和寻找雅可比函数集成在RobotState类本身中。MoveIt2的默认逆运动学插件是使用基于KDL数值雅可比解算器进行配置的。此插件是由MoveIt设置助手自动配置的。 IKFast插件 通常，用户可能会选择实现他们自己的运动学解算器，例如PR2机器人有其自己的运动学解算器。实现此类解算器的一种流行方法就是使用IKFast软件包来生成用于用户特定机器人所需的C++代码 碰撞检测 MoveIt2中的碰撞检测是使用CollisionWorld对象在规划场景中进行配置的。幸运的是，MoveIt2的设置使用户永远不必担心碰撞检测是如何发生的。MoveIt2中的碰撞检测主要使用FCL软件包来执行——该软件包是MoveIt2的主要CC库。 碰撞对象 MoveIt2支持对不同类型的对象进行碰撞检测，包括： 网格（Meshes） 基本形状—例如方盒、圆柱体、圆锥体、球体和平面 Octomap—Octomap对象可以直接用于碰撞检测 允许碰撞矩阵（ACM） 碰撞检测是一项非常昂贵的操作，通常在运动规划期间占用计算费用的近90%。允许碰撞矩阵（Allowed Collision Matrix或ACM）会编码一个二进制值，对应于是否需要对一对物体（可能是在机器人身上或在机器人世界中的物体）之间的碰撞进行碰撞检测。如果在ACM中将两个物体对应的值设置为 1，则表示不需要对这两个物体之间的碰撞进行检测。例如，如果两个物体总是相距很远，以至于它们永远不会相互碰撞，就会出现这种情况。 轨迹处理 时间参数化 运动规划器通常只会生成“路径”，即没有与该路径相关联的时间信息。MoveIt2包括一个轨迹处理程序（trajectory processing routine），可以应用于这些路径上来生成轨迹，这些轨迹为施加在各个关节上的最大速度和加速度限制进行了适当的时间参数化。这些限制是从为每个机器人指定的具体joint_limits.yaml文件中读取的。 参考英语原文地址：https://moveit.ros.org/documentation/concepts/
说明： 介绍在MoveIt2中可用的规划器 MoveIt2中可用的规划器 MoveIt2被设计成可以使用许多不同类型的规划器，非常适合根据以前的方法对改进的规划器进行基准测试。下面列出了已经可以在MoveIt2中使用的规划器，按其在MoveIt2中受欢迎程度/支持程度的降序排列 开源运动规划库（Open Motion Planning Library, OMPL） OMPL是一个开源运动规划库，该库主要实现了随机运动规划器。MoveIt2直接与OMPL集成，并使用该库中的各个运动规划器作为其主要/默认规划器集。OMPL中的规划器是抽象的；即OMPL没有机器人的概念。而MoveIt2会对OMPL进行配置，并为OMPL提供后端以处理机器人问题。该库在MoveIt2中是完全支持的 Pilz工业运动规划器 Pilz工业运动规划器是用于圆周运动和线性运动的确定性生成器。此外，它还支持使用MoveIt2功能将多个运动段混合在一起。 运动规划随机轨迹优化（STOMP） 运动规划随机轨迹优化STOMP（Stochastic Trajectory Optimization for Motion Planning）是一种基于优化的运动规划器，该规划器是基于 PI^2（路径积分策略改进，Theodorou等人，2010）算法之上的。该规划器可以为机械臂规划平滑的轨迹、避开障碍物并优化约束。该算法不需要梯度，因此可以优化成本函数中的任意项，如电机力（motor efforts）。该规划器在MoveIt2中是部分支持的 基于搜索的规划库（SBPL） 是一组通用的运动规划器，这些规划器使用基于搜索的规划方法来离散化空间。该规划库正在集成到最新版本的MoveIt2中 运动规划协变哈密顿优化（CHOMP） 运动规划协变哈密顿优化CHOMP（Covariant Hamiltonian Optimization for Motion Planning）是一种新颖的基于梯度的轨迹优化程序，它使许多日常运动规划问题变得既简单又可训练（Ratliff等人，2009c）。大多数高维运动规划器均将轨迹生成过程分割为规划和优化两个不同的阶段，而此算法在优化阶段利用协变梯度和函数梯度方法来设计完全基于轨迹优化的运动规划算法。在给定一个不可行的初始轨迹的情况下，CHOMP会对周围环境做出反应，以快速将轨迹从碰撞中脱离出来，同时优化诸如关节速度和加速度等动力学量。该算法可以迅速收敛到一条平滑无碰撞的轨迹，该轨迹可以在机器人上高效执行。此规划器正在集成到最新版本的MoveIt2中 参考英语原文地址：https://moveit.ros.org/documentation/planners/
说明: 介绍MoveIt2中不同类型的插件 概述： MoveIt2涉及到多个不同的领域，并提供了许多接口，以便让用户可以覆盖MoveIt2框架的不同部分和实现新的概念，而无需触碰该框架的核心。下面列出了MoveIt2中不同类型的插件，并概述了这些插件在该框架内的用途 MoveGroupCapability接口 用法 通过实现move_group_capability.h接口，可以实现需要频繁执行的功能。每个功能都可以访问move group context，包括例如避免碰撞的障碍物 应用领域 这种类型的插件通常以ros-action或简单的ros-topic的形式创建通信功能，并且可以在move group context上运行。因此，可能的应用非常多样化 接口说明 该接口在move_group_capability.h中定义。该接口主要由一个必须被覆盖的初始化函数组成 具体实现 MoveIt2提供了许多默认功能。可以在此处找到这些默认功能插件的说明。 一个具体的插件示例就是cartesian_path_service_capability。这个具体插件可用于计算笛卡尔路径。其初始化函数的实现如下所示。在这个示例中，该插件会发布一个包含有关轨迹信息的话题，以及一个用于计算新轨迹的服务。 void move_group::MoveGroupCartesianPathService::initialize() { display_path_ = node_handle_.advertise<moveit_msgs::DisplayTrajectory> (planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC, 10, true); cartesian_path_service_ = root_node_handle_.advertiseService(CARTESIAN_PATH_SERVICE_NAME, &MoveGroupCartesianPathService::computeService, this); } KinematicsBase接口 kinematics::KinematicsBase KinematicsBase接口使您能够： 编写您自己的正运动学和逆运动学解算器 封装外部的运动学解算器库 接口说明 具有良好文档的此接口位于此处。 具体实现 KinematicsBase接口的实现示例包括： KDL插件：header, cpp SrvKinematicsPlugin插件: header, cpp PlannerManager接口 planning_interface::PlannerManager 用法 MoveIt2规划器的基类。通过实现该类，可以将自己的规划器用于 MoveIt2中 接口说明 该接口是在planning_interface.h中定义的。 具体实现 MoveIt2这个接口的默认实现是OMPL规划器。 PlanningRequestAdapter接口 planning_request_adapter::PlanningRequestAdapter 此类型插件的概念已经作为“MoveIt2基本概念”文档的一部分进行了说明。 用法 在传递给解算器之前更新/调整运动规划请求，并操纵由该规划器生成的结果运动规划。默认情况下，MoveIt2中会使用以下适配器： AddTimeParameterization：将速度和加速度值添加到基于最大值的一条运动轨迹，用于后期处理 FixWorkspaceBounds：如果未指定，则添加一个规划量 FixStartStateBounds：确保关节编码器报告的关节值不超过关节极限值 FixStartStateCollision：有的时候，根据网格和URDF，关节编码器报告的当前机器人状态会被认为处于碰撞状态—这是由于噪声和建模不准确造成的 FixStartStatePathConstraints：修复违反路径约束的问题，例如在拿着一杯水时抓手的方位 ResolveConstraintFrames：将通过子级坐标系（例如位姿目标cup/handle）定义的约束坐标系变更为物体或机器人坐标系（例如cup） 接口说明 该接口位于这里 具体实现 该规划请求适配器当前是在为每个机器人生成的 ompl_planning_pipeline.launch中指定的。PlanningRequestAdapters的实现示例都可以在这个moveit_ros_planning文件夹中找到 MoveItControllerManager接口 moveit_controller_manager::MoveItControllerManager 用法 MoveIt2不强制要求控制器的实现方式。为了让你的控制器可以在 MoveIt2周使用，需要实现这个接口。此接口的主要目的在于公开一组已知的控制器，并可能允许激活和停用它们（如果有多个控制器可用的话）。通常情况下，可以使用多个控制器来执行一个运动。当有多个控制器可用时，将某个控制器标记为默认控制器会使MoveIt2更喜欢使用该控制器。该管理器还协调两个控制器之间的切换。某个控制器可以是激活的或非激活的。这意味着MoveIt2可以在需要时激活该控制器，并让重叠的控制器（控制同一组关节的控制器）处于非激活状态 具体实现 该接口是在controller_manager.h中定义的。 该接口的一个具体实现可以在这里找到：moveit_simple_controller_manager。 controller_manager.launch应该看起来如下所示： <launch> <rosparam file="$(find xamla_egomo_moveit_config_dev)/config/controllers.yaml"/> </launch> 其中控制器是在“controllers.yaml”文件中定义的。 ControllerHandleAllocator接口 moveit_ros_control_interface::ControllerHandleAllocator 用法 通过实现ControllerHandle.h接口，可以为基于动作的控制器提供处理程序分配。控制器处理程序是基于类MoveItControllerHandle的。 这些处理程序与控制器通信以便执行诸如发送轨迹等任务。 接口说明 该接口是在ControllerHandle.h中定义的。 具体实现 此接口的一个示例性实现是joint_trajectory_controller plugin.cpp。 这也是目前唯一可用的实现 MoveItSensorManager接口 moveit_sensor_manager::MoveItSensorManager 此插件用于将传感器集成到规划过程中。该接口是在sensor_manager.h中定义的。 用法 用于保存传感器的信息，例如：可以执行观察的沿Z轴的最大距离。该管理器可以将传感器指向空间中的某个特定点。这可能需要执行某条轨迹，但可能会或可能不会执行该轨迹。如果没有执行该轨迹，就会将该轨迹作为sensor_trajectory的一部分返回。 这是推荐的行为，因为此函数的调用者可以检查该轨迹的安全性。该函数在成功时会返回true（要么成功完成了执行，要么成功计算了一条轨迹）。 具体实现 待办（TODO）。 ConstraintSamplerAllocator接口 constraint_samplers::ConstraintSamplerAllocator 用法 待办（TODO）。 接口说明 待办（TODO）。 具体实现 待办（TODO）。 CollisionPlugin接口 collision_detection::CollisionPlugin 用于加载自定义碰撞检测机器人/世界的插件API。 用法 namespace my_collision_checker { class MyCollisionDetectorAllocator : public collision_detection::CollisionDetectorAllocatorTemplate<MyCollisionWorld, MyCollisionRobot, MyCollisionDetectorAllocator> { public: static const std::string NAME_; }; const std::string MyCollisionDetectorAllocator::NAME_("my_checker"); } namespace collision_detection { class MyCollisionDetectionLoader : public CollisionPlugin { public: virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const { scene->setActiveCollisionDetector(my_collision_checker::MyCollisionDetectorAllocator::create(), exclusive); return true; } }; 应用领域 如果您想使用自定义的碰撞检测算法进行自碰撞或与环境的碰撞检测，则这是提供必要接口的插件。 接口说明 该接口是在collision_plugin.h中定义的。该接口主要由一个必须被覆盖的初始化函数组成 具体实现 该接口目前尚没有可用的实现。 OccupancyMapUpdater接口 occupancy_map_monitor::OccupancyMapUpdater 用法 待办（TODO）。 接口说明 待办（TODO）。 具体实现 待办（TODO）。 参考英语原文地址：https://moveit.ros.org/documentation/plugins/
说明： 介绍如何在RViz中快速上手MoveIt2 概述： 本教程将通过RViz和MoveIt2插件，快速让您使用MoveIt2进行运动规划。Rviz是ROS中的主要可视化工具，也是调试机器人的非常有用的工具。MoveIt2的Rviz插件允许您设置虚拟环境（场景）、以交互方式为机器人创建起始和目标状态、测试各种运动规划器以及可视化输出。现在让我们开始吧！ 第一步：启动演示和配置插件 启动演示，其命令为： ros2 launch moveit2_tutorials demo.launch.py rviz_tutorial:=true 如果是第一次启动该演示，则应该会在RViz中看到一个空白世界（如下图所示），且必须通过以下步骤添加运动规划插件（Motion Planning Plugin）： 在RViz左侧的“显示（Display）”子窗口的左下角，按下“添加（Add）”按钮； 从弹出的“创建可视化（Create Visualization）”对话框中，点击选择“By display type”选项卡中“moveit_ros_visualization”目录下的“Motion Planning”作为显示类型，按下“确定（Ok）”按钮； 现在应该会在RViz中看见熊猫（Panda）机器人，如下图所示： 一旦加载了运动规划插件（Motion Planning Plugin），就可以对其进行配置。在“显示（Display）”子窗口的“全局选项（Global Options）”选项卡中，将“固定坐标系（Fixed Frame）”字段的值设置为/panda_link0 现在，可以开始为您的机器人（在本例中为 Panda）配置运动规划插件了。点击“显示（Display）”子窗口中的“运动规划（Motion Planning）”项。 确保“机器人描述（Robot Description）”字段值设置为robot_description。 确保“规划场景话题（Planning Scene Topic）”字段值设置为 /monitored_planning_scene。点击话题名称（topic name）以显示话题名称下拉列表。 确保“规划路径（Planned Path）”下的“轨迹话题（Trajectory Topic）”设置为/display_planned_path。 在“规划请求（Planning Request）”中，将“规划组（Planning Group）”更改为panda_arm。这个设置也可以在左下角的“运动规划（MotionPlanning）”面板中看到（如下图所示）。 第二步：玩玩可视化机器人 有四种不同的可重叠可视化： 在/monitored_planning_scene规划环境中的机器人配置（默认处于激状态活）。 机器人的规划路径（默认情况下处于激活状态）。 绿色：运动规划的起始状态（默认情况下是禁用的）。 橙色：运动规划的目标状态（默认情况下处于激活状态）。 可以使用复选框打开和关闭每个可视化的显示状态： 使用“场景机器人（Scene Robot）”选项卡中的“显示机器人视觉（Show Robot Visual）”复选框来打开和关闭规划场景机器人。 使用“规划路径（Planned Path）”选项卡中的“显示机器人视觉（Show Robot Visual）”复选框来打开和关闭规划路径。 使用“规划请求（Planning Request）”选项卡中的“查询起始状态（Query Start State）”复选框来打开和关闭起始状态。 使用“规划请求（Planning Request）”选项卡中的“查询目标状态（Query Goal State）”复选框来打开和关闭目标状态。 使用所有这些复选框来打开和关闭不同的可视化。 第三步：与Panda机器人进行交互 对于接下来的步骤，只需要场景机器人、起始状态和目标状态： 选中“场景机器人（Scene Robot）”选项卡中的“显示机器人视觉（Show Robot Visual）”复选框。 取消选中“规划路径（Planned Path）”选项卡中的“显示机器人视觉（Show Robot Visual）”复选框。 选中“规划请求（Planning Request）”选项卡中的“查询目标状态（Query Goal State）”复选框。 选中“规划请求（Planning Request）”选项卡中的“查询起始状态（Query Start State）”复选框。 现在应该有两个交互标记。橙色手臂对应的标记将用于设置运动规划的“目标状态”，而绿色手臂对应的另一个标记用于设置运动规划的“起始状态”。如果没有看到交互标记，请按下RViz顶部菜单中的“交互（Interact）”按钮（注意：某些工具可能会被隐藏，按下顶部菜单中的“+”按钮来添加“交互（Interact）”工具，如下图所示）。 现在应该能够使用这些标记来拖动手臂转动并改变其方位 进入碰撞状态 请注意一下当您尝试将一个手臂移动到与另一个手臂发生碰撞时会发生什么。发生碰撞的两个链接将会变成红色。 运动规划（MotionPlanning）插件中“规划（Planning）”选项卡下“Collision-aware IK”复选框（下图）允许您切换IK解算器的行为。当勾选该复选框后，IK解算器会继续尝试为期望的末端执行器位姿找到无碰撞的解决方案。当未勾选该复选框时，IK解算器会允许解决方案中发生碰撞。无论该复选框的状态如何，碰撞中的链接都会始终显示为红色。 移出可到达的工作空间 请注意一下当您尝试将末端执行器移出其可到达的工作空间时会发生什么。 移动关节或进入零空间 您可以使用“关节（Joints）”选项卡来移动7-DOF机器人的单个关节和冗余关节。请尝试移动如这个动画中所示的“零空间探索（null space exploration）”滑动条。 第四步：对Panda机器人使用运动规划 现在，可以使用MoveIt2 RViz插件中的熊猫机器人（Panda）开始进行运动规划。 将“起始状态（Start State）”移动到期望的位置。 将“目标状态（Goal State）”移动到另一个期望的位置。 确保这两个状态都没有与机器人本身发生碰撞。 确保“规划路径（Planned Path）”正在被可视化。还要选中“规划路径（Planned Path）”选项卡中的“显示踪迹（Show Trail）”复选框。 在“规划（Planning）”选项卡下的“运动规划（MotionPlanning）”窗口中，按下“规划（Plan）”按钮。这样就应该能够看到手臂移动和踪迹的可视化效果。 检查轨迹航路点 在RViz中可以逐点可视化地检查轨迹。 在“面板（Panels）”菜单中，选择“轨迹（Trajectory）-轨迹滑块（Trajectory Slider）”。这样就会在RViz中看到一个新的滑块面板。 设置目标位姿，然后运行“规划（Plan）”。 玩下“滑块”面板，例如移动该滑块，然后按下“播放（Play）”按钮。 请注意：一旦将您的EEF放置在一个新目标，请务必在运行“播放（Play）”之前运行“规划（Plan）”—否则您会看到前一个目标的航路点（如果有的话）。 规划笛卡尔运动 如果“使用笛卡尔路径（Use Cartesian Path）”复选框被勾选激活，机器人会尝试在笛卡尔空间中线性移动末端执行器。 执行轨迹，调整速度 在规划成功后按下“规划和执行（Plan & Execute）”或“执行（Execute）”按钮会将轨迹发送给机器人—在本教程中，由于您使用了demo.launch，因此机器人只是一个仿真机器人。 开始时，默认速度和加速度会被缩放到机器人最大值的10%（0.1）。 可以在如下图所示的“规划（Planning）”选项卡中更改这些缩放因子，或者在您的机器人的moveit_config中更改这些默认值（在joint_limits.yaml文件中）。 后续步骤 RViz可视化工具 从“面板（Panels）”菜单中选择“添加新面板（Add New Panels）”。 从该菜单中选择“RvizVisualToolsGui”，然后单击“确定（OK）”按钮。这样就会看到添加到RViz中的新面板（如下图所示）。 保存配置 可以使用RViz中的“文件（File）”->“保存配置（Save Config）”菜单来保存您的配置。在继续下一个教程之前，您应该保存您的配置。 参考英语原文地址：https://moveit2_tutorials.picknik.ai/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html
说明： 介绍Move Group C++接口 概述： 在MoveIt2中，最简单的用户界面可以通过MoveGroupInterface类来建立。该类为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或位姿目标、创建运动规划、移动机器人、将物体添加到环境中以及将物体连接到机器人或者将物体从机器人上分离出来。该接口通过ROS话题、服务和动作与MoveGroup节点进行通信。 运行代码 打开两个终端。在第一个终端中启动RViz，然后等待加载完所有项目： ros2 launch moveit2_tutorials move_group.launch.py 在第二个终端中运行启动文件： ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py 片刻之后，RViz窗口应该会出现，并且看起来与本节教程开头的图片类似。要完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的“下一步（Next）”按钮，或者在光标聚焦RViz时选择屏幕顶部“工具（Tools）”面板中的“关键工具（Key Tool）”后按键盘上的N字母键。 预期输出 要查看预期输出，请观看本节教程顶部的YouTube视频。在RViz中，我们应该能够看到以下输出： 机器人将其手臂移动到其前方的位姿目标处。 机器人将其手臂移动到其侧面的关节目标处。 机器人将其手臂移回到一个新的位姿目标，同时保持末端执行器处于水平状态。 机器人沿着期望的笛卡尔路径（一个三角形向下、向右、向上+向左）移动其手臂。 一个盒子物体被添加到环境中位于机器人手臂右侧位置上。 机器人将手臂移动到位姿目标，避免与该盒子发生碰撞。 该物体被连接到机械臂手腕上（其颜色会变成紫色/橙色/绿色）。 该物体从机械臂手腕上分离开来（其颜色会变回绿色）。 该物体从环境中移除。 完整代码及其说明 完整代码可以在此处的MoveIt GitHub项目中看到。接下来将逐段解释各代码段的功能。 MoveIt2对称为“规划组（planning group）”的关节集合进行操作，并将它们存储在一个称为JointModelGroup的对象中。在整个MoveIt2中，术语“规划组（planning group）”和“关节模型组（joint model group）”是可以互换使用的。 static const std::string PLANNING_GROUP = "panda_arm"; 只需使用您想要控制和规划的规划组名称，即可轻松设置 MoveGroupInterface类 moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); 这里将会使用PlanningSceneInterface类以便在“虚拟世界”场景中添加和删除碰撞物体： moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 裸指针（raw pointers）频繁用于引用规划组以提高性能： const moveit::core::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); 可视化 namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "move_group_tutorial", move_group.getRobotModel()); visual_tools.deleteAllMarkers(); /* 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提供了多种类型的标记，在这个演示中将会使用文本、圆柱体和球体标记： Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.0; visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE); 批量发布可用于减少发送到RViz进行大型可视化的消息数量： visual_tools.trigger(); 获取基本信息。可以打印输出这个机器人的参考坐标系名称： RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str()); 还可以打印输出该规划组的末端执行器链接名称： RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str()); 可以使用以下代码获取机器人中所有规划组的列表： RCLCPP_INFO(LOGGER, "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 start the demo"); 规划至一个位姿目标。可以为该规划组规划一个运动，以让末端执行器到达期望的位姿： geometry_msgs::msg::Pose target_pose1; target_pose1.orientation.w = 1.0; target_pose1.position.x = 0.28; target_pose1.position.y = -0.2; target_pose1.position.z = 0.5; move_group.setPoseTarget(target_pose1); 现在，调用规划器来计算规划并将其可视化。请注意，这里只是进行规划，并不是请求move_group实际移动机器人。 moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); 可视化规划结果。还可以将规划结果可视化为RViz中带标记的线： RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); visual_tools.publishAxisLabeled(target_pose1, "pose1"); visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 移动至一个位姿目标。移动至位姿目标与上面的规划步骤类似，除了现在要使用move()函数之外。请注意，之前设置的位姿目标仍然处于活动状态，因此机器人将会尝试移动至该目标。在本教程中不会使用该函数，因为该函数是一个阻塞函数，需要一个控制器处于活动状态并报告轨迹执行成功。 /* Uncomment below line when working with a real robot */ /* move_group.move(); */ 规划至一个关节-空间目标。 这里会设定一个关节空间目标并移向该目标。这将会替换上面设置的位姿目标。 首先会创建一个引用当前机器人状态的指针。RobotState是包含所有当前位置/速度/加速度数据的对象。 moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10); 接着来获取该规划组的当前关节值集合。 std::vector<double> joint_group_positions; current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); 现在，让我们来修改其中一个关节，规划至新的关节空间目标并可视化该规划。 joint_group_positions[0] = -1.0; // radians move_group.setJointValueTarget(joint_group_positions); 我们将允许的最大速度和加速度减小到其最大值的5%。默认值为 10%（0.1）。在机器人的moveit_config的joint_limits.yaml文件中设置您喜欢的默认值，或者如果您需要机器人移动得更快，则在您的代码中显式设置这些因子值。 move_group.setMaxVelocityScalingFactor(0.05); move_group.setMaxAccelerationScalingFactor(0.05); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); 最后在RViz中可视化该规划结果： visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 使用路径约束进行规划。可以轻松地为机器人上的某个链接指定路径约束。现在让我们为该规划组指定一个路径约束和一个位姿目标。首先来定义路径约束： moveit_msgs::msg::OrientationConstraint ocm; ocm.link_name = "panda_link7"; ocm.header.frame_id = "panda_link0"; ocm.orientation.w = 1.0; ocm.absolute_x_axis_tolerance = 0.1; ocm.absolute_y_axis_tolerance = 0.1; ocm.absolute_z_axis_tolerance = 0.1; ocm.weight = 1.0; 现在，将其设置为规划组的路径约束： moveit_msgs::msg::Constraints test_constraints; test_constraints.orientation_constraints.push_back(ocm); move_group.setPathConstraints(test_constraints); 在关节空间中执行规划 取决于规划问题，MoveIt2会为问题表示在joint space和cartesian space之间作出选择。可以在ompl_planning.yaml文件中设置规划组参数enforce_joint_model_state_space:true来强制所有规划使用joint space。 默认情况下，对具有方位路径约束的规划请求会在笛卡尔空间中进行采样，以便调用IK作为生成采样器。 通过强制使用joint space，规划过程将会使用拒绝抽样来查找有效请求。请注意，这可能会大大增加规划时间。 这里我们将会重用已有的旧目标并规划至该目标。请注意，这仅在当前状态已经满足路径约束时才有效。因此需要将起始状态设置为一个新的位姿。 moveit::core::RobotState start_state(*move_group.getCurrentState()); geometry_msgs::msg::Pose start_pose2; start_pose2.orientation.w = 1.0; start_pose2.position.x = 0.55; start_pose2.position.y = -0.05; start_pose2.position.z = 0.8; start_state.setFromIK(joint_model_group, start_pose2); move_group.setStartState(start_state); 现在我们会从刚刚新建的起始状态规划至较早的位姿目标： move_group.setPoseTarget(target_pose1); 使用约束进行规划可能会很慢，因为每个样本都必须调用逆运动学解算器。让我们来增大默认的5秒规划时间，以确保规划器有足够的时间获得成功。 move_group.setPlanningTime(10.0); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); 然后在RViz中对规划结果进行可视化： visual_tools.deleteAllMarkers(); visual_tools.publishAxisLabeled(start_pose2, "start"); visual_tools.publishAxisLabeled(target_pose1, "goal"); visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 完成路径约束规划后，请务必清除它。 move_group.clearPathConstraints(); 笛卡尔路径。您可以通过指定末端执行器要经过的航路点列表来直接规划笛卡尔路径。请注意，上面我们是从的新起始状态开始的。初始位姿（起始状态）不是必须添加到航路点列表中的，但将它添加进来有助于可视化。 std::vector<geometry_msgs::msg::Pose> waypoints; waypoints.push_back(start_pose2); geometry_msgs::msg::Pose target_pose3 = start_pose2; target_pose3.position.z -= 0.2; waypoints.push_back(target_pose3); // down target_pose3.position.y -= 0.2; waypoints.push_back(target_pose3); // right target_pose3.position.z += 0.2; target_pose3.position.y += 0.2; target_pose3.position.x -= 0.2; waypoints.push_back(target_pose3); // up and left 这里希望笛卡尔路径以1厘米的分辨率进行插值，这就是我们将0.01指定为笛卡尔平移的最大步长的原因。我们将跳跃阈值指定为 0.0，从而可以有效地禁用它。警告—在操作真实硬件机器人时禁用跳跃阈值会导致冗余关节发生大量不可预测的运动，并且可能会成为一个安全问题： moveit_msgs::msg::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); RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); 在Rviz中可视化该运动规划： visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE); visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); for (std::size_t i = 0; i < waypoints.size(); ++i) visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 笛卡尔运动通常应该会很慢，例如接近物体时。笛卡尔规划的速度目前还无法通过maxVelocityScalingFactor来设置，而需要您手动为轨迹计时，正如此处所述。欢迎对此拉取请求。 可以如下所示执行轨迹： /* move_group.execute(trajectory); */ 向环境中添加物体。首先让我们规划另一个没有任何障碍物的简单目标： move_group.setStartState(*move_group.getCurrentState()); geometry_msgs::msg::Pose another_pose; another_pose.orientation.w = 0; another_pose.orientation.x = -1.0; another_pose.position.x = 0.7; another_pose.position.y = 0.0; another_pose.position.z = 0.59; move_group.setPoseTarget(another_pose); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(another_pose, "goal"); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 结果可能会如下图所示： 现在让我们来定义机器人要避开的一个碰撞物体的ROS消息： moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id = move_group.getPlanningFrame(); 该物体的id用于标识该物体： collision_object.id = "box1"; 定义一个要添加到虚拟世界中的盒子： shape_msgs::msg::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[primitive.BOX_X] = 0.1; primitive.dimensions[primitive.BOX_Y] = 1.5; primitive.dimensions[primitive.BOX_Z] = 0.5; 为该盒子定义一个位姿（相对于frame_id指定的坐标系）： geometry_msgs::msg::Pose box_pose; box_pose.orientation.w = 1.0; box_pose.position.x = 0.48; box_pose.position.y = 0.0; box_pose.position.z = 0.25; collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; std::vector<moveit_msgs::msg::CollisionObject> collision_objects; collision_objects.push_back(collision_object); 现在，让我们将该碰撞物体添加到虚拟世界中（使用可以包含其他物体的向量）： RCLCPP_INFO(LOGGER, "Add an object into the world"); planning_scene_interface.addCollisionObjects(collision_objects); 在RViz中显示状态文本，并等待MoveGroup接收和处理碰撞物体消息： visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); 现在，当我们规划一条轨迹时，它将会避开该障碍物： success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); 结果应该看起来如下图所示： 将物体连接至机器人。可以将物体连接至机器人上，这样物体就会随机器人几何体移动。 这里仿真了为操纵物体而拾取物体的过程。运动规划还应该要避免两个物体之间的碰撞。 moveit_msgs::msg::CollisionObject object_to_attach; object_to_attach.id = "cylinder1"; shape_msgs::msg::SolidPrimitive cylinder_primitive; cylinder_primitive.type = primitive.CYLINDER; cylinder_primitive.dimensions.resize(2); cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20; cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04; 我们为这个圆柱体定义了坐标系/位姿，以便该物体出现在抓手中： object_to_attach.header.frame_id = move_group.getEndEffectorLink(); geometry_msgs::msg::Pose grab_pose; grab_pose.orientation.w = 1.0; grab_pose.position.z = 0.2; 首先，将该物体添加到仿真的虚拟世界中（没有使用向量）： object_to_attach.primitives.push_back(cylinder_primitive); object_to_attach.primitive_poses.push_back(grab_pose); object_to_attach.operation = object_to_attach.ADD; planning_scene_interface.applyCollisionObject(object_to_attach); 然后，我们将物体“附加”到机器人上。会用 frame_id来确定该物体连接到机器人的哪个链接上，而且还需告诉MoveIt2允许该物体与抓手的手指链接发生碰撞。也可以用applyAttachedCollisionObject将某个物体直接连接至机器人上。 RCLCPP_INFO(LOGGER, "Attach the object to the robot"); std::vector<std::string> touch_links; touch_links.push_back("panda_rightfinger"); touch_links.push_back("panda_leftfinger"); move_group.attachObject(object_to_attach.id, "panda_hand", touch_links); visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); 重新进行规划，但现在该物体是在抓手中： move_group.setStartStateToCurrentState(); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); 现在结果应该看起来如下图所示： 对物体进行分离和移除。现在来将该圆柱体从机器人抓手中分离开来： RCLCPP_INFO(LOGGER, "Detach the object from the robot"); move_group.detachObject(object_to_attach.id); 在Rviz中显示状态文本： visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); 现在来将该物体从虚拟世界中移除掉： RCLCPP_INFO(LOGGER, "Remove the objects from the world"); std::vector<std::string> object_ids; object_ids.push_back(collision_object.id); object_ids.push_back(object_to_attach.id); planning_scene_interface.removeCollisionObjects(object_ids); 然后再在Rviz中显示状态文本： visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); 启动文件。整个启动文件放在GitHub网站上的此处。本教程中的所有代码都可以从作为MoveIt2安装一个组成部分的moveit_tutorials软件包中运行。 容差设置的注意事项 请注意，MoveGroupInterface的setGoalTolerance()和相关方法都是为规划设置容差，而不是为执行设置容差。 如果要配置执行容差，在使用FollowJointTrajectory控制器的情况下必须编辑controller.yaml文件，或者手动将其添加到规划器生成的轨迹消息中。 英语原文地址：https://moveit2_tutorials.picknik.ai/doc/move_group_interface/move_group_interface_tutorial.html
ROS2与MoveIt2入门教程-机器人模型和机器人状态
说明： 介绍MoveItCpp 概述： 在本节教程中，将会引导您了解MoveIt2中使用运动学的C++ API。 RobotModel和RobotState类 RobotModel和RobotState类是MoveIt2的核心类，可以让用户访问机器人运动学。 RobotModel类包含所有链接和关节之间的关系，包括从URDF加载的关节限制等属性。RobotModel类也会将机器人的链接和关节划分成SRDF中定义的多个规划组。可以在此处找到有关URDF和SRDF的单独教程：URDF和SRDF教程。 RobotState类包含有关机器人在某个时间点的信息，存储关节位置向量以及可选的速度和加速度。该信息可用于获取有关机器人的运动学信息，该信息取决于机器人当前状态，例如末端执行器的雅可比矩阵。 RobotState类也包含用于根据末端执行器位置（笛卡尔位姿）设置手臂位置和计算笛卡尔轨迹的辅助函数。 在本教程示例中，将会逐步引导用户了解这些类在Panda机器人上使用的过程。 运行代码 本教程中的所有代码都可以从作为MoveIt2安装组成部分之一的 moveit2_tutorials软件包中编译和运行。 使用ros2 launch命令直接从moveit2_tutorials软件包中的启动文件来运行该代码： ros2 launch moveit2_tutorials robot_model_and_robot_state_tutorial.launch.py 预期输出 预期输出会采用以下形式。由于这里使用的是随机关节值，因此数字会不相同： ... [robot_model_and_state_tutorial]: Model frame: world ... [robot_model_and_state_tutorial]: Joint panda_joint1: 0.000000 ... [robot_model_and_state_tutorial]: Joint panda_joint2: 0.000000 ... [robot_model_and_state_tutorial]: Joint panda_joint3: 0.000000 ... [robot_model_and_state_tutorial]: Joint panda_joint4: 0.000000 ... [robot_model_and_state_tutorial]: Joint panda_joint5: 0.000000 ... [robot_model_and_state_tutorial]: Joint panda_joint6: 0.000000 ... [robot_model_and_state_tutorial]: Joint panda_joint7: 0.000000 ... [robot_model_and_state_tutorial]: Current state is not valid ... [robot_model_and_state_tutorial]: Current state is valid ... [robot_model_and_state_tutorial]: Translation: -0.368232 0.645742 0.752193 ... [robot_model_and_state_tutorial]: Rotation: 0.362374 -0.925408 -0.11093 0.911735 0.327259 0.248275 -0.193453 -0.191108 0.962317 ... [robot_model_and_state_tutorial]: Joint panda_joint1: 2.263889 ... [robot_model_and_state_tutorial]: Joint panda_joint2: 1.004608 ... [robot_model_and_state_tutorial]: Joint panda_joint3: -1.125652 ... [robot_model_and_state_tutorial]: Joint panda_joint4: -0.278822 ... [robot_model_and_state_tutorial]: Joint panda_joint5: -2.150242 ... [robot_model_and_state_tutorial]: Joint panda_joint6: 2.274891 ... [robot_model_and_state_tutorial]: Joint panda_joint7: -0.774846 ... [robot_model_and_state_tutorial]: Jacobian: -0.645742 -0.26783 -0.0742358 -0.315413 0.0224927 -0.031807 -2.77556e-17 -0.368232 0.322474 0.0285092 -0.364197 0.00993438 0.072356 2.77556e-17 0 -0.732023 -0.109128 0.218716 2.9777e-05 -0.11378 -1.04083e-17 0 -0.769274 -0.539217 0.640569 -0.36792 -0.91475 -0.11093 0 -0.638919 0.64923 -0.0973283 0.831769 -0.40402 0.248275 1 4.89664e-12 0.536419 0.761708 0.415688 -0.00121099 0.962317 *注：如果您的输出具有不同的ROS控制台格式，请不必担心。 完整代码 完整代码可以在此处的MoveIt GitHub项目中看到。 启动 使用RobotModel类进行启动设置非常简单。通常，您会发现大多数更高级的组件都会返回一个指向RobotModel类的共享指针。如果可能，您应该始终使用该指针。在本示例中，将会使用这样一个共享指针来启动，而且只会讨论基本的API。可以查看这些类的实际代码API，以获取有关如何使用这些类提供的更多功能的更详细信息。 我们首先会实例化一个RobotModelLoader对象，该对象会在ROS 参数服务器上查找机器人描述并构建一个RobotModel供我们使用。 robot_model_loader::RobotModelLoader robot_model_loader(node); const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); RCLCPP_INFO(LOGGER, "Model frame: %s", kinematic_model->getModelFrame().c_str()); 使用RobotModel，我们可以构建一个RobotState来维护机器人的配置。我们将该状态中的所有关节均设置为默认值。然后可以获得一个JointModelGroup，它代表一个具体规划组的机器人模型，例如Panda机器人的“panda_arm”规划组。 moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm"); const std::vector<std::string>& joint_names = joint_model_group->getVariableNames(); 获取关节值。我们可以检索存储在Panda手臂规划组状态中的当前关节值集合： std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for (std::size_t i = 0; i < joint_names.size(); ++i) { RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } 关节限制。函数setJointGroupPositions()本身并不会强制执行关节限制，但调用enforceBounds()函数就会执行关节限制。 /* Set one joint in the Panda arm outside its joint limit */ joint_values[0] = 5.57; kinematic_state->setJointGroupPositions(joint_model_group, joint_values); /* Check whether any joint is outside its joint limits */ RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); /* Enforce the joint limits for this state and check again*/ kinematic_state->enforceBounds(); RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); 正运动学 现在，我们可以计算一组随机关节值的正运动学（FK）。请注意，这里我们想找到机器人“panda_arm”规划组中最远端链接“panda_link8”的位姿。 kinematic_state->setToRandomPositions(joint_model_group); const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8"); /* Print end-effector pose. Remember that this is in the model frame */ RCLCPP_INFO_STREAM(LOGGER, "Translation: \n" << end_effector_state.translation() << "\n"); RCLCPP_INFO_STREAM(LOGGER, "Rotation: \n" << end_effector_state.rotation() << "\n"); 逆运动学 我们现在可以为Panda机器人求解逆运动学（IK）。要求解IK，我们需要以下信息： 端执行器的期望位姿（默认情况下是“panda_arm”链中的最后一个链接）：即在上一个步骤中计算的end_effector_state。 超时时间：0.1s double timeout = 0.1; bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, timeout); 现在，我们可以打印输出该IK解算结果（如果求解到了的话）： if (found_ik) { kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for (std::size_t i = 0; i < joint_names.size(); ++i) { RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } } else { RCLCPP_INFO(LOGGER, "Did not find IK solution"); } 获取雅可比矩阵。我们还可以从RobotState中获取雅可比矩阵： Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0); Eigen::MatrixXd jacobian; kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()), reference_point_position, jacobian); RCLCPP_INFO_STREAM(LOGGER, "Jacobian: \n" << jacobian << "\n"); 启动文件 要运行该代码，需要一个启动文件，该启动文件要完成以下两件事： 将Panda机器人的URDF和SRDF加载到参数服务器上； 将MoveIt设置助手生成的kinematics_solver配置推送到ROS 参数服务器上实例化了本教程中各个类的节点命名空间中。 import os import yaml from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import ExecuteProcess from ament_index_python.packages import get_package_share_directory import xacro def load_file(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) try: with open(absolute_file_path, "r") as file: return file.read() except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available return None def load_yaml(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) try: with open(absolute_file_path, "r") as file: return yaml.safe_load(file) except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available return None def generate_launch_description(): # planning_context robot_description_config = load_file( "moveit_resources_panda_description", "urdf/panda.urdf" ) robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf" ) kinematics_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/kinematics.yaml" ) tutorial_node = Node( package="moveit2_tutorials", executable="robot_model_and_robot_state_tutorial", output="screen", parameters=[ {"robot_description": robot_description_config}, {"robot_description_semantic": robot_description_semantic_config}, kinematics_yaml, ], ) return LaunchDescription([tutorial_node]) 英语原文地址：https://moveit2_tutorials.picknik.ai/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html
说明： 介绍规划场景监视器 概述： PlanningSceneMonitor是用于维护规划场景最新状态的推荐接口。 RobotState、CurrentStateMonitor、PlanningScene、PlanningSceneMonitor 和PlanningSceneInterface之间的关系一开始可能会令人感到困惑。本节教程旨在厘清这些关键概念。 RobotState RobotState是机器人的一个快照，包含RobotModel和关节值集合。 CurrentStateMonitor CurrentStateMonitor（CSM）可以看作是RobotState的一个ROS封装器。它会订阅一个提供的JointState消息话题，该话题为单自由度执行器（例如旋转关节或棱柱关节）提供最新的传感器值，并使用这些关节值更新其内部的RobotState。除了单自由度关节外，机器人还可以有多自由度的关节，例如浮动关节和平面关节。为了维护附加有多自由度关节的链接和其他坐标系的最新坐标变换信息，CSM会存储一个TF2 Buffer，该缓冲区使用TF2 TransformListener在其内部数据中设置它们的坐标变换。 PlanningScene PlanningScene是机器人世界的一个快照，该世界包括RobotState和任意数量的碰撞物体。规划场景可用于碰撞检测以及获取环境相关信息。 PlanningSceneMonitor PlanningSceneMonitor用ROS接口封装了一个PlanningScene以维持PlanningScene的最新状态。要访问PlanningSceneMonitor底层的PlanningScene，请使用提供的LockedPlanningSceneRW和LockedPlanningSceneRO类。 PlanningSceneMonitor 具有下列对象，它们拥有各自的ROS接口，用于维持规划场景各个子级组件的最新状态： CurrentStateMonitor用于通过一个robot_state_subscriber_和一个tf_buffer_以及一个会侦听其他发布者规划场景差分消息的规划场景订阅者来跟踪RobotState的更新。 OccupancyMapMonitor用于通过ROS话题和服务来跟踪OccupancyMap的更新。 PlanningSceneMonitor具有以下订阅者节点： collision_object_subscriber_—侦听提供的CollisionObject消息话题，这些消息可能会在规划场景中添加、删除或修改碰撞对象，并将它们传递到受监视的规划场景中 planning_scene_world_subscriber_—侦听提供的PlanningSceneWorld消息话题，这些消息可能包含碰撞对象信息和/或占用地图信息。该订阅者节点对于保持规划场景监视器同步很有用 attached_collision_object_subscriber_—侦听提供的AttachedCollisionObject消息话题，这些消息会决定将碰撞物体附加到机器人状态中的链接上，或者从链接上将物体分离开。 PlanningSceneMonitor具有以下服务： get_scene_service_ —获取完整规划场景状态的一项可选服务。 PlanningSceneMonitor会用以下参数进行初始化： startSceneMonitor—启动planning_scene_subscriber_； startWorldGeometryMonitor—启动collision_object_subscriber_、planning_scene_world_subscriber_和OccupancyMapMonitor； startStateMonitor—启动CurrentStateMonitor和attached_collision_object_subscriber； startPublishingPlanningScene—启动另一个线程，用于在提供的话题上发布整个规划场景，供其他PlanningSceneMonitor订阅； providePlanningSceneService—启动get_scene_service_。 PlanningSceneInterface PlanningSceneInterface是一个很有用的类，用于通过C++ API向MoveGroup的PlanningSceneMonitor发布更新，而无需创建您自己的订阅者和服务客户​​端。如果没有MoveGroup或MoveItCpp，它可能会无法正常工作。 英语原文地址：https://moveit2_tutorials.picknik.ai/doc/planning_scene_monitor/planning_scene_monitor_tutorial.html