ROS中控制机械臂抓取目标例程

在上一个博文中介绍了一个简单的目标识别的例子，在这篇博客中，例如是别的结果，完成机械臂的抓取控制，主要进行程序的分析和学习。

包含的头文件：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/PoseStamped.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_listener.h>

还是从主函数看起：

int main(int argc, char **argv)
{//初始化ROS节点，节点名为simple_graspingros::init(argc, argv, "simple_grasping");float length, breadth, pregrasp_x, pregrasp_y, pregrasp_z;//节点句柄ros::NodeHandle n;//获取参数，首先是桌子的长if (!n.getParam("probot_grasping/table_length", length))length = 0.3;//桌子的宽if (!n.getParam("probot_grasping/table_breadth", breadth))breadth = 0.3;//机械臂的初始位置，不让机械臂挡着视野，影响拍照if (!n.getParam("probot_grasping/pregrasp_x", pregrasp_x))pregrasp_x = 0.20;if (!n.getParam("probot_grasping/pregrasp_y", pregrasp_y))pregrasp_y = -0.17;if (!n.getParam("probot_grasping/pregrasp_z", pregrasp_z))pregrasp_z = 0.28;//创建一个对象，将参数传递进去GraspingDemo simGrasp(n, pregrasp_x, pregrasp_y, pregrasp_z, length, breadth);ROS_INFO_STREAM("Waiting for five seconds..");ros::WallDuration(5.0).sleep();//不断查看图像队列，如果有识别到的图像，则进行抓取while (ros::ok()){// Process image callbackros::spinOnce();//控制机械臂运动simGrasp.initiateGrasping();}return 0;
}

先看一下GraspingDemo类的内容：

首先看构造函数当中：

GraspingDemo::GraspingDemo(ros::NodeHandle n_, float pregrasp_x, float pregrasp_y, float pregrasp_z, float length, float breadth) :it_(n_), armgroup("manipulator"), grippergroup("gripper"), vMng_(length, breadth)
{this->nh_ = n_;//获取base_link和camera_link之间的关系，也就是手眼标定的结果try{this->tf_camera_to_robot.waitForTransform("/base_link", "/camera_link", ros::Time(0), ros::Duration(50.0));}catch (tf::TransformException &ex){ROS_ERROR("[adventure_tf]: (wait) %s", ex.what());ros::Duration(1.0).sleep();}//如果查询得到的话，就将结果保存到camera_to_robot_，保存x,y,z和四元数一共7个值try{this->tf_camera_to_robot.lookupTransform("/base_link", "/camera_link", ros::Time(0), (this->camera_to_robot_));}catch (tf::TransformException &ex){ROS_ERROR("[adventure_tf]: (lookup) %s", ex.what());}grasp_running = false;this->pregrasp_x = pregrasp_x;this->pregrasp_y = pregrasp_y;this->pregrasp_z = pregrasp_z;//让机械臂运动到初始的位置ros::AsyncSpinner spinner(1);spinner.start();ros::WallDuration(5.0).sleep();ROS_INFO_STREAM("Getting into the Grasping Position....");//调用该函数控制机械臂运动到设定的位置attainPosition(pregrasp_x, pregrasp_y, pregrasp_z);// Subscribe to input video feed and publish object location//订阅图像话题，一旦收到图像信息，就会进入到callback当中image_sub_ = it_.subscribe("/probot_anno/camera/image_raw", 1, &GraspingDemo::imageCb, this);
}

attainPosition函数：

void GraspingDemo::attainPosition(float x, float y, float z)
{// ROS_INFO("The attain position function called");// 获取当前位置geometry_msgs::PoseStamped currPose = armgroup.getCurrentPose();//初始化数据类型geometry_msgs::Pose target_pose1;target_pose1.orientation = currPose.pose.orientation;// 设置抓取前的机械臂位置target_pose1.position.x = x;target_pose1.position.y = y;target_pose1.position.z = z;armgroup.setPoseTarget(target_pose1);//机械臂运动armgroup.move();
}

看一下订阅图像的回调函数：

