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

2024-04-28 04:58

本文主要是介绍ROS Industrial 软件包_笛卡尔路径规划器_实现,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

笛卡尔(Descarte)路径规划

使用笛卡尔包

使用笛卡尔包需要软件开发人员创建三个对象Obiects:

一个机器人模型(a robot model),将用来计算正向运动学和逆向运动学。

一个轨迹点的轨迹(a trajectory of trajectory points),用于定义路径。

一个规划器(a planner),将使用提供的机器人模型来完成沿着轨迹寻找有效路线的工作。

 

代码解读

源码地址

在descartes_tutorials/src/中,tutorial1.cpp是一个单个文件程序,该程序定义了一条简单的轨迹,使用Moveit创建了机器人模型规划轨迹并执行了它。

顶部是几个函数声明,源代码将在下文中详细解读。

 

   1 // Core ros functionality like ros::init and spin

   2 #include <ros/ros.h>

   3 // ROS Trajectory Action server definition

   4 #include <control_msgs/FollowJointTrajectoryAction.h>

   5 // Means by which we communicate with above action-server

   6 #include <actionlib/client/simple_action_client.h>

   7

头文件的第一部分包含了打印,定义轨迹和发送这些轨迹所需的基本ROS组件。 ROS-Industrial支持的大多数机器人都为仿真模拟实际硬件提供了FollowJointTrajectory动作(action)接口。

 

   8 // Includes the descartes robot model we will be using

   9 #include <descartes_moveit/moveit_state_adapter.h>

  10 // Includes the descartes trajectory type we will be using

  11 #include <descartes_trajectory/axial_symmetric_pt.h>

  12 #include <descartes_trajectory/cart_trajectory_pt.h>

  13 // Includes the planner we will be using

  14 #include <descartes_planner/dense_planner.h>

第二批标题头包含我们生成轨迹所需的所有笛卡尔特定文件。 moveit_state_adapter.h定义了笛卡尔机器人模型(RobotModel),而descartes_trajectory中的两个文件定义了轨迹点(Trajectory Points)的特定类型。 最后,densage_planner.h提供对笛卡尔规划器的访问。 有关特定组件的更多信息,请参见笛卡尔(descartes)。

 

  46 {

  47   // Initialize ROS

  48   ros::init(argc, argv, "descartes_tutorial");

  49   ros::NodeHandle nh;

  50

  51   // Required for communication with moveit components

  52   ros::AsyncSpinner spinner (1);

  53   spinner.start();

在使用ROS组件之前,我们必须通过调用ros :: init初始化节点。 我还将在此处创建一个NodeHandle,该节点将稍后用于从ROS参数服务器中检索组件。

51-52行创建一个新线程来处理在执行笛卡尔时可能需要处理的所有回调。 在没有微调器(spinner)的情况下初始化MoveIt似乎会引起问题。

 

  54   // 1. Define sequence of points

  55   TrajectoryVec points;

  56   for (unsigned int i = 0; i < 10; ++i)

  57   {

  58     Eigen::Affine3d pose;

  59     pose = Eigen::Translation3d(0.0, 0.0, 1.0 + 0.05 * i);

  60     descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);

  61     points.push_back(pt);

  62   }

  63

  64   for (unsigned int i = 0; i < 5; ++i)

  65   {

  66     Eigen::Affine3d pose;

  67     pose = Eigen::Translation3d(0.0, 0.04 * i, 1.3);

  68     descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);

  69     points.push_back(pt);

  70   }

在这里,我们使用本征变换(Eigen transformations)定义了一系列轨迹点。 这些转换定义了末端执行器的向上运动,然后是侧向平移。

makeTolerancedCartesianPoint函数是用于生成笛卡尔点的辅助函数,该笛卡尔点绕末端执行器框架(end effector frame)z轴的旋转不受限制。 求解图(最短路径)时,笛卡尔将尝试通过实质上围绕z轴旋转目标姿态来搜索有效的IK解。

