爱折腾-创客智造实验室 - Arm/机械臂套件
ROS与MoveIt 说明 机械臂的运行，在工业利用上非常普及 MoveIt软件包，提供全面的利用机械臂的功能包，方便在此基础做出更多的二次开发。 此教程在虚拟机上测试可能会有一些问题，推荐是Ubuntu14.04+indigo的电脑上进行。 目录 MoveIt!入门教程-目录 MoveIt!入门教程-简介 MoveIt!入门教程-MoveIt安装 MoveIt!入门教程-配置助手(Setup Assistant) MoveIt!入门教程-RViz插件 MoveIt!入门教程-Move Group接口 MoveIt!入门教程-Move Group Python接口 MoveIt!入门教程-运动学模型(Kinematic Model) MoveIt!入门教程-规划场景(Planning Scene) MoveIt!入门教程-ROS API规划场景 MoveIt!入门教程-运动规划(Motion Planners) MoveIt!入门教程-运动规划管道(传递途径) MoveIt!入门教程-控制器管理 MoveIt!入门教程-游戏杆遥控 MoveIt!入门教程-创建自定义约束采样器 MoveIt!入门教程-压力测试(Benchmarking) MoveIt!入门教程-运行测试 MoveIt!入门教程-控制器配置 MoveIt!入门教程-3D感知/配置 MoveIt!入门教程-生成IKFast插件 MoveIt!入门教程-Trac-IK运动学求解(Trac-IK Kinematics Solver) MoveIt!入门教程-运动学配置 MoveIt!入门教程-OMPL接口 MoveIt!入门教程-在rviz中为PR2增加场景物体 MoveIt!入门教程-创建库卡机械臂LWR的MoveIt!配置包 MoveIt!入门教程-库卡机械臂LWR的MoveIt!配置包解读 参考： http://docs.ros.org/indigo/api/moveit_tutorials/html http://moveit.ros.org/ http://wiki.ros.org/moveit_ros http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/index.html http://docs.ros.org/indigo/api/moveit_tutorials/html/ https://github.com/ncnynl/moveit_pr2
MoveIt!入门教程-简介 说明 MOVEit！是目前针对移动操作最先进的软件。 它结合了运动规划，操纵，三维感知，运动学，控制和导航的最新进展 它提供了一个易于使用的平台，开发先进的机器人应用程序，评估新的机器人设计和建筑集成的机器人产品 它广泛应用于工业，商业，研发和其他领域。 MOVEit！是最广泛使用的开源软件的操作，并已被用于超过65个机器人 概念 系统结构 The move_group node（move_group节点） 如上图，其中最重要的就是move_group节点，充当整合器：整合多个独立的组件，并提供ROS风格的Action和service User Interface（用户接口，三种接口可供调用） C++， 利用move_group_interface包可以方便使用move_group。 Python，利用moveit_commander包 GUI（界面），利用Motion Planning 的 Rviz插件。 配置，move_group是ROS节点，它在ROS param server获取三种信息， URDF ，从ROS param server中查找robot_description，获取URDF，它是机器人的描述文件。 SRDF ，从ROS param server中查找robot_description_semantic，获取SRDF.它一般通过MoveIt! Setup Assistant生成。 MoveIt! configuration ，从ROS param server中获取更多信息，如joint limits, kinematics, motion planning, perception and other information。 配置文件由MoveIt! Setup Assistant生成，存放在MoveIt! config目录中。 Robot Interface（机器人接口） move_group通过ROS topics和actions与机器人通讯，获取机器人的状态（位置，节点等），获取点云或其他传感器数据再传递给机器人的控制器。 Joint State Information（节点状态信息） move_group监听 /joint_states 主题确定状态信息。例如：确定每个节点的位置。 Move_group能够监听在这主题的多个发布器信息，即使是发布部分的信息（例如：独立的发布器可能是用于机械臂或移动机器人）。 ove_group不会建立自己的节点状态发布器。这就需要在每个机器人单独来建立。 Transform Information（变换信息） Move_group通过ROS TF库来监视变换信息。这允许节点获取全局的姿态信息。 例如：navigation包发布机器人的map frame和base frame到TF，move_group可以使用TF找出这个变换信息，在内部使用。 注意：Move_group只是监听TF，你需要启动robot_state_publisher才能发布TF。 Controller Interface （控制器接口） 通过ROS的action接口，FollowJointTrajectoryAction接口来使用控制器。 一个机器人的服务器服务于这个action-这个服务器不是有move_group提供。 move_group只会实例化客户端与机器人的控制器action服务器通讯。 Planning Scene（规划场景） move_group使用规划场景监视器来维护规划场景。 场景是世界的和机器人的状态的表现。 机器人状态包含机器人刚性连接到机器人的所有物体。 关于维护和更新规划场景的体系结构的详细信息在下面的规划场景部分中描述。 Extensible Capabilities（可扩展能力） move_group的结构被设计成容易扩展，独立的能力如抓放，运动学，运动规划。 扩展自公共类，但实际作为独立的插件运行。 插件可经由一系列的ROS yaml parameters 和ROS pluginlib库配置。 运动规划 The Motion Planning Plugin（运动规划插件） 运动规划器通过插件接口方式与MoveIt一起工作。这使得MoveIt更容易扩展，能同时与不同的库不同的运动规划器通讯。 运动规划器接口通过ROS Action或service方式提供。 针对 move_group的默认规划器通过MoveIt! Setup Assistant来配置使用OMPL或OMPL的MoveIt!接口 The Motion Plan Request（运动规划请求） 运动计划请求清楚地指定你想要的运动计划做什么。 通常情况下，你会要求运动规划器将一只手臂移动到一个不同的位置（在关节空间）或末端执行器到一个新的姿势。 默认情况下会进行碰撞检查（包括自碰撞） 您可以将一个对象附加到末端执行器（或机器人的任何一部分），例如，如果机器人拿起一个对象。这使得运动规划器考虑到对象的运动而进行路径规划。 你可以指定运动规划器检查的约束 - 内置的约束由kinematic constraints提供： Position constraints（位置约束） - 限制连接的位置在某个空间区域 Orientation constraints（方向约束） - 限制连接的方向在指定的roll, pitch和yaw范围 Visibility constraints（可视化约束） - 限制连接的点在特定传感器的一个可视化的锥形范围 Joint constraints（节点约束） - 限制节点位于两个值之间 User-specified constraints（自定义约束） - 利用自定义回调函数来指定自定义的约束。 The Motion Plan Result（运动规划结果） move_group节点会根据的运动规划请求，产生一个期望的轨迹。这个轨迹会移动机械臂（一组节点）到期待的位置。 move_group的结果是轨迹不仅仅是路径 - move_group会以希望最大的速度和加速度（需指定）来生成用于轨迹。 此轨迹需要遵循速度和加速度的限制 The Motion Planning Pipeline（运动规划管道） Motion planners and Plan Request Adapters（运动规划器和规划请求适配器） 图示： 完整的运动规划管道链整合规划规划器与多个叫规划请求适配器的组件。 规划请求适配器允许规划请求预处理和规划反馈后处理。 预处理在某些情况有用，例如：机器人的起始状态稍微超出关节限制之外的情况。 后处理需要处理几个操作，例如：转换生成的路径为带时间参数的轨迹。 MoveIt提供一系列默认的运动规划器适配器，每个都执行一个指定的功能。 常用运动规划适配器： FixStartStateBounds,这个适配器修复在URDF文件里面描述的关节限制的开始状态。用于仿真器中，当机器人配置不正确的时候。当有一个或多个关节稍微超出限制，机器人可能会结束。在这种情况下，运动规划就不能正常执行，因为它认为已经超出了关节限制。FixStartStateBounds请求适配器会修复开始状态到设置的关节限制内。但这不是每一次都是合适的解决方案。例如有多少关节超出限制。适配器参数则指定有多少关节超出限制才启用修复。 FixWorkspaceBounds，这个适配器会为规划指定一个默认的工作空间，一个立方体的大小为10米×10米×10米。如果规划请求的规划器没有填充这些区域，将会指定工作空间。 FixStartStateCollision，The fix start state collision adapter will attempt to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will perturb the values by is specified by a “jiggle_factor” parameter that controls the perturbation as a percentage of the total range of motion for the joint. The other parameter for this adapter specifies how many random perturbations the adapter will sample before giving up. FixStartStatePathConstraints，This adapter is applied when the start state for a motion plan does not obey the specified path constraints. It will attempt to plan a path between the current configuration of the robot to a new location where the path constraint is obeyed. The new location will serve as the start state for planning. AddTimeParameterization，The motion planners will typically generate “kinematic paths”, i.e., paths that do not obey any velocity or acceleration constraints and are not time parameterized. This adapter will “time parameterize” the motion plans by applying velocity and acceleration constraints. OMPL OMPL (Open Motion Planning Library) 是一个开源的运动规划库，主要是执行随机规划器。MoveIt直接整合OMPL，使用其库里的运动规划器作为主要/默认的一套规划器。在OMPL规划器是抽象的，例如：OMPL没有机器人的概念。MoveIt!配置OMPL，提供一个后端处理，用于解决机器人的问题。 规划场景 规划场景，用于显示机器人的世界，同时保存机器人自己的状态。它由Move_group节点内的规划场景监视器来维护。规划场景监视器监听： State Information（状态的信息）: joint_states 主题 Sensor Information（传感器的信息）: using the world geometry monitor described below World geometry information（世界的几何图形信息）: from user input on the planning_scene topic (as a planning scene diff). World Geometry Monitor（世界几何图形监视器），它通过来自机器人的传感器信息和来自用户的输入建立世界几何图形。它使用occupancy map monitor（occupancy地图监视器）建立围绕机器人的3D感知环境和通过planning_scene主题中附带的参数来增加对象的信息。 3D Perception（3D感知） 在MoveIt，3D感知是由occupancy map monitor处理，它使用插件结构处理不同的传感器输入。MoveIt有两个内置支持可以处理两种输入： Point clouds: handled by the point cloud occupancy map updater plugin Depth images: handled by the depth image occupancy map updater plugin 你可为occupancy map monitor编写自己的插件。 Octomap， Occupancy map monitor使用Octomap维持Occupancy map的环境。The Octomap can actually encode probabilistic information about individual cells although this information is not currently used in MoveIt!. The Octomap can directly be passed into FCL, the collision checking library that MoveIt! uses. Depth Image Occupancy Map Updater，深度图像栅格地图的更新器包括它自己的过滤器，例如：它可以从深度图消除机器人的可见部分。它使用目前有关的机器人的信息（机器人状态）进行此操作。 Kinematics（运动学） The Kinematics Plugin（运动学插件），MoveIt!使用插件结构，尤其是允许用户编写自己的逆运动学算法。Forward kinematics（正向运动学） and finding jacobians（查找雅可比矩阵） 被整合到自己的RobotState类。默认逆运动学插件配置使用KDL numerical jacobian-based solver.由MoveIt! Setup Assistant自动配置。 IKFast Plugin（IKFast插件），通常，用户可以选择执行自己的运动学求解器，例如PR2的有自己的运动学求解器。要实现这样的求解的一种流行的方法是使用ikfast包产生的需要与您的特定工作的机器人的C++代码。 Collision Checking（冲突检测），在规划场景中，冲突检测通过CollisionWorld对象来配置，由FCL包（主要的CC库）来执行。 Collision Objects（冲突对象），MoveIt支持不同类型对象的冲突检测。 Meshes（网格） Primitive Shapes（基本形状） - 例如： boxes（箱）, cylinders（圆柱）, cones（圆锥）, spheres（球） and planes（平面） Octomap - Octomap 对象能直接用于冲突检测 Allowed Collision Matrix (ACM)（免检冲突矩阵），在运动规划里，冲突检测会耗费甚至达到90%的计算资源。ACM编码需要检测的对象间的对应关系（机器人的或世界的）。如果在ACM关联两对象的值为1，那就不需要检测，这情况就比如两个对象相隔很远，永远不会发生碰撞。 Trajectory Processing（轨迹处理） Time parameterization（时间参数化），运动规划器一般只会生成路径，这个路径不带时间信息。MoveIt包含轨迹处理程序。它对结合路径和时间参数化的关节限制的速度和加速度来生成轨迹。这些限制是在joint_limits.yaml中为每个机器人指定的。 许可证 MOVEit！是BSD许可下发布的开源和V3。 在MOVEit每个文件！源代码应该包含一个许可证的副本。 致谢 MOVEit！最初在Willow Garage由Sachin Chitta，Ioan Sucan，Gil E. Jones，Acorn Pooley，Suat Gedikli，Dave Hershberger开发，感谢Willow Garage对MOVEit项目的支持！。也要感谢SRI国际在2013十月至2015六月MOVEit项目的支持！。 我们感谢以下的人对MoveIt的贡献！和相关的软件包，MOVEit！使用（或在某一点使用）： Lydia Kavraki，Mark Moll，和相关的kavraki实验室成员（莱斯大学）开发了OMPL - 自动随机规划算法软件包。 Dinesh Manocha 和 来自UNC Chapel Hill的 Jia Pan 开发了 FCL - 冲突检测算法的软件包 马克西姆利哈乔夫（CMU）、Ben Cohen（Penn）和Mike Phillips（CMU）开发SBPL-基于搜索的规划库 Armin Hornung，Kai Wurm，Maren Bennewitz，Cyril Stachniss，开发octomap-针对3D occupancy mapping Mrinal Kalakrishnan，Peter Pastor和Stefan Schaal在南加大开发STOMP-the distance field components 和CHOMP算法用于机械臂的导航 Dave Coleman从科罗拉多大学开发MOVEit的配置助理和添加文件到MOVEit！网站. Michael Ferguson开发简单的控制器管理插件 Sachin Chitta和Praveen Singh创建当前网站 MoveIt! 由臂导航和Grasping管道组件演变而来，感谢所有参与这些软件包的开发和研究工作者，特别是Edward Gil Jones, Matei Ciocarlie, Kaijen Hsiao, Adam Leeper, 和Ken Anderson. 同样感谢曾经在Willow Garage工作的实习生！臂导航和相关组件的ROS和PR2的社区成员，为MOVEit！提供反馈和提供贡献。 同样感谢ROS工业联盟的贡献者（由the Southwest Research Institute领导）为MoveIt和臂导航应用与工业环境提供的基础设施。感谢Fraunhofer IPA的贡献者在欧洲为MoveIt带来的支持。
MoveIt!入门教程-MoveIt安装 说明 MoveIt在ROS的Indigo和Jade版本发布，Kinetic版本仍没发布 Indigo版本的安装 基于Ubuntu14.04 + indigo版本 简单安装方法： sudo apt-get install ros-indigo-moveit-full 可选：（安装PR2的MoveIt相关的包） sudo apt-get install ros-indigo-moveit-full-pr2 更新环境 source /opt/ros/indigo/setup.bash 其他安装 采用其他版本访问：http://moveit.ros.org/install/ 采用源码安装访问：http://moveit.ros.org/install/source 采用Docker安装访问：http://moveit.ros.org/install/docker
MoveIt!入门教程-配置助手(Setup Assistant) 说明 配置助手(Setup Assistant)是一个图形化的用户接口，用于配置整合你的机器人使用MoveIt! 核心功能为你的机器人生成Semantic Robot Description Format (SRDF)文件，了解更多SRDF 额外也生成了使用MoveIt管道的配置文件 要求 安装MoveIt和ROS，没安装MoveIt参考，没安装ROS参考 配置步骤 启动配置助手 roslaunch moveit_setup_assistant setup_assistant.launch 启动后，有两个按钮选项： Create New MoveIt! Configuration Package Edit Existing MoveIt! Configuration Package. 点击Create New MoveIt! Configuration Package进入： 点击browse按钮找到 pr2.urdf.xacro 文件，默认在(This file gets installed in /opt/ros/indigo/share/pr2_description/robots/pr2.urdf.xacro。此文件是安装ros-indigo-moveit-full-pr2时候带有了。之后点击Load文件，等待进度条100%。 配置碰撞免检矩阵(ACM) 默认的碰撞免检矩阵生成器搜索机器人所有关节，这个碰撞免检矩阵是可以安全地关闭检查，从而减少行动规划的处理时间 在某些关节会关闭碰撞检查，比如总是碰撞，从不碰撞，在默认的位置碰撞，或在运动学链条上的相邻处。 采样密度指定了多少个随机机器人位置来检查碰撞。更高的密度需要更多的计算时间，而较低的密度就要减少关闭的检查节点 默认值是10000个碰撞检查。碰撞检查是并行完成，以减少处理时间。 生成碰撞免检矩阵 点击左边的Self-Collisions，得到如图： 再点击Regenerate Collision Matrix按钮，过一会在表格栏获得数据, 效果如下： 增加虚拟节点 虚拟关节主要用于将机器人连接到世界上。 我们将为PR2定义了一个虚拟节点连接PR2的base_footprint和odom_combined世界框架。 这个虚拟的关节表示在一个平面上的机器人基座的运动。 添加步骤： 点击左边的Virtual Joints ，再点击右下角的Add Virtual Joint 设置节点名为“virtual_joint” 设置子link为“base_footprint”和父参考系为“odom_combined”. 设置Joint类型为“planar”. 点击保存，效果如下： 增加规划组群 规划组用于在语义上描述你的机器人的不同部分，如定义一个手臂是什么，或一个末端执行器。 添加步骤： 点击左边的Planning Groups 点击Add Group，效果如下： 增加右臂 输入Group名为right_arm 选择kdl_kinematics_plugin/KDLKinematicsPlugin作为运动学求解 注意: 如果你有自定义机器人和想更强大的自定义IK求解，查看Kinematics/IKFast 保持Kin. Search Resolution和Kin. Search Timeout 为默认值. 点击Save and Add Joints按钮 获得一个关节列表，需要从左边选择合适的关节，增加到右边的关节列表。左边按住选择从r_shoulder_pan_joint到r_wrist_roll_joint。点击>按钮，增加到右边。如图： 点击Save按钮，保存已经选择的组。保存后效果图： 注意：PR2每个臂的只有7个关节，但在这里我们增加了9关节。一些关节(r_upper_arm_joint and r_forearm_joint)是固定的关节，不会被用于规划或运动学处理。 增加左臂 增加左臂类似右臂，选择从l_shoulder_pan_joint到the l_wrist_roll_joint. 增加抓手 我们将增加两组，左端和右端的末端执行器。跟增加手臂的流程不太一样。 点击 Add Group 按钮 输入组名为right_gripper 保持Kin. Search Resolution and Kin. Search Timeout 为默认值 点击Save and Add Links按钮 选择所有r_gripper相关的Links，增加到Selected Links。 点击Save 在左臂重复相同操作。这次选择left_gripper。增加后效果如下： 增加机器人的姿态 配置助手允许你添加固定的姿态到配置文件 这有一定作用，比如，你添加机器人到家里的某个位置。 点击Robot Poses按钮 点击Add Pose按钮，选择Pose名，默认值为中间值。你可以选择合适的位置，点击Save。 注意：姿势是如何关联特定的组。你可以为每个组保存独立的姿势。 重要提示：尝试移动所有的关节转动一圈。如果在你的URDF设置的限制有错误，你应该能够在这里看到。 标签末端执行器 你已经增加PR2的左右的抓手. 现在我们指定这两个组为特定的组end-effectors，这允许内部执行一些特别的操作 增加步骤： 点击End Effectors， 在点击Add End-Effectors.。 选择right_eef作为right抓手的End-Effectors的名称 选择right_gripper 作为End Effector Group 选择r_wrist_roll_link 作为end-effector的Parent Link 保留Parent Group空白.效果如下： 点击Save 类似的增加left_eef 添加被动关节 被动关节标签意味着允许任何存在于机器人的被动关节的规范 这告诉规划者不能规划这些被动节点。 被动关节的例子包括被动脚轮 PR2没有被动关节，所以我们将跳过这一步。 生成配置文件 最后一步生成能运行MoveIt!的配置文件 增加步骤： 点击Configuration Files，点击browse选择一个合适的位置，输入配置名。（例如在Home目录创建一个新目录pr2_moveit_generated） 点击 Generate Package ，会生成一系列的launch和配置文件。所有的文件显示在Generated Files/Folders列表。 效果图： 恭喜，至此配置完成
MoveIt!入门教程-RViz插件 说明 这是MoveIt的RViz插件，用于生成机器人的场景，生成计划，可视化输出，以及与机器人直接交互。 要求 完成配置助手教程 有一个PR2的MoveIt配置文件包 假设你的配置文件包名为 “pr2_moveit_config”. 或者，你可以安装已经配置好的 sudo apt-get install ros-indigo-moveit-pr2 教程不需要你有一个PR2，只是利用一系列机器人PR2的相关模型文件 运行步骤： 启动demo.launch文件和配置插件 roslaunch pr2_moveit_config demo.launch 第一次启动需要配置 Motion Planning 在Rviz Displays 选项卡，点add 从moveit_ros_visualization目录，选择MotionPlanning作为DisplayType，点OK.效果图如下: 在“Global Options” 选项 “Displays” 子窗口, 设置Fixed Frame 为“/odom_combined” 现在，你可以开始为你的机器人配置插件（目前是PR2）点击 “MotionPlanning” 后面的“Displays”. 确保Robot Description设置为“robot_description” 确保Planning Scene Topic设置为“planning_scene”. 在Planning Request, 改变组为 “right_arm”. 在Planned path里设置Trajectory Topic 为 “/move_group/display_planned_path”. 效果图： 配置可视化机器人 这里有四个不同的可视化活动 运动规划的开始状态（计划组显示为绿色） 运动规划的目标状态（计划组显示为橙色）。 在规划场景/规划环境机器人的配置 机器人的规划路径 显示状态可以通过是否打勾复选框进行开或关 在“Planning Request” 选项卡，开始状态使用“Query Start State”复选框 在“Planning Request” 选项卡，目标状态使用“Query Goal State”复选框 在 “Scene Robot”选项卡，规划场景机器人使用“Show Scene Robot”复选框 在“Planned Path” 选项卡，规划路径使用“Show Robot Visual”复选框 效果图如下： 应用所有这些选项去开或关得到不同的可视化效果 与PR2交互 按下Rviz顶部的"Interact" . 你应该看到多个交互标记显示在PR2的右臂 一个橙色的标记的右臂用于设置行为规划的目标状态。另一个绿色的标记的右臂设置为开始状态 您将能够使用这些标记（这是连接到每个手臂的前端链接）拖动手臂周围，并改变其方向。 - 移动到发生碰撞 注意：当你尝试将一个手臂与另一个碰撞时会发生什么情况。在碰撞中的两个环节会变红。 效果图： “使用冲突感知IK”复选框允许您切换IK解算器的行为。当复选框打勾，求解器将试图找到一个理想的末端执行器的位姿自由冲突的解决。当它未打勾，求解器将允许发生碰撞。在碰撞环节始终还是会显示红色。 效果图： - 移动不可到达的工作区 注意：当你试图将一个末端执行器移动到不能到达的工作区（有时候禁止标记不会出现）时会发生什么情况。 效果图： 在PR2里使用行为规划 现在可以使用MoveIt的Rviz插件在PR2上进行行为规划 移动开始状态到希望的位置 移动目标状态到另一个希望的位置 确保状态不会与机器人产生冲突 确保路径规划能可视化，在Planned Path选项卡上检查“Show Trail”复选框 在Motion Planning的planning选项卡，按下Plan 按钮，应该看到手臂移动并显示它的轨迹。 效果图：
MoveIt!入门教程-Move Group接口 说明 MoveIt主要的用户接口功能通过MoveGroup类实现 这个类提供简易方式去实现大部分功能，比如：设置关节或目标姿态，创建行为规划，移动机器人，在环境中增加对象或给机器人增加或减少对象。 使用 安装 MoveGroup类的使用非常简单，只需提供你想去控制或规划的组名 moveit::planning_interface::MoveGroup group("right_arm"); 我们使用PlanningSceneInterface类去直接处理. moveit::planning_interface::PlanningSceneInterface planning_scene_interface; (可选)在Rviz中创建一个发布用于可视化规划. ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; 获得基本信息 打印这个组的参考系的名称 ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str()); 打印这个组的末端执行器的名称 ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str()); 规划一个姿态目标 我们为这个组计划一个动作，移动末端执行器到希望的姿态。 geometry_msgs::Pose target_pose1; target_pose1.orientation.w = 1.0; target_pose1.position.x = 0.28; target_pose1.position.y = -0.7; target_pose1.position.z = 1.0; group.setPoseTarget(target_pose1); 现在，我们调用规划器去计算规划和可视化过程，注意：我们只是规划，不要求move_group去实际移动机器人。 moveit::planning_interface::MoveGroup::Plan my_plan; bool success = group.plan(my_plan); ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); 4. 可视化规划 我们可以在Rviz里可视化这个规划。这个不是必需的，因为group.plan()的调用使得上面的过程会自动完成。在明确发布计划有用的情况下，我们想去可视化之前创建的规划。 if (1) { ROS_INFO("Visualizing plan 1 (again)"); display_trajectory.trajectory_start = my_plan.start_state_; display_trajectory.trajectory.push_back(my_plan.trajectory_); display_publisher.publish(display_trajectory); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); } 5. 移动到目标姿态 移动到一个姿势的目标是类似于上述步骤除了我们现在使用的move()功能。 请注意，我们已经设置了前面的姿势目标仍然是有效的，所以机器人将尝试移动到那个目标。 我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器是激活，执行后报告成功的轨迹。 /* Uncomment below line when working with a real robot*/ /* group.move() */ 6. 规划到一个关节空间内的目标 现在规划一个到关节空间内的目标并向其移动。这个会替代之前设置的目标 首先获取指定组的当前关节的设置 std::vector<double> group_variable_values; group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values); 修改一个关节，规划到新的目标并可视化 group_variable_values[0] = -1.0; group.setJointValueTarget(group_variable_values); success = group.plan(my_plan); ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); 7. 路径约束规划 路径约束容易指定机器人的一个连接。为组指定路径约束和目标姿态。 首先定义路径约束 moveit_msgs::OrientationConstraint ocm; ocm.link_name = "r_wrist_roll_link"; ocm.header.frame_id = "base_link"; 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::Constraints test_constraints; test_constraints.orientation_constraints.push_back(ocm); group.setPathConstraints(test_constraints); 我们将重用我们曾经有过的旧的目标，并规划它。 请注意，如果目前的状态已经满足路径约束。这就能开始工作， 因此，我们需要将启动状态设置为一个新的姿势。 robot_state::RobotState start_state(*group.getCurrentState()); geometry_msgs::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; const robot_state::JointModelGroup *joint_model_group = start_state.getJointModelGroup(group.getName()); start_state.setFromIK(joint_model_group, start_pose2); group.setStartState(start_state); 我们规划从新的开始姿态移动到之前的目标姿态 group.setPoseTarget(target_pose1); success = group.plan(my_plan); ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(10.0); 当做了路径约束时一定要清除它。 group.clearPathConstraints(); 8. 笛卡尔路径 你可以计划一个笛卡尔路径通过直接为末端执行器指定航点列表。 请注意，我们是从新的开始状态。初始姿态（起始状态）不需要加入航点列表。 std::vector<geometry_msgs::Pose> waypoints; geometry_msgs::Pose target_pose3 = start_pose2; target_pose3.position.x += 0.2; target_pose3.position.z += 0.2; waypoints.push_back(target_pose3); // up and out target_pose3.position.y -= 0.2; waypoints.push_back(target_pose3); // left target_pose3.position.z -= 0.2; target_pose3.position.y += 0.2; target_pose3.position.x -= 0.2; waypoints.push_back(target_pose3); // down and right (back to start) 我们希望笛卡尔路径被内插在一个分辨率为1厘米，这就是为何在笛卡尔转换指定0.01作为最大值。我们指定跳阈值为0，实际上是禁用它。（没理解好留原文）We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the max step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it. moveit_msgs::RobotTrajectory trajectory; double fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)", fraction * 100.0); /* Sleep to give Rviz time to visualize the plan. */ sleep(15.0); 9. 添加/删除对象和附着/分离对象 首先，我们将定义碰撞对象消息。 moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = group.getPlanningFrame(); /* The id of the object is used to identify it. */ collision_object.id = "box1"; /* Define a box to add to the world. */ shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 0.4; primitive.dimensions[1] = 0.1; primitive.dimensions[2] = 0.4; /* A pose for the box (specified relative to frame_id) */ geometry_msgs::Pose box_pose; box_pose.orientation.w = 1.0; box_pose.position.x = 0.6; box_pose.position.y = -0.4; box_pose.position.z = 1.2; collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; std::vector<moveit_msgs::CollisionObject> collision_objects; collision_objects.push_back(collision_object); 现在，让我们添加碰撞对象到世界 ROS_INFO("Add an object into the world"); planning_scene_interface.addCollisionObjects(collision_objects); /* Sleep so we have time to see the object in RViz */ sleep(2.0); 带有碰撞检测的规划是很慢的。让我们制定计划时间，以确保规划有足够的时间来计划。10秒应该是足够的。 group.setPlanningTime(10.0); 现在当我们计划一个轨迹，它将避免障碍 group.setStartState(*group.getCurrentState()); group.setPoseTarget(target_pose1); success = group.plan(my_plan); ROS_INFO("Visualizing plan 5 (pose goal move around box) %s", success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(10.0); 现在，让我们把碰撞物体附加到机器人上。 ROS_INFO("Attach the object to the robot"); group.attachObject(collision_object.id); /* Sleep to give Rviz time to show the object attached (different color). */ sleep(4.0); 现在，让我们从机器人中分离碰撞物体。 ROS_INFO("Detach the object from the robot"); group.detachObject(collision_object.id); /* Sleep to give Rviz time to show the object detached. */ sleep(4.0); 现在，让我们从世界上删除碰撞对象。 ROS_INFO("Remove the object from the world"); std::vector<std::string> object_ids; object_ids.push_back(collision_object.id); planning_scene_interface.removeCollisionObjects(object_ids); /* Sleep to give Rviz time to show the object is no longer there. */ sleep(4.0); 10. 双手臂位姿 首先定义一个新组安置两个手臂。定义两个新的独立的姿态目标，分别是为每个末端执行器。 注意：我们会重用以上针对右臂的目标。 moveit::planning_interface::MoveGroup two_arms_group("arms"); two_arms_group.setPoseTarget(target_pose1, "r_wrist_roll_link"); geometry_msgs::Pose target_pose2; target_pose2.orientation.w = 1.0; target_pose2.position.x = 0.7; target_pose2.position.y = 0.15; target_pose2.position.z = 1.0; two_arms_group.setPoseTarget(target_pose2, "l_wrist_roll_link"); 规划并可视化 moveit::planning_interface::MoveGroup::Plan two_arms_plan; two_arms_group.plan(two_arms_plan); sleep(4.0); 11. 完整代码 完整代码可以这里找到。下载代码 12. 编译代码 参考这里 13. launch文件 所有的launch文件在这 代码来自moveit_tutorials软件包(是你安装MoveIt的一部分)，你可以编译和运行。 14. 启动代码 roslaunch moveit_tutorials move_group_interface_tutorial.launch 效果如下： 在窗口的底部的右侧部分的运动规划部分可以关闭，以获得更好的机器人视图。 15. 期望输出 在Rviz，我们应该能够看到下面的（会有一个延迟5-10秒之间每一步）： 机器人移动右臂到右前方的姿态目标 重复上面的动作 机器人移动右臂到右边的连接目标 机器人移动右臂回到新的姿态目标并保持末端执行器水平 机器人沿着笛卡尔路径期望的路径移动右臂（三角形向上+向前，左，下+后） 盒子对象增加到右臂的右边。 机器人移动右臂到姿态目标，避开与盒子碰撞 该对象连接到手腕上（它的颜色将改变到紫色/橙色/绿色） 该对象是从手腕上脱离（它的颜色会变回绿色） 该对象被从环境中移除。
MoveIt!入门教程-Move Group Python接口 说明 MoveIt主要的用户接口功能通过MoveGroup类实现 这个类提供简易方式去实现大部分功能，比如：设置关节或目标姿态，创建行为规划，移动机器人，在环境中增加对象或给机器人增加或减少对象。 使用 安装 使用Python接口前，要先导入moveit_commander等相关的Python包 import sys import copy import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg 初始化moveit_commander和rospy print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) 实例化RobotCommander对象，这个接口是机器人总入口 robot = moveit_commander.RobotCommander() 实例化PlanningSceneInterface对象，这个接口围绕机器人的世界 scene = moveit_commander.PlanningSceneInterface() 实例化MoveGroupCommander对象，这个接口应用与一组关节。这里使用左臂，能使用左臂来规划和执行动作。 group = moveit_commander.MoveGroupCommander("left_arm") 创建DisplayTrajectory发布器，可以得到轨迹在Rviz总实现可视化。 display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) 等待Rviz初始化 print "============ Waiting for RVIZ..." rospy.sleep(10) print "============ Starting tutorial " 获得基本信息 打印参考系的名称 print "============ Reference frame: %s" % group.get_planning_frame() 打印这个组的末端执行器的连接名称 print "============ Reference frame: %s" % group.get_end_effector_link() 获得机器人的所有组 print "============ Robot Groups:" print robot.get_group_names() 用于调式，打印机器人的状态 print "============ Printing robot state" print robot.get_current_state() print "============" 规划姿态目标 为这个组规划动作，实现末端执行器到达期望的姿态目标。 print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target) 调用规划器计算规划并在Rviz里显示，注意：这里只是规划，不是实际执行。 plan1 = group.plan() print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) 你可以要求Rviz显示规划（或叫轨迹），但group.plan()方法会自动实现。 print "============ Visualizing plan1" display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) 移动到姿态目标 移动到一个姿势的目标是类似于上述步骤除了我们现在使用的go()功能。 请注意，我们已经设置了前面的姿势目标仍然是有效的，所以机器人将尝试移动到那个目标。 我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器是激活，执行后报告成功的轨迹。 # Uncomment below line when working with a real robot # group.go(wait=True) 规划到连接空间目标 设置连接空间目标并移动，首先清除姿态目标 group.clear_pose_targets() 取得组目前设置的连接值 group_variable_values = group.get_current_joint_values() print "============ Joint values: ", group_variable_values 修改其中一个关节，规划移动到新的连接空间目标，可视化规划。 group_variable_values[0] = 1.0 group.set_joint_value_target(group_variable_values) plan2 = group.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(5) 笛卡尔路径 你可以计划一个笛卡尔路径通过直接为末端执行器指定航点列表。 waypoints = [] # start with the current pose waypoints.append(group.get_current_pose().pose) # first orient gripper and move forward (+x) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x + 0.1 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z waypoints.append(copy.deepcopy(wpose)) # second move down wpose.position.z -= 0.10 waypoints.append(copy.deepcopy(wpose)) # third move to the side wpose.position.y += 0.05 waypoints.append(copy.deepcopy(wpose)) 我们希望笛卡尔路径被内插在一个分辨率为1厘米，这就是为何在笛卡尔转换指定0.01作为最大值。我们指定跳阈值为0，实际上是禁用它。（没理解好留原文）We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the max step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it. (plan3, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(5) 7. 添加/删除对象和附着/分离对象 首先，我们将定义碰撞对象消息。 collision_object = moveit_msgs.msg.CollisionObject() 完成后关闭 moveit_commander.roscpp_shutdown() 完整代码 完整代码在这里 launch 文件 文件在这里 在moveit_tutorials包直接运行代码 roslaunch moveit_tutorials move_group_python_interface_tutorial.launch 期望的输出 在Rviz，我们应该能够看到下面的（会有一个延迟5-10秒之间每一步）： 机器人移动左臂到它前面的姿态目标(plan1) 重复上面的动作 机器人移动左臂到连接目标。 机器人沿着期望的笛卡尔路径运行
MoveIt!入门教程-运动学模型(Kinematic Model) 说明 通过 C++ API来应用kinematics RobotModel和RobotState类 核心类为RobotModel和RobotState 利用PR2的右臂来演示类的使用 使用 开始 设置开始使用RobotModel类是很容易的。在一般情况下，你会发现，最高级的组件将返回一个共享指针给RobotModel类。你应该总是在可能的情况下使用它。在这个示例中，我们将从这样一个共享指针开始，只讨论基本的。您可以查看这些类的实际代码类以获取更多关于如何使用这些类功能的更多信息。 我们开始实例化RobotModelLoader对象，它从ROS参数服务器查找机器人描述和构建我们要使用的机器人模型 robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); 使用 RobotModel, 我们可构建RobotState维护机器人的配置。我们在状态中设置所有关节为默认值。我们能获得一个 JointModelGroup, 它显示机器人模型在特定组，比如： PR2机器人“right_arm” . robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("right_arm"); const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames(); 获得关节值 我们可以获取保持在状态中右臂的关节的设置值 std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for(std::size_t i = 0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } 关节限制 setJointGroupPositions()函数不能强制自己设置关节限制，但可以通过调用enforceBounds()来设置 /* Set one joint in the right arm outside its joint limit */ joint_values[0] = 1.57; kinematic_state->setJointGroupPositions(joint_model_group, joint_values); /* Check whether any joint is outside its joint limits */ ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); /* Enforce the joint limits for this state and check again*/ kinematic_state->enforceBounds(); ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); 正向运动学 现在，我们可以计算出一组随机关节值的正向运动学。 请注意，我们要找到机器人右臂("right_arm")最远端的连接“r_wrist_roll_link”的姿态。 kinematic_state->setToRandomPositions(joint_model_group); const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("r_wrist_roll_link"); /* Print end-effector pose. Remember that this is in the model frame */ ROS_INFO_STREAM("Translation: " << end_effector_state.translation()); ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation()); 逆运动学 我们现在可以在PR2机器人的右臂解决逆运动学（IK）。为了解决IK，我们需要以下： 末端执行器所需的姿势（默认情况下，这是“right_arm”链最后一个环节） end_effector_state为我们上一步计算得到的。 试图解决IK的数量：5 每个尝试超时：0.1秒 bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1); 如果找到，打印出解决方案 if (found_ik) { kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for(std::size_t i=0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } } else { ROS_INFO("Did not find IK solution"); } 获得Jacobian 我们可以从RobotState获取Jacobian 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); ROS_INFO_STREAM("Jacobian: " << jacobian); 完整代码 这里可获得完整代码 编译代码 编译代码方法 launch文件 运行代码，需要这个launch文件做两样事情： 上传PR2 URDF和SRDF到参数服务器 放置运动学求解配置（由配置助手生成的）到ROS参数服务器对应节点的命名空间下，这个节点由是教程里实例化的类。 <launch> <include file="$(find pr2_moveit_config)/launch/planning_context.launch"> <arg name="load_robot_description" value="true"/> </include> <node name="kinematic_model_tutorial" pkg="moveit_tutorials" type="kinematic_model_tutorial" respawn="false" output="screen"> <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/> </node> </launch> 教程里的代码可以从moveit_tutorials包找到，可以进行编译和运行。 运行代码 roslaunch moveit_tutorials kinematic_model_tutorial.launch 期待的输出 输出效果如下，因为使用随机节点，数字会不一样。 [ INFO] [1384819451.749126980]: Model frame: /odom_combined [ INFO] [1384819451.749228320]: Joint r_shoulder_pan_joint: 0.000000 [ INFO] [1384819451.749268059]: Joint r_shoulder_lift_joint: 0.000000 [ INFO] [1384819451.749298929]: Joint r_upper_arm_roll_joint: 0.000000 [ INFO] [1384819451.749337412]: Joint r_upper_arm_joint: -1.135650 [ INFO] [1384819451.749376593]: Joint r_elbow_flex_joint: 0.000000 [ INFO] [1384819451.749404669]: Joint r_forearm_roll_joint: -1.050000 [ INFO] [1384819451.749480167]: Joint r_forearm_joint: 0.000000 [ INFO] [1384819451.749545399]: Joint r_wrist_flex_joint: 0.000000 [ INFO] [1384819451.749577945]: Joint r_wrist_roll_joint: 0.000000 [ INFO] [1384819451.749660707]: Current state is not valid [ INFO] [1384819451.749723425]: Current state is valid [ INFO] [1384819451.750553768]: Translation: 0.0464629 -0.660372 1.08426 [ INFO] [1384819451.750715730]: Rotation: -0.0900434 -0.945724 -0.312248 -0.91467 -0.0455189 0.40163 -0.394045 0.321768 -0.860926 [ INFO] [1384819451.751673044]: Joint r_shoulder_pan_joint: -1.306166 [ INFO] [1384819451.751729266]: Joint r_shoulder_lift_joint: 0.502776 [ INFO] [1384819451.751791355]: Joint r_upper_arm_roll_joint: 0.103366 [ INFO] [1384819451.751853793]: Joint r_upper_arm_joint: -1.975539 [ INFO] [1384819451.751907012]: Joint r_elbow_flex_joint: 2.805000 [ INFO] [1384819451.751963863]: Joint r_forearm_roll_joint: -1.851939 [ INFO] [1384819451.752015895]: Joint r_forearm_joint: -0.414720 [ INFO] [1384819451.752064923]: Joint r_wrist_flex_joint: 0.000000 [ INFO] [1384819451.752117933]: Joint r_wrist_roll_joint: 0.000000 [ INFO] [1384819451.753252155]: Jacobian: 0.472372 0.0327816 -0.28711 0.0708816 1.38778e-17 0 0 0.0964629 -0.120972 -0.0626032 -0.311436 0 0 0 0 -0.381159 -0.0266776 -0.0320097 8.67362e-19 0 0 0 0.965189 0.229185 0.973042 -0.0665617 -0.991369 -0.0900434 0 0.261552 -0.845745 0.212168 -0.116995 0.12017 -0.91467 1 0 -0.48186 0.0904127 0.990899 -0.0524048 -0.394045
MoveIt!入门教程-规划场景(Planning Scene) 说明 planningscene类提供的主要的接口，可使用碰撞检测和约束检测。 在本教程中，我们将探讨C++接口类。 使用 安装 planningscene类可以方便地安装和配置使用一个RobotModel或URDF和SRDF 不推荐直接实例化PlanningScene类 推荐的方法是使用PlanningSceneMonitor类（下一个教程中详细讨论），它利用机器人的关节和传感器数据来创建和维护目前的规划场景。 在本教程中，我们将planningscene直接实例化一个类，但这种方法只用于说明实例。 robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); planning_scene::PlanningScene planning_scene(kinematic_model); 碰撞检测 自碰撞检测 首先检查机器人的状态是否在自碰撞。例如是否会左手碰右手。 我们会实例化CollisionRequest对象和CollisionResult对象，传递他们到碰撞检测函数。 collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); 改变状态 现在，让我们改变机器人的当前状态。 规划场景保持当前状态。我们可以得到一个参考，并改变它，然后检查新的机器人配置的碰撞。 特别注意，我们需要在发起新的碰撞检测要求前，清除之前的冲突检测的结果。 robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst(); current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); 检查一个组 目前冲突检测只针对PR2机器人的右臂。例如我们检测右臂跟机器人其他部位是否会发生碰撞。 可以通过增加组名 “right_arm”来进行冲突检测。 collision_request.group_name = "right_arm"; current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); 获得联系信息 首先，手动设置正确的手臂到一个位置，我们知道内部（自我）碰撞发生。 注意，这个状态现在实际上不在PR2的关节限制范围，我们也可以直接检查。 std::vector joint_values; const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("right_arm"); current_state.copyJointGroupPositions(joint_model_group, joint_values); joint_values[ 现在，我们可以得到任何可能发生在一个给定的配置的右手臂的碰撞的接触点信息。 我们可以要求通过填充在适当的字段中的碰撞请求的接触点信息，并指定要返回的接触点的最大数量。 collision_request.contacts = true; collision_request.max_contacts = 1000; 获取接触点信息： collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 4: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); collision_detection::CollisionResult::ContactMap::const_iterator it; for(it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) { ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); } 修改免检碰撞矩阵（AllowedCollisionMatrix (ACM) ） AllowedCollisionMatrix (ACM) 提供一个机制，告诉冲突世界去忽略与某些特定对象之间的冲突：在世界中的机器人的所有部分与对象间。我们告诉冲突检测去忽略所有在以上报告的冲突的连接。例如：即使这些连接事实发生冲突，冲突检测还是会忽略，返回机器人的状态，这些并没冲突。 请注意，在这个例子中，我们如何获得免检碰撞矩阵和当前状态的副本，并将它们传递到碰撞检查功能。 collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix(); robot_state::RobotState copied_state = planning_scene.getCurrentState(); collision_detection::CollisionResult::ContactMap::const_iterator it2; for(it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2) { acm.setEntry(it2->first.first, it2->first.second, true); } collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); 充分的碰撞检测 我们可以进行自碰撞检测，也可以使用checkCollision函数与环境间的冲突检测。 这是规划器最常用的一组碰撞检测函数。与环境的碰撞检测将会使用填充版的机器人。 填充版使得机器人在环境中远离障碍物。 collision_result.clear(); planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); 约束检测 PlanningScene类同样提供便利的函数用于约束检测。 两种约束类型： 来自KinematicConstraint集合的约束选择，例如：JointConstraint, PositionConstraint, OrientationConstraint 和VisibilityConstraint 用户定义的约束，可以通过回调函数来调用。 下面看看简单的KinematicConstraint 检查运动学约束 首先定义PR2机器人右臂的末端执行器的一个位置和方向约束 注意：一些便利的函数用于填充约束（这些函数在moveit_core的kinematic_constraints目录的utils.h文件找到） std::string end_effector_name = joint_model_group->getLinkModelNames().back(); geometry_msgs::PoseStamped desired_pose; desired_pose.pose.orientation.w = 1.0; desired_pose.pose.position.x = 0.75; desired_pose.pose.position.y = -0.185; desired_pose.pose.position.z = 1.3; desired_pose.header.frame_id = "base_footprint"; moveit_msgs::Constraints goal_constraint = kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose); 使用PlanningScene类的isStateConstrained来检查状态是否遵守约束。 copied_state.setToRandomPositions(); copied_state.update(); bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint); ROS_INFO_STREAM("Test 7: Random state is " << (constrained ? "constrained" : "not constrained")); 有很多方便的方法检测约束(当你在规划里反复检测同类约束)。首先构建已经在ROS约束信息预处理的KinematicConstraintSet和设置它，可以快速处理。 kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model); kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms()); bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set); ROS_INFO_STREAM("Test 8: Random state is " << (constrained_2 ? "constrained" : "not constrained")); 有一个直接的方法通过使用KinematicConstraintSet类 kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = kinematic_constraint_set.decide(copied_state); ROS_INFO_STREAM("Test 9: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); 自定义约束 用户定义的约束也可以被指定给PlanningScene类，通过setStateFeasibilityPredicate函数来使用回调函数。 这是简单的回调例子，检查PR2机器人“r_shoulder_pan”是在正或反向角度。 bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose) { const double* joint_values = kinematic_state.getJointPositions("r_shoulder_pan_joint"); return (joint_values[0] > 0.0); } 现在，无论isStateFeasible是否调用，自定回调函数都会被调用。 planning_scene.setStateFeasibilityPredicate(userCallback); bool state_feasible = planning_scene.isStateFeasible(copied_state); ROS_INFO_STREAM("Test 10: Random state is " << (state_feasible ? "feasible" : "not feasible")); 无论isStateValid是否给调用，三个检测会被实施：(a) 冲突检测 (b) 约束检测 (c) 用户自定义回调函数的可行性检测 bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "right_arm"); ROS_INFO_STREAM("Test 10: Random state is " << (state_valid ? "valid" : "not valid")); 所有的规划器通过 MoveIt!和OMPL来执行冲突检测，约束检测和用户自定义回调函数的可行性检测。 完整代码 下载完整代码 编译代码 编译方法 运行代码 roslaunch moveit_tutorials planning_scene_tutorial.launch 期望的输出 输出效果如下，使用随机关节值，数字可能有差异。 [ INFO] [1385487628.853237681]: Test 1: Current state is not in self collision [ INFO] [1385487628.857680844]: Test 2: Current state is in self collision [ INFO] [1385487628.861798756]: Test 3: Current state is not in self collision [ INFO] [1385487628.861876838]: Current state is not valid [ INFO] [1385487628.866177315]: Test 4: Current state is in self collision [ INFO] [1385487628.866228020]: Contact between: l_shoulder_pan_link and r_forearm_link [ INFO] [1385487628.866259030]: Contact between: l_shoulder_pan_link and r_shoulder_lift_link [ INFO] [1385487628.866305963]: Contact between: l_shoulder_pan_link and r_shoulder_pan_link [ INFO] [1385487628.866331036]: Contact between: l_shoulder_pan_link and r_upper_arm_link [ INFO] [1385487628.866358135]: Contact between: l_shoulder_pan_link and r_upper_arm_roll_link [ INFO] [1385487628.870629418]: Test 5: Current state is not in self collision [ INFO] [1385487628.877406467]: Test 6: Current state is not in self collision [ INFO] [1385487628.879610797]: Test 7: Random state is not constrained [ INFO] [1385487628.880027331]: Test 8: Random state is not constrained [ INFO] [1385487628.880315077]: Test 9: Random state is not constrained [ INFO] [1385487628.880377445]: Test 10: Random state is feasible [ INFO] [1385487628.887157707]: Test 10: Random state is not valid
MoveIt!入门教程-ROS API规划场景 说明 本章使用两个操作来体验"planning scene diffs"的作用： 在世界里增加或移除对象 在机器人上增加和移除对象 ROS API ROS API（planning scene publisher） 通过主题接口diffs,进行规划场景发布。 planning scene diff在当前的规划场景（由move_group维护）和新的用户期望的规划场景之间是不同的 发布必需的主题 注意：这个主题可能需要在launch文件重新映射 ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1); while(planning_scene_diff_publisher.getNumSubscribers() < 1) { ros::WallDuration sleep_t(0.5); sleep_t.sleep(); } 定义附加的对象信息 我们将使用这个消息来添加或去除来自世界的对象，并增加对象到机器人 moveit_msgs::AttachedCollisionObject attached_object; attached_object.link_name = "r_wrist_roll_link"; /* The header must contain a valid TF frame*/ attached_object.object.header.frame_id = "r_wrist_roll_link"; /* The id of the object */ attached_object.object.id = "box"; /* A default pose */ geometry_msgs::Pose pose; pose.orientation.w = 1.0; /* Define a box to be attached */ shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 0.1; primitive.dimensions[1] = 0.1; primitive.dimensions[2] = 0.1; attached_object.object.primitives.push_back(primitive); attached_object.object.primitive_poses.push_back(pose); 注意：附加对象到机器人要求指定正确对应的操作作为ADD操作。 attached_object.object.operation = attached_object.object.ADD; 增加对象到环境 增加对象到环境是通过增加到在规划场景的世界部分的冲突对象集合中来完成。 我们只使用attached_object消息的“object”区域 ROS_INFO("Adding the object into the world at the location of the right wrist."); moveit_msgs::PlanningScene planning_scene; planning_scene.world.collision_objects.push_back(attached_object.object); planning_scene.is_diff = true; planning_scene_diff_publisher.publish(planning_scene); sleep_time.sleep(); 插曲：同步和异步更新 通过diffs与move_group节点进行交互有两种机制： 通过rosservice调用发送一个diff，会阻塞直到diff被应用（同步更新） 通过主题发送一个diff，会持续发送，不能确定diff是否被应用（异步更新） 本教程选择异步更新机制(针对可视化插入长时间的睡眠，异步更新机制就不构成问题)。但是通过service客户端替代planning_scene_diff_publisher也是很合理的。 ros::ServiceClient planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene"); planning_scene_diff_client.waitForExistence(); 通过service 调用发送diffs到规划场景: moveit_msgs::ApplyPlanningScene srv; srv.request.scene = planning_scene; planning_scene_diff_client.call(srv); 注意：这里程序不会继续，直到我们确保diff被应用。 附加对象到机器人 当机器人从环境里捡起一个对象，我们就需要附加对象到机器人上。 因此，任何处理机器人模型的组件都知道要考虑到所附的对象，例如碰撞检查。 附加对象需要两个操作： 从环境中移除原始对象 附件对象到机器人 /* First, define the REMOVE object message*/ moveit_msgs::CollisionObject remove_object; remove_object.id = "box"; remove_object.header.frame_id = "odom_combined"; remove_object.operation = remove_object.REMOVE; 通过首先清楚这些区域来确保diff消息不包含其他的附加对象或冲突对象。 /* Carry out the REMOVE + ATTACH operation */ ROS_INFO("Attaching the object to the right wrist and removing it from the world."); planning_scene.world.collision_objects.clear(); planning_scene.world.collision_objects.push_back(remove_object); planning_scene.robot_state.attached_collision_objects.push_back(attached_object); planning_scene_diff_publisher.publish(planning_scene); sleep_time.sleep(); 从机器人上减少对象 从机器人上减少对象需要两个操作： 从机器人上减少对象 重新引入对象到环境中 /* First, define the DETACH object message*/ moveit_msgs::AttachedCollisionObject detach_object; detach_object.object.id = "box"; detach_object.link_name = "r_wrist_roll_link"; detach_object.object.operation = attached_object.object.REMOVE; 通过首先清楚这些区域来确保diff消息不包含其他的附加对象或冲突对象。 /* Carry out the DETACH + ADD operation */ ROS_INFO("Detaching the object from the robot and returning it to the world."); planning_scene.robot_state.attached_collision_objects.clear(); planning_scene.robot_state.attached_collision_objects.push_back(detach_object); planning_scene.world.collision_objects.clear(); planning_scene.world.collision_objects.push_back(attached_object.object); planning_scene_diff_publisher.publish(planning_scene); sleep_time.sleep(); 从冲突世界里删除对象 从冲突世界中删除对象只是需要使用较早定义的删除对象消息。值得注意的是如何清楚这些区域来确保diff消息不包含其他的附加对象或冲突对象。 ROS_INFO("Removing the object from the world."); planning_scene.robot_state.attached_collision_objects.clear(); planning_scene.world.collision_objects.clear(); planning_scene.world.collision_objects.push_back(remove_object); planning_scene_diff_publisher.publish(planning_scene); 完整代码 下载完整代码 编译代码 编译方法 launch文件 所需的launch文件 启动代码 roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch 理想输出 在Rviz，你可以看到： 对象显示在规划场景里 对象附加到机器人 对象从机器人删除 对象从规划场景删除