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

aubo i5 + realsense D435i识别抓取实践四]
前言
一、抓取思路和流程
二、部分实现代码
1.结构体定义
2.获取工件位置信息
3.发布工件坐标系
4.计算各个关键点的姿态
5.机械臂初始化操作
6.机械臂执行
总结

前言

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

一、抓取思路和流程

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

根据物体在相机下的位姿建立一个临时的工件坐标系，发布其相对机械臂基坐标的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.y, pose.pose.position.z,
            pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w,
            base_pose.pose.position.x, base_pose.pose.position.y, base_pose.pose.position.z,
            base_pose.pose.orientation.x, base_pose.pose.orientation.y, base_pose.pose.orientation.z, base_pose.pose.orientation.w);
            //保存工件坐标系
            tf::Transform transform;
            std::string link = "target_";
            tfStruct tf_st;
            link.append(std::to_string(i));
            ROS_INFO("Creat a link named %s.", link.c_str());
            transform.setOrigin(tf::Vector3(base_pose.pose.position.x, base_pose.pose.position.y, base_pose.pose.position.z));
            transform.setRotation(tf::Quaternion(base_pose.pose.orientation.x, base_pose.pose.orientation.y, base_pose.pose.orientation.z, base_pose.pose.orientation.w));
            tf_st.link = link;
            tf_st.base_link = "base_link";
            tf_st.transform = transform;
            tf_v_.push_back(tf_st);
            //tb_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", link));
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR("Received an exception: %s", ex.what());
        }
    }
}
else
{
    obj_num_ = 0;
    ROS_WARN("Could not find target.");
}

3.发布工件坐标系

void tfThread()
{
    ros::Rate r(30);
    while (ros::ok())
    {
        {
            boost::lock_guard<boost::mutex> lock(mutex_);
            if (!tf_v_.empty())
            {
                for (int i = 0; i < tf_v_.size(); i++)
                {
                    //ROS_INFO("TF broadcaster a transform.");
                    tb_.sendTransform(tf::StampedTransform(tf_v_[i].transform, ros::Time::now(), tf_v_[i].base_link, tf_v_[i].link));
                }
            }
        }
        r.sleep();
    }
}

4.计算各个关键点的姿态

这里只有第一个准备点的姿态，其他类似，也可以将tf转换关系的部分封装，使得代码更简洁一些。

bool computePose(landPose &positions, std::string source_link, std::string base_link = "base_link", std::string target_link = "")
{
    geometry_msgs::PoseStamped source_pose;
    geometry_msgs::PoseStamped land_pose;
    source_pose.header.frame_id = source_link;

    tf::Quaternion q;
    q.setRPY(-1.57, 3.14, 0);
    //compute grap pose，相对工件坐标系的偏置
    source_pose.pose.position.x = 0.05;
    source_pose.pose.position.y = -0.085;
    source_pose.pose.position.z = 0.06;
    source_pose.pose.orientation.x = q.x();
    source_pose.pose.orientation.y = q.y();
    source_pose.pose.orientation.z = q.z();
    source_pose.pose.orientation.w = q.w();
    try
    {
        tf_.transformPose(base_link, source_pose, land_pose);
        positions.grap_pose = land_pose.pose;
    }
    catch (tf::TransformException &ex)
    {
        ROS_ERROR("Received an exception: %s", ex.what());
        return false;
    }
    return true;
}

5.机械臂初始化操作

move_group.setPoseReferenceFrame("base_link");
joint_model_group = move_group.getCurrentState()->getJointModelGroup("manipulator_i5");
visual_tools.deleteAllMarkers();
//Load remote control tool
visual_tools.loadRemoteControl();
move_group.setMaxVelocityScalingFactor(0.1);

text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 1.2;
visual_tools.publishText(text_pose, "AUBO Demo", rvt::RED, rvt::XLARGE);
// Text visualization takes effect
visual_tools.trigger();

6.机械臂执行

bool gotoPosition(geometry_msgs::Pose target_pose)
{
    move_group.setPoseTarget(target_pose);

    // Call the planner for planning calculations Note: This is just planning
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "Success" : "FAILED");
    if(!success)
    {
        return false;
    }
    // visual planning path in Rviz
    visual_tools.deleteAllMarkers();
    visual_tools.publishAxisLabeled(target_pose, "pose1");
    visual_tools.publishText(text_pose, "AUBO Pose Goal Example1", rvt::RED, rvt::XLARGE);
    // Parameter 1 (trajectory_): path information
    // Parameter 2 (JointModelGroup): Joint angle information and arm model information of the initial pose
    visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
    visual_tools.trigger();

    // Perform planning actions
    move_group.execute(my_plan);
    return true;
    //visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to go on");
}

最后编写一个执行流程，控制机械臂到对应点位执行对应动作即可。
这里手爪采用的是大寰AG-95手爪，官方提供了ros包，具体可参考：github
以上是部分源代码，仅供参考。具体怎么触发检测，怎么触发抓取这个可以添加server或者action的方式进行，演示的话server就可以，实际应用中最好使用action机制。

总结

至此aubo i5+realsense d435i识别抓取的全部工作流程就基本结束了，欢迎留言交流学习。