makeTolerancedCartesianPoint的定义很简单,将在稍后讨论。

 

  72   // 2. Create a robot model and initialize it

  73   descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);

  74

  75   // Name of description on parameter server. Typically just "robot_description".

  76   const std::string robot_description = "robot_description";

  77

  78   // name of the kinematic group you defined when running MoveitSetupAssistant

  79   const std::string group_name = "manipulator";

  80

  81   // Name of frame in which you are expressing poses.

  82   // Typically "world_frame" or "base_link".

  83   const std::string world_frame = "base_link";

  84

  85   // tool center point frame (name of link associated with tool)

  86   const std::string tcp_frame = "tool0";

  87

  88   if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))

  89   {

  90     ROS_INFO("Could not initialize robot model");

  91     return -1;

在此代码块中,我们通过提供有关我们要控制的机器人的信息来创建和初始化机器人模型。使用提供的moveit_state_adapter的先决条件是,在运行此示例之前,必须已运行Moveit设置助手并至少创建了一个运动学组。

接下来,代码从ROS参数服务器中搜索robot_description参数以获取对机器人(或包含机器人的系统)的描述。group_name参数应与运行Moveit设置助手时创建的运动组的名称匹配。默认的ROS-Industrial软件包通常使用'manipulator'作为其默认组名。

world_frame参数指定您传入的位置姿势的参考框架。如果要指定相对于某个预定原点的全局位置,则该原点的名称是world_frame的参数(它也是URDF中的根框架)。如果这些点是相对于机器人基座或其他固定框架表示的,则输入基础链接(base link)或您要使用的任何其他框架的名称。

tcp_frame工具中心点框架(tool center point frame)是计算正向和反向运动学时使用的参考框架。它几乎始终是操纵器组中的最后一个链接(与名称group_name关联)。

 

  94   // 3. Create a planner and initialize it with our robot model

  95   descartes_planner::DensePlanner planner;

  96   planner.initialize(model);

  97

  98   // 4. Feed the trajectory to the planner

  99   if (!planner.planPath(points))

 100   {

 101     ROS_ERROR("Could not solve for a valid path");

 102     return -2;

 103   }

 104

 105   TrajectoryVec result;

 106   if (!planner.getPath(result))

 107   {

 108     ROS_ERROR("Could not retrieve path");

 109     return -3;

一旦定义了机器人模型和轨迹,规划轨迹就很简单。 创建你所选择的规划器(DensePlanner / Sparse Planner),使用刚刚创建的模型对其进行初始化(并自行初始化),并在传递所需轨迹的同时调用planPath函数。 此函数调用会进行大量计算以生成适当的关节轨迹,并且可能需要一段时间才能返回,具体取决于所分析计划的复杂性。

getPath函数用于检索结果轨迹。 为了使规划器尽可能通用,它返回多态类型TrajectoryPtPtr,但对于密集规划器和稀疏规划器而言,其基础类型都是JointTrajectoryPt 关节值本身将在稍后的toROSJointTrajectory函数中解压缩。

确保检查planPathgetPath函数的返回值是否正确,因为它们可能由于各种原因而失败,包括无法到达的点不平滑的联合轨迹

 

 112   // 5. Translate the result into a type that ROS understands

 113   // Get Joint Names

 114   std::vector<std::string> names;

 115   nh.getParam("controller_joint_names", names);

 116   // Generate a ROS joint trajectory with the result path, robot model,

 117   // joint names, and a certain time delta between each trajectory point

 118   trajectory_msgs::JointTrajectory joint_solution =

 119       toROSJointTrajectory(result, *model, names, 1.0);

 120

 121   // 6. Send the ROS trajectory to the robot for execution

 122   if (!executeTrajectory(joint_solution))

 123   {

 124     ROS_ERROR("Could not execute trajectory!");

 125     return -4;

这个最终的代码块将笛卡尔生成的关节轨迹转换为ROS可以理解的格式。 toROSJointTrajectory辅助函数执行此任务,但需要一些信息:

1.笛卡尔关节轨迹解

2.用于插补这些点的笛卡尔机器人模型

3.被主动操控的关节名称

4.每个关节位置之间的时间

114-115行从ROS参数服务器中检索教程中机器人的关节名称。

 

 

辅助函数

   1 descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose)

   2 {

   3   using namespace descartes_core;

   4   using namespace descartes_trajectory;

   5   return TrajectoryPtPtr(

new AxialSymmetricPt(pose, // Nominal pose

   6                            M_PI/2.0,           // Search discretization

   7                            AxialSymmetricPt::Z_AXIS) ); // Free axis                   

   8 }

makeTolerancedCartesianPoint是围绕笛卡尔的AxialSymmetricPt类的小型包装函数。 此类本身是笛卡尔点的一种特殊形式,其中的最后一个参数(此处为AxialSymmetricPt :: Z_AXIS)是工具可以围绕其自由旋转的名义姿态(nominal pose)的轴。 其他选项包括X_AXISY_AXIS

如果要创建固定的6DOF(6自由度)笛卡尔姿势,或者想要更好地控制可接受的公差,则可以创建具有相对应属性的笛卡尔点。

TrajectoryPtPtr是类型为descartes_core :: TrajectoryPtboost :: shared_ptr的别名。 共享指针是一种自动管理对象生存期的方法,在许多情况下,共享指针可以像普通指针一样使用。

 

   1 trajectory_msgs::JointTrajectory

   2 toROSJointTrajectory(const TrajectoryVec& trajectory,

   3                      const descartes_core::RobotModel& model,

   4                      const std::vector<std::string>& joint_names,

   5                      double time_delay)

   6 {

   7   // Fill out information about our trajectory

   8   trajectory_msgs::JointTrajectory result;

   9   result.header.stamp = ros::Time::now();

  10   result.header.frame_id = "world_frame";

  11   result.joint_names = joint_names;

  12

  13   // For keeping track of time-so-far in the trajectory

  14   double time_offset = 0.0;

  15   // Loop through the trajectory

  16   for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it)

  17   {

  18     // Find nominal joint solution at this point

  19     std::vector<double> joints;

  20     it->get()->getNominalJointPose(std::vector<double>(), model, joints);

  21

  22     // Fill out a ROS trajectory point

  23     trajectory_msgs::JointTrajectoryPoint pt;

  24     pt.positions = joints;

  25     // velocity, acceleration, and effort are given dummy values

  26     // we'll let the controller figure them out

  27     pt.velocities.resize(joints.size(), 0.0);

  28     pt.accelerations.resize(joints.size(), 0.0);

  29     pt.effort.resize(joints.size(), 0.0);

  30     // set the time into the trajectory

  31     pt.time_from_start = ros::Duration(time_offset);

  32     // increment time

  33     time_offset += time_delay;

  34

  35     result.points.push_back(pt);

  36   }

  37

  38   return result;

  39 }

该函数负责将笛卡尔轨迹转换为ROS trajectory_msgs :: JointTrajectory 与任何ROS消息类型一样,请确保填写标头信息,包括frame_id和时间戳。

笛卡尔的规划器返回轨迹点指针的向量,因此我们必须以多态方式访问基础数据。 稀疏和密集规划器返回的基础类型是关节轨迹点。 getNominalJointPose函数可用于提取所需的关节位置。 对于这种类型的点,函数的第一个参数只是一个哑元(虚设的参数) 接下来,我们调整速度,加速度和力度字段(effort fields)的大小,并将所有值设置为零,以使控制器不会拒绝我们的点,并自己计算出合适的值。

 最终,我们对轨迹中的时间进行了连续计数。 相对于轨迹的起点(而不是先前的点)指定ROS轨迹点的时间。

 

   1 bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory)

   2 {

   3   // Create a Follow Joint Trajectory Action Client

   4   actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("joint_trajectory_action", true);

   5   if (!ac.waitForServer(ros::Duration(2.0)))

   6   {

   7     ROS_ERROR("Could not connect to action server");

   8     return false;

   9   }

  10

  11   control_msgs::FollowJointTrajectoryGoal goal;

  12   goal.trajectory = trajectory;

  13   goal.goal_time_tolerance = ros::Duration(1.0);

  14   

  15   ac.sendGoal(goal);

  16

  17   if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5)))

  18   {

  19     ROS_INFO("Action server reported successful execution");

  20     return true;

  21   } else {

  22     ROS_WARN("Action server could not execute trajectory");

  23     return false;

  24   }

  25 }

