笛卡尔路径规划_机器人焊接
机器人焊接示例,将说明如何可视化轨迹点,将碰撞对象添加到计划场景中,并说明了为什么应该对路径使用足够的轨迹点。
代码配置参考http://wiki.ros.org/descartes/Tutorials/Robot%20Welding%20With%20Descartes
示例运行
source setup.bash
首先运行
roslaunch descartes_tutorials kuka_sim.launch
这将打开RViz,使机器人处于原位(home position)。
运行上一个命令后,接下来可以运行tutorial2.launch文件,该文件将添加1多个碰撞对象,2可视化轨迹,对其3进行规划,最后4在仿真机器人上执行其路径。
roslaunch descartes_tutorials tutorial2.launch
源码分析
接下来将简要描述tutorial2.cpp文件中使用的代码。 我们将解释实现轨迹点可视化的方式,以及如何启用或禁用碰撞检测。
与上一教程相比,添加了一些include包含行:
1 #include <tutorial_utilities/path_generation.h>
2 #include <tutorial_utilities/collision_object_utils.h>
3 #include <tutorial_utilities/visualization.h>
5 // Include the visualisation message that will be used to
6 // visualize the trajectory points in RViz
7 #include <visualization_msgs/MarkerArray.h>
前三行包含了本教程中将使用的不同库。 为了使主程序更具可读性,许多帮助功能被放在单独的程序包中。 这里包括的三个不同头文件包含帮助定义轨迹点,定义碰撞对象以及可视化轨迹点的功能。
最后的包含对于将必要的标记数组消息发送到RViz是必需的,该消息用于可视化RViz中的内容。
然后是几个函数声明。 函数定义可在程序末尾找到。 与上一教程相比,添加了一些额外的功能:
1 descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(Eigen::Affine3d pose,
2 double rxTolerance, double ryTolerance, double rzTolerance);
4 /**
5 * Waits for a subscriber to subscribe to a publisher
6 */
7 bool waitForSubscribers(ros::Publisher &pub, ros::Duration timeout);
9 /**
10 * Add the welding object (l-profile) to the planning scene.
11 * This is put in a function to keep the tutorial more readable.
12 */
13 void addWeldingObject(moveit_msgs::PlanningScene &planningScene);
15 /**
16 * Add the welding table to the planning scene.
17 * This is put in a function to keep the tutorial more readable.
18 */
19 void addTable(moveit_msgs::PlanningScene &planningScene);
第一个函数是makeTolerancedCartesianPoint()函数的第二个声明。 该声明采用更多参数,允许用户定义围绕多个轴的允差。
waitForSubscribers()函数需要一个发布者和一个时间(以秒为单位)。 该功能至少等待订阅者设定的持续时间来订阅给定的发布者。 这可以帮助确保信息到达所需的终点。
addWeldingObject()和addTable()函数均用来将碰撞对象添加到仿真中。
然后,我们进入main()函数。 第一个增加的部分是添加碰撞对象:
1 // 0. Add objects to planning scene
2 moveit_msgs::PlanningScene planning_scene;
4 addTable(planning_scene);
5 addWeldingObject(planning_scene);
7 ros::Publisher scene_diff_publisher;
8 scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
10 planning_scene.is_diff = true;
12 ROS_INFO("Waiting for planning_scene subscriber.");
13 if (waitForSubscribers(scene_diff_publisher, ros::Duration(2.0)))
14 {
15 scene_diff_publisher.publish(planning_scene);
16 ros::spinOnce();
17 loop_rate.sleep();
18 ROS_INFO("Object added to the world.");
19 }
20 else
21 {
22 ROS_ERROR("No subscribers connected, collision object not added");
23 }
在此部分代码中,我们声明一个新的规划场景,然后使用辅助函数将工作台和工件都添加到场景中。 然后在“ planning_scene”主题(topic)上发布planning_scene。 使用waitForSubscriber()函数,我们等待planning_scene被读取,最多等待2秒钟。 读取新的计划场景后,对象应出现在RViz中。
作为示例,我们将简要介绍文档末尾的addTable()函数定义:
1 void addTable(moveit_msgs::PlanningScene &scene)
2 {
3 Eigen::Vector3d scale(1.0, 1.0, 1.0);
4 Eigen::Affine3d pose;
5 pose = descartes_core::utils::toFrame(1.5, -0.6, 0.0, 0.0, 0.0, 0.0, descartes_core::utils::EulerConventions::XYZ);
6 scene.world.collision_objects.push_back(
7 tutorial_utilities::makeCollisionObject("package://descartes_tutorials/meshes/table.stl", scale, "Table", pose));
8 scene.object_colors.push_back(tutorial_utilities::makeObjectColor("Table", 0.2, 0.2, 0.2, 1.0));
9 }
首先,使用矢量定义对象的缩放比例。 因为我们将在此处使用的网格已经正确缩放,所以我们将该比例设置为1。接下来,使用toFrame()函数,使用笛卡尔XYZ平移和XYZ Euler旋转定义姿态。 该姿势将定义碰撞对象的原点,通过此函数可以控制对象的位置和旋转。
然后,使用教程实用程序中的makeCollisionObject()创建对象。 然后,将该对象通过scene.world.collision_objects.push_back()添加到规划场景世界中的对象列表中。 最后一行用于为碰撞对象提供非标准颜色。
然后,我们继续定义轨迹点以及它们的可视化:
1 // 1. Define sequence of points
2 double x, y, z, rx, ry, rz;
3 x = 2.0 - 0.025;
4 y = 0.0;
5 z = 0.008 + 0.025;
6 rx = 0.0;
7 ry = (M_PI / 2) + M_PI / 4;
8 rz = 0.0;
9 TrajectoryVec points;
10 int N_points = 9;
12 std::vector<Eigen::Affine3d> poses;
13 Eigen::Affine3d startPose;
14 Eigen::Affine3d endPose;
15 startPose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ);
16 endPose = descartes_core::utils::toFrame(x, y + 0.4, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ);
17 poses = tutorial_utilities::line(startPose, endPose, N_points);
19 for (unsigned int i = 0; i < N_points; ++i)
20 {
21 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(poses[i], 0.0, M_PI_4, M_PI);
22 points.push_back(pt);
23 }
在本节代码中,我们首先声明一系列双精度变量,用于定义第一个轨迹点。我们定义框架的XYZ位置和XYZ Euler旋转。然后,我们使用这些变量来定义运动的开始姿势和结束姿势。这是使用toFrame()函数完成的。既然我们已经有了开始和结束框架,我们就可以使用tutorial_utilities :: line()函数沿着开始姿势和结束姿势之间的空间中的直线创建更多姿势。在该示例中,生成9个框架。
在for循环中,然后我们使用这些生成的姿势中的每一个,来生成有允差的笛卡尔轨迹点。轨迹点将添加到点列表中。为了生成轨迹点,我们使用makeTolerancedCartesianPoint(poses [i],0.0,0.4,M_PI).姿势之后的三个数字是允许的绕X,Y和Z轴的旋转公差大小。在这种情况下,我们允许绕Y轴的对称弧度为Π/4弧度,而绕Z轴的对称弧度为Π弧度。
接下来继续可视化上一段代码中生成的轨迹点:
1 // Visualize the trajectory points in RViz
2 // Transform the generated poses into a markerArray message that can be visualized by RViz
3 visualization_msgs::MarkerArray ma;
4 ma = tutorial_utilities::createMarkerArray(poses);
5 // Start the publisher for the Rviz Markers
6 ros::Publisher vis_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1);
8 // Wait for subscriber and publish the markerArray once the subscriber is found.
9 ROS_INFO("Waiting for marker subscribers.");
10 if (waitForSubscribers(vis_pub, ros::Duration(2.0)))
11 {
12 ROS_INFO("Subscriber found, publishing markers.");
13 vis_pub.publish(ma);
14 ros::spinOnce();
15 loop_rate.sleep();
16 }
17 else
18 {
19 ROS_ERROR("No subscribers connected, markers not published");
20 }
使用createMarkerArray(poses)函数,我们将姿势列表转换为所谓的“标记数组”。 顾名思义,标记数组包含一组标记,可以在订阅的某个主题上将其发送给RViz。 然后,RViz将可视化标记数组中的每个标记。 然后使用与发布规划场景相同的方法发布标记数组。
第三步是创建机器人模型并正确初始化。 本教程与上一个教程相比有两个区别:
1 // 2. Create a robot model and initialize it
2 descartes_core::RobotModelPtr model(new descartes_moveit::IkFastMoveitStateAdapter);
4 //Enable collision checking
5 model->setCheckCollisions(true);
在创建机器人模型的第一行,创建了一个新的descartes_moveit :: IkFastMoveitStateAdapter。 我们需要使用该特定对象,因为在这种情况下,我们将使用IKfast求解器来计算逆运动学解。 这与上一教程中使用的KDL求解器不同。 IKfast求解器似乎更快,生成更多解决方案并且更准确。
声明模型后,我们打开碰撞检测(collision detection)。
剩余的代码在第一个教程https://blog.csdn.net/tuziaaa/article/details/103059741中进行了说明。
笛卡尔路径规划代码结构总结
- 添加物体到规划场景
- 定义一系列空间中的位姿点(空间轨迹)
- 创建并初始化机器人模型
- 创建并使用之前创建的机器人模型初始化规划器
- 将先前定义的空间轨迹传递给规划器,并调用规划器进行规划
- 将规划器规划的结果转换为ROS标准信息格式
- 将ROS格式的空间轨迹传给机器人执行
实现效果
使用KUKA机器人(kr120)实现焊接过程中的避障
基于ROS控制KUKA机器人(kr120)实现焊接过程中的避障仿真