ROS Industrial 软件包_笛卡尔路径规划器_实现2

笛卡尔路径规划_机器人焊接

机器人焊接示例，将说明如何可视化轨迹点，将碰撞对象添加到计划场景中，并说明了为什么应该对路径使用足够的轨迹点。

 

代码配置参考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>

   4

   5 // Include the visualisation message that will be used to

   6 // visualize the trajectory points in RViz

   7 #include <visualization_msgs/MarkerArray.h>

   8

前三行包含了本教程中将使用的不同库。 为了使主程序更具可读性，许多帮助功能被放在单独的程序包中。 这里包括的三个不同头文件包含帮助定义轨迹点，定义碰撞对象以及可视化轨迹点的功能。

最后的包含对于将必要的标记数组消息发送到RViz是必需的，该消息用于可视化RViz中的内容。

然后是几个函数声明。 函数定义可在程序末尾找到。 与上一教程相比，添加了一些额外的功能：

   1 descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(Eigen::Affine3d pose,

   2                        double rxTolerance, double ryTolerance, double rzTolerance);

   3

   4 /**

   5  * Waits for a subscriber to subscribe to a publisher

   6  */

   7 bool waitForSubscribers(ros::Publisher &pub, ros::Duration timeout);

   8

   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);

  14

  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;

   3

   4   addTable(planning_scene);

   5   addWeldingObject(planning_scene);

   6

   7   ros::Publisher scene_diff_publisher;

   8   scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

   9

  10   planning_scene.is_diff = true;

  11

  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;

  11

  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);

  18

  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);

   7

   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);

   3

   4   //Enable collision checking

   5   model->setCheckCollisions(true);

在创建机器人模型的第一行，创建了一个新的descartes_moveit :: IkFastMoveitStateAdapter。 我们需要使用该特定对象，因为在这种情况下，我们将使用IKfast求解器来计算逆运动学解。 这与上一教程中使用的KDL求解器不同。 IKfast求解器似乎更快，生成更多解决方案并且更准确。

声明模型后，我们打开碰撞检测(collision detection)。

剩余的代码在第一个教程https://blog.csdn.net/tuziaaa/article/details/103059741中进行了说明。

 

笛卡尔路径规划代码结构总结

1. 添加物体到规划场景
2. 定义一系列空间中的位姿点（空间轨迹）
3. 创建并初始化机器人模型
4. 创建并使用之前创建的机器人模型初始化规划器
5. 将先前定义的空间轨迹传递给规划器，并调用规划器进行规划
6. 将规划器规划的结果转换为ROS标准信息格式
7. 将ROS格式的空间轨迹传给机器人执行

实现效果

使用KUKA机器人(kr120)实现焊接过程中的避障

基于ROS控制KUKA机器人(kr120)实现焊接过程中的避障仿真