executeTrajectory函数创建了FollowJointTrajectoryAction客户端,通过动作客户端调用ac.sendGoal(goal)函数调用动作服务器执行动作。

 

 

 

构建

descartes_tutorials存储库中的CMakeLists.txt显示了上面代码的最小示例。 将必要的配置添加到您的CMakeLists.txt,然后通过catkin_make/catkin build等进行构建。

 

 

这篇关于ROS Industrial 软件包_笛卡尔路径规划器_实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/942376

相关文章

hdu1043(八数码问题,广搜 + hash(实现状态压缩) )

利用康拓展开将一个排列映射成一个自然数,然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu

hdu2544(单源最短路径)

模板题: //题意:求1到n的最短路径,模板题#include<iostream>#include<algorithm>#include<cstring>#include<stack>#include<queue>#include<set>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#i

动态规划---打家劫舍

题目: 你是一个专业的小偷,计划偷窃沿街的房屋。每间房内都藏有一定的现金,影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统,如果两间相邻的房屋在同一晚上被小偷闯入,系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组,计算你 不触动警报装置的情况下 ,一夜之内能够偷窃到的最高金额。 思路: 动态规划五部曲: 1.确定dp数组及含义 dp数组是一维数组,dp[i]代表

【C++】_list常用方法解析及模拟实现