void GraspingDemo::imageCb(const sensor_msgs::ImageConstPtr &msg)
{if (!grasp_running){ROS_INFO_STREAM("Processing the Image to locate the Object...");//将图像变换到opencv当中try{cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);}catch (cv_bridge::Exception &e){ROS_ERROR("cv_bridge exception: %s", e.what());return;}//调用vision_manager中的函数获取目标的位置，位置坐标是以摄像头中心点的位置作为0坐标// ROS_INFO("Image Message Received");float obj_x, obj_y;vMng_.get2DLocation(cv_ptr->image, obj_x, obj_y);// Temporary Debuggingstd::cout<< " X-Co-ordinate in Camera Frame :" << obj_x << std::endl;std::cout<< " Y-Co-ordinate in Camera Frame :" << obj_y << std::endl;//通过坐标变换，将二维坐标变换为相机坐标系下的三维坐标，在本程序中与URDF建模有关系obj_camera_frame.setZ(-obj_y);obj_camera_frame.setY(-obj_x);obj_camera_frame.setX(0.45);//关键的一行代码，将相机坐标系下的位置转化为base_link坐标系下的坐标obj_robot_frame = camera_to_robot_ * obj_camera_frame;grasp_running = true;// Temporary Debuggingstd::cout<< " X-Co-ordinate in Robot Frame :" << obj_robot_frame.getX() << std::endl;std::cout<< " Y-Co-ordinate in Robot Frame :" << obj_robot_frame.getY() << std::endl;std::cout<< " Z-Co-ordinate in Robot Frame :" << obj_robot_frame.getZ() << std::endl;}
}

然后机械臂开始根据这个目标位置进行运动：

void GraspingDemo::initiateGrasping()
{//开启新的线程ros::AsyncSpinner spinner(1);spinner.start();ros::WallDuration(3.0).sleep();//获取当前的位置homePose = armgroup.getCurrentPose();//调用attainObject()函数使机械臂靠近目标ROS_INFO_STREAM("Approaching the Object....");attainObject();//夹取物体ROS_INFO_STREAM("Attempting to Grasp the Object now..");grasp();//夹住物体做一个小范围移动ROS_INFO_STREAM("Lifting the Object....");lift();//机械臂返回到初始状态ROS_INFO_STREAM("Going back to home position....");goHome();grasp_running = false;
}

看一下涉及到的三个函数：

靠近物体：

void GraspingDemo::attainObject()
{// ROS_INFO("The attain Object function called");attainPosition(obj_robot_frame.getX(), obj_robot_frame.getY(), obj_robot_frame.getZ() + 0.04);// Open Gripperros::WallDuration(1.0).sleep();grippergroup.setNamedTarget("open");grippergroup.move();// Slide down the Objectgeometry_msgs::PoseStamped currPose = armgroup.getCurrentPose();geometry_msgs::Pose target_pose1;target_pose1.orientation = currPose.pose.orientation;target_pose1.position = currPose.pose.position;target_pose1.position.z = obj_robot_frame.getZ() - 0.02;armgroup.setPoseTarget(target_pose1);armgroup.move();
}

抓取：

void GraspingDemo::grasp()
{// ROS_INFO("The Grasping function called");ros::WallDuration(1.0).sleep();grippergroup.setNamedTarget("close");grippergroup.move();
}

夹爪离开：

void GraspingDemo::lift()
{// ROS_INFO("The lift function called");// For getting the posegeometry_msgs::PoseStamped currPose = armgroup.getCurrentPose();geometry_msgs::Pose target_pose1;target_pose1.orientation = currPose.pose.orientation;target_pose1.position = currPose.pose.position;// Starting Postion after picking//target_pose1.position.z = target_pose1.position.z + 0.06;if(rand() % 2){target_pose1.position.y = target_pose1.position.y + 0.02;}else{target_pose1.position.y = target_pose1.position.y - 0.02;}armgroup.setPoseTarget(target_pose1);armgroup.move();// Open Gripperros::WallDuration(1.0).sleep();grippergroup.setNamedTarget("open");grippergroup.move();target_pose1.position.z = target_pose1.position.z + 0.06;armgroup.setPoseTarget(target_pose1);armgroup.move();
}

参考博客：古月居

ROS中控制机械臂抓取目标例程相关推荐

基于机器视觉的ROS机械臂抓取实验
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨曾三来源丨混沌无形点击进入->3D视觉工坊学习交流群摘要:为了减少机械臂在产品分类 ...
【实战+源码】基于RGB-D（深度视觉）的具有机械臂抓取功能的自主规划移动服务机器人的设计与实现（一）——准备工作
目录一,实物or仿真 1,实物或仿真的利弊 2,从哪些角度去考虑是选择实物还是仿真二,环境准备 1,首推ROS 2,其他环境三,理论学习四,实物搭建一,实物or仿真我想这个问题是在开发之前 ...
机械臂抓取学习笔记二
论文: Antipodal Robotic Grasping using Generative Residual Convolutional Neural Network 摘要: 本文提出了一个模块化 ...
基于深度学习的旋翼无人机机械臂抓取
基于深度学习的旋翼无人机机械臂抓取摘要:随着空中机器人技术的快速发展与日益成熟,无人机在越来越多的领域得到了广泛的应用.而多旋翼无人机作为最常用的无人机之一,以其体积小.运动灵活.定点悬停等优势广泛 ...
【详细讲解附全部代码】【openmv控制三自由度机械臂抓取物品】硬件+软件
前言: 这份代码很难得的是纯自己写的,虽然openmv梯子都搭成这样了也没什么大技术含量,只有一丢丢细小的逻辑.. 整体代码放在最后了,有需要的自取吧实现功能机械臂抓取一定范围内任意位置的物品,将 ...
vrep小车避障算法_[番外1]Vrep小车机械臂抓取
插一句,大家做仿真的时候一定要注意仿真的目的:仿真是为了尽可能的模拟实际的状态,使得得到的观测数据对实际实验具有指导作用.仿真并不是为了看起来像,而是要经过合理的抽象,真正的使仿真结果指导实际使用. ...
机械臂抓取学习笔记三
论文:Real-Time Deep Learning Approach to Visual Servo Control and Grasp Detection for Autonomous Robot ...
基于学习的机械臂抓取研究综述
1.基于学习的机械臂抓取研究综述 Kleeberger, K., Bormann, R., Kraus, W. et al. A Survey on Learning-Based Robotic Gr ...
【UbuntuROS】干货！小伙伴们快来拿，超全机械臂抓取开源项目！
一.基础入门 1.首先对机械臂的整体认识: http://blog.exbot.net/archives/3337 2.CSDN上一个博主写的抓取.机械臂控制.机械臂抓取的代码解析: https:// ...

ROS中控制机械臂抓取目标例程

ROS中控制机械臂抓取目标例程相关推荐

最新文章

热门文章