MoveIt教程[10]：Motion Planning Pipeline

在MoveIt中，motion planners被设置为规划路径。然而，在很多情况下，可能希望对运动规划请求进行预处理，或者对规划的路径进行后处理[例如，对时间参数化]。在这种情况下，使用规划管道，其中链的motion planner与预处理和后处理阶段。预处理和后处理阶段称为计划请求适配器，可以通过ROS参数服务器的名称进行配置。在本教程中，将运行C++代码来实例化和调用这样的规划管道。
一.Running the Code
Roslaunch launch文件运行代码直接从moveit_tutorials：

roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch

注意：本教程使用RvizVisualToolsGui面板逐步演示。要将此面板添加到RViz，请遵循可视化教程中的说明。
过一会儿，RViz窗口应该会出现，并且看起来与页面顶部的窗口相似。要完成每个演示步骤，要么按下屏幕底部RvizVisualToolsGui面板中的Next按钮，要么在屏幕顶部的Tools面板中选择Key Tool，然后在RViz聚焦时按键盘上的N。

二.Expected Output
在RViz中，应该能够看到三种最终被重放的轨迹：
[1]robot将它的右臂移动到它前面的姿势目标
[2]robot将右臂移动到关节目标的一侧
[3]robot将它的右臂移动回它前面的初始姿势目标

三.The Entire Code
全部代码参考文献[2]。
1.Start
设置开始使用规划管道非常简单。在加载规划器之前，需要两个对象，一个RobotModel和一个PlanningScene。
将从实例化一个RobotModelLoader对象开始，该对象将在ROS参数服务器上查找robot描述并构造一个供使用的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参数服务器来确定要使用的请求适配器集和planning插件。

planning_pipeline::PlanningPipelinePtr planning_pipeline(new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

2.Visualization
MoveItVisualTools包提供了许多在RViz中可视化对象、机器人和轨迹的功能，还提供了调试工具，比如脚本的分步内省。

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();/* Remote control is an introspection tool that allows users to step through a high level scriptvia buttons and keyboard shortcuts in RViz */
visual_tools.loadRemoteControl();/* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Motion Planning Pipeline Demo", rvt::WHITE, rvt::XLARGE);/* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
visual_tools.trigger();/* Sleep a little to allow time to startup rviz, etc..This ensures that visual_tools.prompt() isn't lost in a sea of logs*/
ros::Duration(10).sleep();/* We can also use visual_tools to wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

3.Pose Goal
现在，将为Panda的右臂创建一个运动计划请求，指定所需的末端执行器的姿势作为输入。

planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "panda_link0";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.75;
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包中提供的帮助函数创建请求作为约束。

req.group_name = "panda_arm";
moveit_msgs::Constraints pose_goal =kinematic_constraints::constructGoalConstraints("panda_link8", 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;
}

4.Visualize the result

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);/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

5.Joint Space Goals

/* 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("panda_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 = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
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);/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

6.Using a Planning Request Adapter
规划请求适配器允许指定一系列操作，这些操作应该在规划发生之前或在对结果路径进行规划之后发生。

/* 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("panda_joint3");
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();
req.goal_constraints.push_back(pose_goal);

再次调用planner，并可视化轨迹：

 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);/* Wait for user input */visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");ROS_INFO("Done");return 0;
}

四.The Launch File
全部launch文件参考文献[3]。本教程中的所有代码都可以从moveit_tutorials包[MoveIt的一部分]编译并运行设置。

参考文献：
[1]Motion Planning Pipeline：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.html
[2]motion_planning_pipeline：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/motion_planning_pipeline
[3]motion_planning_pipeline_tutorial：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch

MoveIt教程[10]：Motion Planning Pipeline相关推荐

MoveIt教程[9]：Motion Planning API
在MoveIt中,运动规划器加载使用插件基础设施.这允许MoveIt在运行时加载运动规划器.在本例中,将运行完成此任务所需的C++代码. 一.Running the Demo 打开两个shell.在第 ...
MoveIt! Tutorials, MoveIt! 教程——demo
MoveIt! Tutorials,gihub地址:https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/index. ...
MoveIt!教程 - MoveGroup接口
最新教程见MoveIt!教程在MoveIt!中主要的用户界面是通过MoveGroup类实现,为用户可能想要执行的大多数操作提供了易于使用的功能,特别是设置关节或姿态目标,创建运动计划,移动机器人,将 ...
路径规划(Path Planning)与运动规划(Motion Planning)
路径规划(Path Planning)与运动规划(Motion Planning) 评论区有个观点: 也可以把路径规划理解为运动规划的一份．我个人具体的认识是,特别是在实现的时候,路径规划我们强调的是 ...
Apollo进阶课程㉕丨Apollo规划技术详解——Optimization Inside Motion Planning
原文链接:进阶课程㉕丨Apollo规划技术详解--Optimization Inside Motion Planning 在自动驾驶软件的开发中,运动规划是最核心的模块之一.它将综合感知.定位和地图等 ...
Apollo进阶课程㉑丨Apollo规划技术详解——Basic Motion Planning and Overview
原文链接:进阶课程㉑丨Apollo规划技术详解--Basic Motion Planning and Overview 运动规划(Motion Planning)就是在给定的位置A与位置B之间为机器人 ...
Motion Planning中的问题与挑战
在自动驾驶系统中包含定位.感知.预测.决策规划和控制算法模块,其中决策规划模块相当于自动驾驶系统的大脑.SAE将自动驾驶分为L0-L5六个等级,随着自动驾驶等级的提供,决策规划模块的重要性也随之提高. ...
Motion planning for self-driving cars课程笔记1：应用雷达数据生成占用栅格地图（Occupancy Grid Map）
此文章为Motion planning for self-driving cars上第二课的笔记,主要讲述占据栅格地图的生成.由于课程中算法也是参考<probability robot>这 ...
高动态环境下基于随机可及集的Path-Guided APF算法的Motion Planning
文章目录摘要引言相关工作问题假设动态障碍物 Relative robot-obstacle dynamics(运动学) SR Sets for Collision Avoidance 方法! ...

MoveIt教程[10]：Motion Planning Pipeline

MoveIt教程[10]：Motion Planning Pipeline相关推荐

最新文章

热门文章