相信自己的力量,只要对自己始终保持信心,尽自己最大努力去完成任何事,就算事情最终结果是失败了,努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一:什么是list? •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二:list常用接口 •🌰1.默认成员函数 🔥构造函数(⭐) 🔥析构函数 •🌰2.list对象

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯,先赞后看哦~🎈🎈 🏆 作者简介:景天科技苑 🏆《头衔》:大厂架构师,华为云开发者社区专家博主,阿里云开发者社区专家博主,CSDN全栈领域优质创作者,掘金优秀博主,51CTO博客专家等。 🏆《博客》:Python全栈,前后端开发,小程序开发,人工智能,js逆向,App逆向,网络系统安全,数据分析,Django,fastapi

让树莓派智能语音助手实现定时提醒功能

最初的时候是想直接在rasa 的chatbot上实现,因为rasa本身是带有remindschedule模块的。不过经过一番折腾后,忽然发现,chatbot上实现的定时,语音助手不一定会有响应。因为,我目前语音助手的代码设置了长时间无应答会结束对话,这样一来,chatbot定时提醒的触发就不会被语音助手获悉。那怎么让语音助手也具有定时提醒功能呢? 我最后选择的方法是用threading.Time

Android实现任意版本设置默认的锁屏壁纸和桌面壁纸(两张壁纸可不一致)

客户有些需求需要设置默认壁纸和锁屏壁纸  在默认情况下 这两个壁纸是相同的  如果需要默认的锁屏壁纸和桌面壁纸不一样 需要额外修改 Android13实现 替换默认桌面壁纸: 将图片文件替换frameworks/base/core/res/res/drawable-nodpi/default_wallpaper.*  (注意不能是bmp格式) 替换默认锁屏壁纸: 将图片资源放入vendo

C#实战|大乐透选号器[6]:实现实时显示已选择的红蓝球数量

哈喽,你好啊,我是雷工。 关于大乐透选号器在前面已经记录了5篇笔记,这是第6篇; 接下来实现实时显示当前选中红球数量,蓝球数量; 以下为练习笔记。 01 效果演示 当选择和取消选择红球或蓝球时,在对应的位置显示实时已选择的红球、蓝球的数量; 02 标签名称 分别设置Label标签名称为:lblRedCount、lblBlueCount

Kubernetes PodSecurityPolicy:PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy:PSP能实现的5种主要安全策略 1. 特权模式限制2. 宿主机资源隔离3. 用户和组管理4. 权限提升控制5. SELinux配置 💖The Begin💖点点关注,收藏不迷路💖 Kubernetes的PodSecurityPolicy(PSP)是一个关键的安全特性,它在Pod创建之前实施安全策略,确保P

poj 1734 (floyd求最小环并打印路径)

题意: 求图中的一个最小环,并打印路径。 解析: ans 保存最小环长度。 一直wa,最后终于找到原因,inf开太大爆掉了。。。 虽然0x3f3f3f3f用memset好用,但是还是有局限性。 代码: #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#incl