爱折腾-创客智造实验室 - MoveIt!入门教程
type: channel
link: https://www.ncnynl.com/category/ros-moveit
description: 通过MoveIt!入门教程学习，了解机械臂的工作原理，以及利用MoveIt来控制机械臂的运动, 或做运动规划等
items: MoveIt!入门教程-目录
items: MoveIt!入门教程-简介
items: MoveIt!入门教程-MoveIt安装
items: MoveIt!入门教程-配置助手(Setup Assistant)
items: MoveIt!入门教程-RViz插件
items: MoveIt!入门教程-Move Group接口
items: MoveIt!入门教程-Move Group Python接口
items: MoveIt!入门教程-运动学模型(Kinematic Model)
items: MoveIt!入门教程-规划场景(Planning Scene)
items: MoveIt!入门教程-ROS API规划场景

MoveIt!入门教程-目录
type: item
date: 2016-10-21T04:57:00+00:00
description: 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!入门教程-简介
type: item
date: 2016-10-28T02:15:00+00:00
description: 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（运动规划请求） 运动计划请求清楚地指定你想要的运动计划做什么。 通常情况下，你会要求运动规划器…

MoveIt!入门教程-MoveIt安装
type: item
date: 2016-10-28T03:30:00+00:00
description: 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)
type: item
date: 2016-10-28T03:31:00+00:00
description: 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。增加后效果如下： 增加机器人的姿态 配置助手允许你添加固定的姿态到配置文…

MoveIt!入门教程-RViz插件
type: item
date: 2016-10-28T03:31:00+00:00
description: 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接口
type: item
date: 2016-10-28T04:59:00+00:00
description: 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.getCurrentStat…

MoveIt!入门教程-Move Group Python接口
type: item
date: 2016-10-28T04:59:00+00:00
description: 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_traject…

MoveIt!入门教程-运动学模型(Kinematic Model)
type: item
date: 2016-10-28T04:59:00+00:00
description: 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 E…

MoveIt!入门教程-规划场景(Planning Scene)
type: item
date: 2016-10-28T05:00:00+00:00
description: 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_v…

MoveIt!入门教程-ROS API规划场景
type: item
date: 2016-10-28T05:00:00+00:00
description: 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::ApplyPlan…