aubo i5 + realsense D435i识别抓取实践四

前言

前面已经通过手眼标定得到了相机与机械臂末端的位姿关系，然后又通过物体识别得到了物体在相机坐标系下的位姿，这样我们就可以得到物体相对机械臂末端或者物体相对机械臂基坐标的位姿，最后实现对物体的抓取。

一、抓取思路和流程

这里首先梳理一下抓取思路流程：

• 根据物体在相机下的位姿建立一个临时的工件坐标系，发布其相对机械臂基坐标的tf关系
• 根据抓取要求在工件坐标系下求得抓取准备点、抓取点、取走准备点的位姿
• 通过坐标转换关系将抓取准备点、抓取点、取走准备点转换为机械臂基坐标下的位姿
• 通过夹爪与机械臂末端之间的转换关系得到夹爪中心到达上述三个点时机械臂末端的位姿
• 最后通过moveit将上述三点逐一发布给机械臂执行，执行过程中对夹爪进行打开或夹紧操作，即可完成对物体的抓取。

二、部分实现代码

下面是个人实现的部分代码，仅供参考，存在不足之处还望批评指正。

1.结构体定义

• 下面是定义的抓取一个物体所需的动作点位的结构体，包括抓取动作所需要的各个点位，可以根据实际应用进行定义。

struct landPose
{
   
	geometry_msgs::Pose recognition_pose; //识别点
	geometry_msgs::Pose ready_pose; //准备点
	geometry_msgs::Pose grap_pose; //抓取点
	geometry_msgs::Pose up_pose; //抬起点
	geometry_msgs::Pose target_pose; //目标点
	geometry_msgs::Pose action_pose; //动作姿态
	geometry_msgs::Pose exit_pose; //退出点
};

• 这里定义了一个工件坐标系结构体：

struct tfStruct
{
   
	std::string link; //child link
	std::string base_link; //parent link
	tf::Transform transform; //transform
};

2.获取工件位置信息

• 订阅检测结果消息并保存工件坐标系结构体：

if (!result->recognized_objects.objects.empty())
{
   
	obj_num_ = result->recognized_objects.objects.size();
	for (int i = 0; i < obj_num_; i++)
	{
   
		geometry_msgs::PoseStamped pose;
		pose.pose = result->recognized_objects.objects.at(0).pose.pose.pose;
		pose.header = result->recognized_objects.objects.at(0).pose.header;
		
		//tf　transform
		geometry_msgs::PoseStamped base_pose;
		try
		{
   
			tf_.transformPose("base_link", pose, base_pose);
			ROS_INFO("camera_link:(%f,%f,%f,%f,%f,%f,%f)------>base_link:(%f,%f,%f,%f,%f,%f,%f).",
			pose.pose.position.x, pose.pose.position