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，你可以看到： 对象显示在规划场景里 对象附加到机器人 对象从机器人删除 对象从规划场景删除
MoveIt!入门教程-运动规划(Motion Planners) 说明 在MoveIt，运动规划通过插件加载方式实现 这可以让MoveIt在运行过程中进行实时加载 使用 开始 在MoveIt，规划器是作为插件来安装的 可以通过ROS的pluginlib接口来加载需要的规划器 在加载规划器前，需要建立两个对象RobotModel和PlanningScene 首先实例化RobotModelLoader对象，它可从ROS parameter server查找robot description和构建RobotModel。 robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); 使用RobotModel，可构建PlanningScene，它可以保持世界和机器人的状态。 planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); 现在构建一个加载器通过指定规划器的名称来载入对应规划器 这里使用了ROS pluginlib库 boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; 从ROS param server获取规划器的名称。 载入规划器并确保能获取所有的异常 if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); try { planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager")); } catch(pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); } catch(pluginlib::PluginlibException& ex) { const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0 ; i < classes.size() ; ++i) ss << classes[i] << " "; ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } /* Sleep a little to allow time to startup rviz, etc. */ ros::WallDuration sleep_time(15.0); sleep_time.sleep(); 姿态目标 为PR2的右臂创建规划请求，指定终端执行器的期望的姿态 planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; pose.header.frame_id = "torso_lift_link"; pose.pose.position.x = 0.75; pose.pose.position.y = 0.0; pose.pose.position.z = 0.0; pose.pose.orientation.w = 1.0; 设置位置偏差是0.01m, 角度偏差是0.01度 std::vector<double> tolerance_pose(3, 0.01); std::vector<double> tolerance_angle(3, 0.01); 使用来自kinematic_constraints包help函数构建一个约束请求 req.group_name = "right_arm"; moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); 现在构建一个规划的上下文，封装场景，请求和反馈，我们调用的规划器会使用到这个规划的上下文。 planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } 可视化结果 代码： ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); moveit_msgs::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); sleep_time.sleep(); 节点空间目标 代码： /* First, set the state in the planning scene to the final state of the last plan */ robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst(); planning_scene->setCurrentState(response.trajectory_start); const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("right_arm"); robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); 构建目标 robot_state::RobotState goal_state(robot_model); std::vector<double> joint_values(7, 0.0); joint_values[0] = -2.0; joint_values[3] = -0.2; joint_values[5] = -0.15; goal_state.setJointGroupPositions(joint_model_group, joint_values); moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); 调用规划器并可视化运行轨迹 /* Re-construct the planning context */ context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); /* Call the Planner */ context->solve(res); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); /* Now you should see two planned trajectories in series*/ display_publisher.publish(display_trajectory); /* We will add more goals. But first, set the state in the planning scene to the final state of the last plan */ robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); /* Now, we go back to the first goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal); context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); 增加路径约束 再次设定姿态目标，这次要在运动规划里增加路径约束。 /* Let's create a new pose goal */ pose.pose.position.x = 0.65; pose.pose.position.y = -0.2; pose.pose.position.z = -0.1; moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle); /* First, set the state in the planning scene to the final state of the last plan */ robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); /* Now, let's try to move to this new pose goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal_2); /* But, let's impose a path constraint on the motion. Here, we are asking for the end-effector to stay level*/ geometry_msgs::QuaternionStamped quaternion; quaternion.header.frame_id = "torso_lift_link"; quaternion.quaternion.w = 1.0; req.path_constraints = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", quaternion); 设施路径约束要求规划器考虑终端执行器可能位置的空间（机器人的工作空间）。因此要指定允许的边界值。 注意：默认边界会自动被WorkspaceBounds请求适配器填充(OMPL管道的一部分，这个例子没用到) 我们使用一个边界，包含了机器臂可以到达空间，这样规划手臂运行时候采样就不会使用这个值。 边界只是用来确定采样的配置是否是有效 req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0; req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0; 调用规划器并可视化计划 context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); 现在可以看到四个规划的轨迹 display_publisher.publish(display_trajectory); 完整代码 下载完整代码 编译代码 编译方法 launch文件 下载launch文件 启动代码 roslaunch moveit_tutorials motion_planning_api_tutorial.launch 理想输出 在Rviz，我们应该能看到四的轨迹被重播： 机器人移动右臂到前面的姿态目标 机器人移动右臂到侧边的节点目标 机器人移动右臂回到之前的姿态目标 机器人保持终端执行器水平移动到新的姿态目标
MoveIt!入门教程-运动规划管道(传递途径) 说明 在MoveIt！，运动规划是设置计划路径 很多时候我们预处理运动规划请求和后处理已规划的路径（例如：时间参数化），这时候就要用到规划管道。 规划管道可以让运动规划连通预处理和后处理阶段。 预处理和后处理叫做规划请求适配器，可在ROS parameter server中通过名字来配置。 本教程利用C++演示，如何实例化并调用规划管道 使用 开始 在加载规划器前，需要建立两个对象RobotModel和PlanningScene 首先实例化RobotModelLoader对象，它可从ROS parameter server查找robot description和构建RobotModel。 robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); 使用RobotModel，可构建PlanningScene，它可以保持世界和机器人的状态。 planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); 现在建立PlanningPipeline对象，它使用ROS param server去确定请求适配器的集合和使用的规划插件。 planning_pipeline::PlanningPipelinePtr planning_pipeline(new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); /* Sleep a little to allow time to startup rviz, etc. */ ros::WallDuration sleep_time(20.0); sleep_time.sleep(); 姿态目标 为PR2的右臂创建规划请求，指定终端执行器的期望的姿态 planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; pose.header.frame_id = "torso_lift_link"; pose.pose.position.x = 0.75; pose.pose.position.y = 0.0; pose.pose.position.z = 0.0; pose.pose.orientation.w = 1.0; 设置位置偏差是0.01m, 角度偏差是0.01度 std::vector<double> tolerance_pose(3, 0.01); std::vector<double> tolerance_angle(3, 0.01); 使用来自kinematic_constraints包help函数构建一个约束请求 req.group_name = "right_arm"; moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); 调用管道检查规划是否成功 planning_pipeline->generatePlan(planning_scene, req, res); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } 可视化结果 代码： ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); moveit_msgs::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); sleep_time.sleep(); 节点空间目标 代码： /* First, set the state in the planning scene to the final state of the last plan */ robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst(); planning_scene->setCurrentState(response.trajectory_start); const robot_model::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("right_arm"); robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); 建立目标 robot_state::RobotState goal_state(robot_model); std::vector<double> joint_values(7, 0.0); joint_values[0] = -2.0; joint_values[3] = -0.2; joint_values[5] = -0.15; goal_state.setJointGroupPositions(joint_model_group, joint_values); moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); 调用管道并可视化结果 planning_pipeline->generatePlan(planning_scene, req, res); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); 现在你应该看到两个计划中的轨迹 display_publisher.publish(display_trajectory); sleep_time.sleep(); 使用规划请求适配器 一个规划请求适配器允许我们指定一系列的操作，应该发生在规划发生之前或之后的已经完成结果路径的规划 有针对性设置初始状态超出关节限制，让规划请求适配器来处理它。 /* First, set the state in the planning scene to the final state of the last plan */ robot_state = planning_scene->getCurrentStateNonConst(); planning_scene->setCurrentState(response.trajectory_start); robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); 设置一个关节稍微超出它的限制 const robot_model::JointModel* joint_model = joint_model_group->getJointModel("r_shoulder_pan_joint"); const robot_model::JointModel::Bounds& joint_bounds = joint_model->getVariableBounds(); std::vector<double> tmp_values(1, 0.0); tmp_values[0] = joint_bounds[0].min_position_ - 0.01; robot_state.setJointPositions(joint_model, tmp_values); req.goal_constraints.clear(); 再次调用规划器并可视化轨迹 planning_pipeline->generatePlan(planning_scene, req, res); if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); /* Now you should see three planned trajectories in series*/ display_publisher.publish(display_trajectory); sleep_time.sleep(); ROS_INFO("Done"); return 0; } req.goal_constraints.push_back(pose_goal); 完整代码 下载完整代码 编译代码 编译方法 launch文件 下载launch文件 启动代码 roslaunch moveit_tutorials motion_planning_api_tutorial.launch 理想输出 在Rviz, 我应该看到轨迹重播: 机器人移动右臂到前面的姿态目标 机器人移动右臂到侧边的节点目标 机器人移动右臂回前面的原姿态目标