精华内容
下载资源
问答
  • 一种基于深度学习的机械臂抓取方法
  • 基于Kinect的机载机械手臂抓取方法研究
  • 机械臂抓取相关博客

    2021-01-15 20:04:01
    最前沿:机械臂抓取的落地进展与思考 https://zhuanlan.zhihu.com/p/142533861

    最前沿:机械臂抓取的落地进展与思考

    https://zhuanlan.zhihu.com/p/142533861

    展开全文
  • IntelRealSenseZR300+UR10RG2机械手臂+Linemod算法 机械臂抓取可乐罐 系列第六篇-附件资源
  • 机械臂抓取过程

    2018-09-30 17:54:52
    这份代码基于stm32芯片结合平衡小车之家的库函数开发的舵机机械臂抓取过程的。
  • halcon机械臂抓取程序

    2020-12-02 10:28:07
    利用halcon软件,6轴机械臂进行抓取螺丝的程序可结合halcon实例来进行手眼标定后的抓取任务。具体程序放在
  • 文章目录前言一、动图展示二、基于深度学习的机械臂抓取算法研究总结 前言 提示:这是本人从事研究基于时觉得机械臂抓取的第一个成功实验。物体识别和定位算法利用opencv颜色识别技术。 一、动图展示 二、基于...


    前言

    提示:这是本人从事研究基于时觉得机械臂抓取的第一个成功实验。物体识别和定位算法利用opencv颜色识别技术。


    一、前期工作

    1. 相机标定
    2. 手眼标定
    3. 位姿变换
    4. MoveIt
      在这里插入图片描述

    二、动图展示

    在这里插入图片描述

    二、基于深度学习的机械臂抓取算法研究

    ROS下基于物体识别的机器人抓取算法以及机械臂实物抓取实验

    总结

    联系方式见上文!!!

    展开全文
  • 提出了基于AdaBoost目标检测算法的机械臂抓取控制,当通过AdaBoost算法检测到目标物体,通过串口发送数据给单片机;单片机分析处理数据生成控制指令控制机械臂运动,实现了机械臂的动态抓取。
  • 实现功能 机械臂抓取一定范围内任意位置的物品,将其放到指定位置 物品以形状和颜色区分

    前言:
    这份代码很难得的是纯自己写的,虽然openmv梯子都搭成这样了也没什么大技术含量,只有一丢丢细小的逻辑。。
    整体代码放在最后了,有需要的自取吧

    实现功能

    机械臂抓取一定范围内任意位置的物品,将其放到指定位置
    物品以形状和颜色区分

    硬件清单

    1.openmv 以及数据线
    2.电源,可以使用充电宝替代
    3.机械臂一份,在淘宝十几块钱就可以买到
    4.小舵机3个,长的没问题就可以不需要太纠结型号
    5.杜邦线 之类的零件若干
    6.抓取的物品若干,由于便宜的机械臂摩擦力不够,可能需要稍微处理一下
    7.随便什么可以把openmv固定起来的杆,我使用的是硬纸壳粘在一起,使用起来没问题,你也可以尝试雪糕棒一类的东西

    实物图

    原谅我没好好拍图片。。只能在视频里面截取一下了
    示意图示意图

    整体程序设计思路

    识别部分

    由于需要同时识别形状和颜色,我的第一反应当然是星瞳上面找例程啦!官方确实有一个同时识别形状和颜色的例程.
    但是!
    经过我自己试验发现,关于形状的判断十分不准确,尤其我使用的海绵块本身边界不是那么的平整。。就导致 根!本!用!不!了!
    当然这种小问题难不倒我啦,官方的程序思路是先识别形状,然后使用“class Statistics – 统计数据对象”判断。

    官方代码如下:

    img = sensor.snapshot().lens_corr(1.8)
        for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,
                r_min = 2, r_max = 100, r_step = 2):
            area = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
            #area为识别到的圆的区域,即圆的外接矩形框
            statistics = img.get_statistics(roi=area)#像素颜色统计
            print(statistics)
            #(0,100,0,120,0,120)是红色的阈值,所以当区域内的众数(也就是最多的颜色),范围在这个阈值内,就说明是红色的圆。
            #l_mode(),a_mode(),b_mode()是L通道,A通道,B通道的众数。
            if 0<statistics.l_mode()<100 and 0<statistics.a_mode()<127 and 0<statistics.b_mode()<127:#if the circle is red
                img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))#识别到的红色圆形用红色的圆框出来
            else:
                img.draw_rectangle(area, color = (255, 255, 255))
                #将非红色的圆用白色的矩形框出来

    但是既然形状识别没有颜色识别靠谱。那咱们就改成先识别颜色再判断形状不就好啦?
    另外在这里我加入了一个变量记录区域被识别为圆还是方的次数,如果你需要考虑更多形状可以改成数组的形式储存次数。

    我的代码:

     blobs = img.find_blobs([yellow_threshold,green_threshold], roi=(0,0,160,80),pixels_threshold=100, area_threshold=100)# merge=True, margin=10
        if blobs:#如果找到了
            data=[]#初始化数据
            blob=find_max(blobs)#找最大
            color=blob.code()#记录颜色
            img.draw_rectangle(blob.x(),blob.y(),blob.w(),blob.h(), color = (255, 255, 255), thickness = 2, fill = False)
            #在色块周边一定范围内找圆
            circles= img.find_circles(threshold = 2600,r_min=10, x_margin = 10, y_margin = 10, r_margin = 10,
                        r_min = 2, r_max = 100, r_step = 2,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
            #在色块周边一定范围内找方
            rects = img.find_rects(threshold = 10000,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
            if circles:
                shape=shape+1
            if rects:
                shape=shape-1
            #上面两句会在一次抓取任务中对shape进行运算,shape表示该物品形状是圆还是方的概率,因此值理论上是-无穷到+无穷,实际不会太大,
            #正数表示更可能为圆,负数表示更可能为方,绝对值越大可能性越高,可以把中间值改成其他值以更好地提高准确率
            #由于openmv不能每一次都准确识别,因此采用这种方式显著提高了准确率。

    硬件连接设计

    我这里只用了3个舵机
    openmv2有2个舵机控制引脚
    openmv3有3个舵机控制引脚
    可以通过连接pca9685连接更多舵机,不需要担心引脚不够用
    顺便一提!openmv接屏幕 串口 舵机 等等外设多的时候特别容易引脚冲突!强烈建议买个pca9685备用,tb买单独的模块就行,比官方扩展板便宜很多,用法都一样两三句代码的事。

    我的代码接引脚是这样的(没有使用扩展板):

    s1 = Servo(1) # P7
    s2 = Servo(2) # P8
    s3 = Servo(3) # P9 Only for OpenMV3 M7
    #实例化3个舵机

    运动控制

    万能pid,我觉得不用多说了,我只用了一个p
    注意的是我把运动拆解成了拿取之前的运动和拿取之后的运动,因为拿取之后的运动基本固定的,只要判断几个参数就行。

    以下是拿取之前的运动:

    def move(s3_error,s2_error):#定义一个函数,作用是运动到目标物体位置
        kp=0.2
        global s2_now
        global s3_now #global关键字 用于全局变量
        s3_move = s3_error*kp+s3_now#pid计算
        if s3_move > 90:#这4句限制舵机运动角度,防止卡死
            s3_move = 90
        if s3_move < -90:
            s3_move = -90
        s3.angle(s3_move)#这一句是输出pwm控制舵机,采用openmv控制舵机时需要,采用arduino时不需要
        s3_now = s3_move#更新当前舵机角度存档
        if abs(s3_error)<10:
            s2_move = s2_error*kp+s2_now#pid计算
            if s2_move > -20:#这4句限制舵机运动角度,防止卡死
                s2_move = -20
            if s2_move < -90:
                s2_move = -90
                s2.angle(s2_move)#这一句是输出pwm控制舵机,采用openmv控制舵机时需要,采用arduino时不需要
                s2_now = s2_move

    拿取之后的运动我基本就是for循环加延时,

    判断应该放到哪里的代码如下:

     angle = 0#这个值表示目的角度
        if color == 1:#根据颜色判断
            angle = 90
        else :
            angle = -90
        if shape > 0:#根据形状判断
            if angle >0:
                angle = angle - 30
            else:
                angle = angle + 30

    需要注意的是:
    for循环设计上要考虑到目标位置与循环条件的关系!
    解决办法是写2个循环就可以了,每次只会执行符合的哪一个

    不多说,直接看代码就理解了:

    for i in range(s2_now,-20,-1):
            s2.angle(i)
            time.sleep(20)
        for i in range(s2_now,-20):
            s2.angle(i)
            time.sleep(20)

    关于机械臂硬件

    我在淘宝买的4自由度机械臂,强行改成3自由度
    如果你是4自由度,单独控制一下第四个舵机就行,差别不大,方法都一样的。

    如何get同款硬件?

    1.购买一个特别便宜的长得差不多就对了的机械臂 什么颜色都可以
    2.其他部分正常组装,只有控制高度的舵机不装
    3.使用一根剩下来的杆子固定高度
    4.差不多就完工了!具体堆栈我的图片就可以,这东西只要运动没问题就行。

    关于整体代码

    
    
    
    
    
    
    
    # 设计:沈阳航空航天大学 berry
    
    
    # 博客:https://blog.csdn.net/qq_45037925/article/details/105901893
    
    
    # 交流方式:私信我的博客即可
    
    
    
    
    
    
    import time
    
    
    from pyb import Servo
    import sensor, image, time
    
    
    from pyb import UART
    
    
    #导入需要的模块
    
    
    
    
    s1 = Servo(1) # P7
    
    
    s2 = Servo(2) # P8
    
    
    s3 = Servo(3) # P9 Only for OpenMV3 M7
    #实例化3个舵机
    
    
    uart = UART(3, 115200)#初始化串口
    
    
    s2_now = -90
    s3_now = 0
    
    
    
    
    red_threshold  = (21, 100, 36, 127, -9, 34)#颜色阈值1   0001(14, 100, 8, 127, 9, 116)
    blue_threshold  = ((20, 67, -52, -13, -36, -1))#颜色阈值2   0010
    yellow_threshold  = (14, 100, 22, 127, 42, 113)#颜色阈值4    0100
    green_threshold  = (20, 100, -128, -24, 17, 127)#颜色阈值8   1000
    
    
    
    
    
    
    
    
    
    
    sensor.reset() # Initialize the camera sensor.
    sensor.set_pixformat(sensor.RGB565) # use RGB565.
    sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
    sensor.skip_frames(10) # Let new settings take affect.
    sensor.set_auto_gain(False) # must be turned off for color tracking
    sensor.set_auto_whitebal(False) # turn this off.
    clock = time.clock() # Tracks FPS.
    
    
    
    
    #以上部分是每个程序差不多都有的初始化设定
    
    
    
    
    
    
    def move(s3_error,s2_error):#定义一个函数,作用是运动到目标物体位置
        kp=0.2
        global s2_now
        global s3_now #global关键字 用于全局变量
    
    
        s3_move = s3_error*kp+s3_now#pid计算
    
    
        if s3_move > 90:#这4句限制舵机运动角度,防止卡死
            s3_move = 90
        if s3_move < -90:
            s3_move = -90
        s3.angle(s3_move)#这一句是输出pwm控制舵机,采用openmv控制舵机时需要,采用arduino时不需要
        s3_now = s3_move#更新当前舵机角度存档
    
    
        if abs(s3_error)<10:
            s2_move = s2_error*kp+s2_now#pid计算
            if s2_move > -20:#这4句限制舵机运动角度,防止卡死
                s2_move = -20
            if s2_move < -90:
                s2_move = -90
                s2.angle(s2_move)#这一句是输出pwm控制舵机,采用openmv控制舵机时需要,采用arduino时不需要
                s2_now = s2_move
        #data.append((s2_now,s3_now))#这三句时串口输出舵机应转的角度,采用openmv控制舵机时不需要,采用arduino时需要
        #data_out = json.dumps(set(data))
        #uart.write(data_out +'\n')
    
    
    
    
    def lay(shape,color):#定义一个函数,用于把抓取的物品放到指定位置
        global s2_now#global关键字 用于全局变量
        global s3_now
        angle = 0#这个值表示目的角度
        if color == 1:#根据颜色判断
            angle = 90
        else :
            angle = -90
        if shape > 0:#根据形状判断
            if angle >0:
                angle = angle - 30
            else:
                angle = angle + 30
    
    
    
    
    
    
    #下面是机械臂把物品放到指定angle对应位置,并回到初始位置
        for i in range(s2_now,-20,-1):
    
    
    
    
            s2.angle(i)
    
    
            time.sleep(20)
    
    
        for i in range(s2_now,-20):
    
    
    
    
            s2.angle(i)
    
    
            time.sleep(20)
    
    
        for i in range(5,-19,-1):
    
    
            s1.angle(i)
            time.sleep(10)
    
    
        for i in range(-20,-90,-1):
    
    
    
    
            s2.angle(i)
    
    
            time.sleep(20)
    
    
        for i in range(s3_now,angle,-1):
    
    
    
    
            s3.angle(i)
    
    
            time.sleep(20)
    
    
        for i in range(s3_now,angle):
    
    
    
    
            s3.angle(i)
    
    
            time.sleep(20)
    
    
    
    
    
    
        for i in range(-90,-40):
    
    
    
    
            s2.angle(i)
    
    
            time.sleep(20)
    
    
    
    
        for i in range(-10,5):
    
    
            s1.angle(i)
            time.sleep(10)
    
    
        time.sleep(200)
        #data.append((s2_now,s3_now))
        #data_out = json.dumps(set(data))
        #uart.write(data_out +'\n')
    
    
    
    
        for i in range(-20,-90,-1):
    
    
    
    
            s2.angle(i)
    
    
            time.sleep(20)
    
    
    
    
        for i in range(angle,0,-1):
    
    
    
    
            s3.angle(i)
    
    
            time.sleep(20)
    
    
        for i in range(angle,0):
    
    
    
    
            s3.angle(i)
    
    
            time.sleep(20)
        time.sleep(2000)
    
    
        s2.angle(-90)
        s2_now = -90
        s3_now = angle
        #data.append((s2_now,s3_now))
        #data_out = json.dumps(set(data))
        #uart.write(data_out +'\n')
    
    
    
    def find_max(blobs):#定义一个找最大色块的函数,输入一个色块对象列表,输出一个色块对象
        max_size=0
        for blob in blobs:
            if blob[2]*blob[3] > max_size:
                max_blob=blob
                max_size = blob[2]*blob[3]
        return max_blob
    
    
    def find_max_c(blobs):#定义一个找最大圆的函数,输入一个圆对象列表,输出一个圆对象
        max_size=0
        for blob in blobs:
            if blob[2] > max_size:
                max_blob=blob
                max_size = blob[2]
        return max_blob
    
    
    
    s3.angle(s3_now)#初始化舵机
    s2.angle(s2_now)
    shape=0#记录形状和颜色
    color=0
    time.sleep(2000)
    #下面是循环执行的部分
    while(True):
        clock.tick() # Track elapsed milliseconds between snapshots().
        img = sensor.snapshot().lens_corr(1.8)#拍摄一张照片,并畸变矫正
        claps = img.find_blobs([red_threshold])#寻找色块
    
    
    
    
        if claps:
            clap=find_max(claps)
            img.draw_cross(clap.x(), clap.y(), color = (0, 255, 0), size = 10, thickness = 2)#画一个十字
    
    
        #寻找色块
        blobs = img.find_blobs([yellow_threshold,green_threshold], roi=(0,0,160,80),pixels_threshold=100, area_threshold=100)# merge=True, margin=10
        if blobs:#如果找到了
            data=[]#初始化数据
            blob=find_max(blobs)#找最大
            color=blob.code()#记录颜色
            img.draw_rectangle(blob.x(),blob.y(),blob.w(),blob.h(), color = (255, 255, 255), thickness = 2, fill = False)
            #在色块周边一定范围内找圆
            circles= img.find_circles(threshold = 2600,r_min=10, x_margin = 10, y_margin = 10, r_margin = 10,
                        r_min = 2, r_max = 100, r_step = 2,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
            #在色块周边一定范围内找方
            rects = img.find_rects(threshold = 10000,roi=(blob.x(),blob.y(),blob.w(),blob.h()))
            if circles:
                shape=shape+1
            if rects:
                shape=shape-1
            #上面两句会在一次抓取任务中对shape进行运算,shape表示该物品形状是圆还是方的概率,因此值理论上是-无穷到+无穷,实际不会太大,
            #正数表示更可能为圆,负数表示更可能为方,绝对值越大可能性越高,可以把中间值改成其他值以更好地提高准确率
            #由于openmv不能每一次都准确识别,因此采用这种方式显著提高了准确率。
            tx=blob.cx()
            ty=blob.cy()
            #s3_error = clap.cx()-tx#计算error
            #s2_error = clap.cy()-ty
            s3_error = 78-tx#计算error
            s2_error = 44-ty
            move(s3_error,s2_error)#使用前面定义的函数
            if abs(s3_error)<10:
                lay(shape,color)#使用前面定义的函数
                shape=0#清除标志
                color=0
    
    
    
            #s1.angle(5)打开#s1.angle(-10)闭合
            #s2.angle(-90)#前s2.angle(-20)后
            #s3.angle(-90)#左 s3.angle(0)#中s3.angle(90)#右
    
    
    

    后话

    这个程序其实有一个不完美的地方,就是。。它不会主动判断夹取是否成功,没写的原因是我懒。。
    简单写一下我的思路吧,

    思路:
    1.在原来程序基础上,在夹取完成之后每xx时间(可以使用定时器也可以穿插在之后的程序中)检测一下屏幕中间一定区域的颜色,如果不符合预期就重新寻找一次色块,如果发现色块就重新回去取色块
    2.使用帧差分啥的不知道行不行

    展开全文
  • 实现机械臂抓取Marker,并放置到指定位置1. Version-1 Program2. Version-2 Program 1. Version-1 Program 实现了kinova的end effector随Marker的移动而移动 仍需改进的地方 (1)代码结构凌乱,可以引入类来...

    实现机械臂抓取Marker,并放置到指定位置

    1. Version-1 Program

    1. 实现了kinova的end effector随Marker的移动而移动
    2. 仍需改进的地方
      (1)代码结构凌乱,可以引入类来提高代码的整洁与可读性。
      (2)实现kinova抓取Marker并可以将其放置到指定位置的demo
    #include "ros/ros.h"
    #include<iostream> //C++标准输入输出库 
    #include <vector>
    #include "std_msgs/String.h"
    #include "geometry_msgs/PoseStamped.h"
    #include <kinova_msgs/ArmPoseActionGoal.h> // 注意格式源文件名字为ArmPose.action
    #include <ros/ros.h>
    #include <kinova_driver/kinova_ros_types.h>
    
    #include <actionlib/client/simple_action_client.h>
    #include <kinova_msgs/SetFingersPositionAction.h>
    #include <kinova_msgs/ArmPoseAction.h>
    #include <kinova_msgs/HomeArm.h>
    #include <kinova_msgs/ArmJointAnglesAction.h>
    
    const int FINGER_MAX = 6400;
    
    bool cartesian_pose_client(float x, float y, float z, float qx, float qy, float qz, float qw)//四元数
    {
    
    
        actionlib::SimpleActionClient <kinova_msgs::ArmPoseAction> *client = new actionlib::SimpleActionClient<kinova_msgs::ArmPoseAction>(
                "/j2n6s300_driver/pose_action/tool_pose");//取的主题
    
        client->waitForServer();
    
        kinova_msgs::ArmPoseGoal pose_goal;//注意形式ArmPose.action
        pose_goal.pose.header.frame_id = "j2n6s300_link_base";
    
        pose_goal.pose.pose.position.x = x;
        pose_goal.pose.pose.position.y = y;
        pose_goal.pose.pose.position.z = z;
    
        pose_goal.pose.pose.orientation.x = qx;
        pose_goal.pose.pose.orientation.y = qy;
        pose_goal.pose.pose.orientation.z = qz;
        pose_goal.pose.pose.orientation.w = qw;
    
    
        client->sendGoal(pose_goal);
    
    
        if (client->waitForResult(ros::Duration(150.0)))//延时得高机械臂不会发送,只能自己检测坐标,奇异值也不会告诉所有信息都通过主题反馈
        {
            client->getResult();
            return true;
        } else {
            client->cancelAllGoals();
            ROS_WARN_STREAM("The gripper action timed-out");
            return false;
        }
    
    }
    
    bool finger_control_client(float finger1, float finger2, float finger3)//0-1
    {
    
    
        actionlib::SimpleActionClient <kinova_msgs::SetFingersPositionAction> *finger_client = new actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction>(
                "j2n6s300_driver/fingers_action/finger_positions");
    
        finger_client->waitForServer();
    
        kinova_msgs::SetFingersPositionGoal finger_goal;
    
        finger_goal.fingers.finger1 = finger1 * FINGER_MAX;
        finger_goal.fingers.finger2 = finger2 * FINGER_MAX;
        finger_goal.fingers.finger3 = finger3 * FINGER_MAX;
    
    
        finger_client->sendGoal(finger_goal);
    
    
        if (finger_client->waitForResult(ros::Duration(100.0))) {
            finger_client->getResult();
            return true;
        } else {
            finger_client->cancelAllGoals();
            ROS_WARN_STREAM("The gripper action timed-out");
            return false;
        }
    
    }
    
    void MarkerCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        float Tf_Mat[4][4];
    
        const float tf[16] = {0.9998, -0.0143, 0.0161, -0.002768, -0.0149, -0.9991, 0.0402, -0.56808, 0.0155, -0.0404, -0.9991, 1.09819, 0, 0, 0, 1};
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                Tf_Mat[i][j] = tf[i*4+j];
    
            }
        }
    //    ROS_INFO("I heard the pose from the robot"); 
        ROS_INFO("the marker pose with cam_frame position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    
    
        std::cout<<"\n \n"<<std::endl; 
        float x, y, z, qx, qy, qz, qw, finger1, finger2, finger3, p[4], pm[4] = {msg->pose.position.x, msg->pose.position.y,  msg->pose.position.z, 1};
        ROS_INFO("the marker pose with pm position(x,y,z) is %f , %f, %f", pm[0], pm[1], pm[2]);
    
        for (int i = 0; i < 4; ++i) {
            p[i] = Tf_Mat[i][0]*pm[0]+Tf_Mat[i][1]*pm[1]+Tf_Mat[i][2]*pm[2]+Tf_Mat[i][3]*pm[3];
        }
        x=p[0];
        y=p[1];//y可能需要补偿4cm y的原值为-0.528081440518
        z=0.2582;
        qx=0.999739147086;
        qy=-0.00729168962601;
        qz=0.00791491117064;
        qw=0.0201450546611;
        finger1 = finger2 = 0;
        finger3 = 0;
    
    
        try {
    
            bool result = cartesian_pose_client(x, y, z, qx, qy, qz, qw);
            if (result) {
                bool finger_re = finger_control_client(finger1, finger2, finger3);
    
                ROS_INFO("Cartesian pose sent! %f , %f, %f", x, y, z);
                if (finger_re)ROS_INFO("finger move!.");
            }
        }
        catch (ros::Exception) {
            ROS_INFO("program interrupted before completion.");
        }
    }
    
    int main(int argc, char **argv) {
    
    
        ros::init(argc, argv, "paragram1");
        ros::NodeHandle node_handle;
    
        ros::ServiceClient start_to_home = node_handle.serviceClient<kinova_msgs::HomeArm>("j2n6s300_driver/in/home_arm");
    
        kinova_msgs::HomeArm srv;
    
        try {
    
            start_to_home.call(srv);
            ROS_INFO("go to home");
    
        }
        catch (ros::Exception) {
            ROS_INFO("Failed to HOme.");
        }
        ros::Subscriber Marke_sub = node_handle.subscribe("aruco_single/pose", 1, MarkerCallback);
        ros::spin();
        return 0;
    }
    
    

    2. Version-2 Program

    1. Record several kinova end effector oriention:
      (1) 记录了一个当前的Marker
      在这里插入图片描述
      (2)End effector竖直向下的oriention:
      qx=0.999739147086;
      qy=-0.00729168962601;
      qz=0.00791491117064;
      qw=0.0201450546611;

      (3) End effector 水平向前的oriention:
      在这里插入图片描述
      (4) Marker 最终放置位置:
      在这里插入图片描述

    代码:

    #include "ros/ros.h"
    #include <cstdlib> 
    #include "std_msgs/Float64.h"
    #include <unistd.h>
    #include<iostream> //C++标准输入输出库 
    #include <vector>
    #include "std_msgs/String.h"
    #include "geometry_msgs/PoseStamped.h"
    #include <kinova_msgs/ArmPoseActionGoal.h> // 注意格式源文件名字为ArmPose.action
    #include <ros/ros.h>
    #include <kinova_driver/kinova_ros_types.h>
    
    #include <actionlib/client/simple_action_client.h>
    #include <kinova_msgs/SetFingersPositionAction.h>
    #include <kinova_msgs/ArmPoseAction.h>
    #include <kinova_msgs/HomeArm.h>
    #include <kinova_msgs/ArmJointAnglesAction.h>
    
    const int FINGER_MAX = 6400;
    
    class tl1{
    public:
        tl1();
        void registerNodeHandle(ros::NodeHandle& _nh);
        void registerSub();
        void registerSrv();
        void MarkerCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
        bool cartesian_pose_client(float x, float y, float z, float qx, float qy, float qz, float qw);
        bool finger_control_client(float finger1, float finger2, float finger3);
        void GotoHome(kinova_msgs::HomeArm& _srv);
    private:
        ros::Subscriber Marke_sub;
        ros::NodeHandle nh;
        ros::ServiceClient start_to_home;
    };
    
    int main(int argc, char **argv) {
    
    
        ros::init(argc, argv, "paragram1");
        ros::NodeHandle node_handle;
    
        tl1 loandmove;
        kinova_msgs::HomeArm srv;
    
        loandmove.registerNodeHandle(node_handle);
    
        loandmove.registerSrv();
        loandmove.GotoHome(srv);
        loandmove.registerSub();
    
    
    
    
    
        ros::spin();
        return 0;
    }
    
    
    tl1::tl1(){};
    
    void tl1::registerNodeHandle(ros::NodeHandle& _nh){
        nh = _nh;
    };
    
    void tl1::registerSub(){
    
        Marke_sub = nh.subscribe("aruco_single/pose", 1, &tl1::MarkerCallback, this); //this pointer here means the class itself
    
    
    };
    
    void tl1::registerSrv(){
    
        start_to_home = nh.serviceClient<kinova_msgs::HomeArm>("j2n6s300_driver/in/home_arm");
    };
    bool tl1::cartesian_pose_client(float x, float y, float z, float qx, float qy, float qz, float qw)//四元数
    {
        actionlib::SimpleActionClient <kinova_msgs::ArmPoseAction> *client = new actionlib::SimpleActionClient<kinova_msgs::ArmPoseAction>(
                "/j2n6s300_driver/pose_action/tool_pose");//取的主题
    
        client->waitForServer();
    
        kinova_msgs::ArmPoseGoal pose_goal;//注意形式ArmPose.action
        pose_goal.pose.header.frame_id = "j2n6s300_link_base";
    
        pose_goal.pose.pose.position.x = x;
        pose_goal.pose.pose.position.y = y;
        pose_goal.pose.pose.position.z = z;
    
        pose_goal.pose.pose.orientation.x = qx;
        pose_goal.pose.pose.orientation.y = qy;
        pose_goal.pose.pose.orientation.z = qz;
        pose_goal.pose.pose.orientation.w = qw;
    
    
        client->sendGoal(pose_goal);
    
    
        if (client->waitForResult(ros::Duration(200.0)))//延时得高机械臂不会发送,只能自己检测坐标,奇异值也不会告诉所有信息都通过主题反馈
        {
            client->getResult();
            return true;
        } else {
            client->cancelAllGoals();
            ROS_WARN_STREAM("The gripper action timed-out");
            return false;
        }
    
    }
    
    bool tl1::finger_control_client(float finger1, float finger2, float finger3)//0-1
    {
    
    
        actionlib::SimpleActionClient <kinova_msgs::SetFingersPositionAction> *finger_client = new actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction>(
                "j2n6s300_driver/fingers_action/finger_positions");
    
        finger_client->waitForServer();
    
        kinova_msgs::SetFingersPositionGoal finger_goal;
    
        finger_goal.fingers.finger1 = finger1 * FINGER_MAX;
        finger_goal.fingers.finger2 = finger2 * FINGER_MAX;
        finger_goal.fingers.finger3 = finger3 * FINGER_MAX;
    
    
        finger_client->sendGoal(finger_goal);
    
    
        if (finger_client->waitForResult(ros::Duration(200.0))) {
            finger_client->getResult();
            return true;
        } else {
            finger_client->cancelAllGoals();
            ROS_WARN_STREAM("The gripper action timed-out");
            return false;
        }
    
    }
    
    void tl1::MarkerCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        float Tf_Mat[4][4];
    
        const float tf[16] = {0.9998, -0.0143, 0.0161, -0.002768, -0.0149, -0.9991, 0.0402, -0.56808, 0.0155, -0.0404, -0.9991, 1.09819, 0, 0, 0, 1};
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                Tf_Mat[i][j] = tf[i*4+j];
    
            }
        }
    
        ROS_INFO("the marker pose with cam_frame position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    
        std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly
    
        float x, y, z, qx, qy, qz, qw, finger1, finger2, finger3, p[4], pm[4] = {msg->pose.position.x, msg->pose.position.y,  msg->pose.position.z, 1};
        ROS_INFO("the marker pose with pm position(x,y,z) is %f , %f, %f", pm[0], pm[1], pm[2]);
    
        for (int i = 0; i < 4; ++i) {
            p[i] = Tf_Mat[i][0]*pm[0]+Tf_Mat[i][1]*pm[1]+Tf_Mat[i][2]*pm[2]+Tf_Mat[i][3]*pm[3];
        }
        x=p[0];
        y=p[1];//y可能需要补偿4cm y的原值为-0.528081440518
        z=p[2]+0.1;
        qx=0.7041;
        qy=0.01358;
        qz=0.07847;
        qw=0.7056;
        finger1 = finger2 = finger3 = 0;
    
    
        try {
    
            bool result = cartesian_pose_client(x, y, z, qx, qy, qz, qw);
            if (result) {
                bool finger_re = finger_control_client(finger1, finger2, finger3);
    
                ROS_INFO("Move to Marker up! %f , %f, %f", x, y, z);
                if (finger_re)ROS_INFO("finger move!.");
            }
        }
        catch (ros::Exception) {
            ROS_INFO("program interrupted before completion.");
        }
    
        x=p[0];
        y=p[1];//y可能需要补偿4cm y的原值为-0.528081440518
        z=p[2]-0.03;
        qx=0.7041;
        qy=0.01358;
        qz=0.07847;
        qw=0.7056;
        finger1 = finger2 = finger3 = 1;
    
    
        try {
    
            bool result = cartesian_pose_client(x, y, z, qx, qy, qz, qw);
            if (result) {
                bool finger_re = finger_control_client(finger1, finger2, finger3);
    
                ROS_INFO("Catch the Marker! %f , %f, %f", x, y, z);
                if (finger_re)ROS_INFO("finger move!.");
            }
        }
        catch (ros::Exception) {
            ROS_INFO("program interrupted before completion.");
        }
    
        kinova_msgs::HomeArm srv;
        
        GotoHome(srv);
    
        x= 0.472484827042;
        y= -0.172647804022;
        z= 0.0302095841616;
    
        qx= -0.0346102192998;
        qy= -0.999143242836;
        qz= -0.00137171335518;
        qw= -0.0226511508226;
        finger1 = finger2 = finger3 = 0;
    
    
        try {
    
            bool result = cartesian_pose_client(x, y, z, qx, qy, qz, qw);
            if (result) {
                bool finger_re = finger_control_client(finger1, finger2, finger3);
    
                ROS_INFO("Marker move to final pose! %f , %f, %f", x, y, z);
                if (finger_re)ROS_INFO("finger open!.");
            }
        }
        catch (ros::Exception) {
            ROS_INFO("program interrupted before completion.");
        }
    exit(0);
    }
    
    void tl1::GotoHome(kinova_msgs::HomeArm& _srv)
    {
    
        kinova_msgs::HomeArm srv;
    
        try {
    
            start_to_home.call(srv);
            ROS_INFO("go to home");
    
        }
        catch (ros::Exception) {
            ROS_INFO("Failed to HOme.");
        }
    }
    
    1. 代码更加简练,把几乎所有的函数变量都定义到类里面。
    2. 主题订阅中的函数callback仍然太繁琐,仍然需要精简!
    展开全文
  • ROS中控制机械臂抓取目标例程

    万次阅读 多人点赞 2019-06-09 14:41:43
    在上一个博文中介绍了一个简单的目标识别的例子,在这篇博客中,例如是别的结果,完成机械臂抓取控制,主要进行程序的分析和学习。 包含的头文件: #include <ros/ros.h> #include <image_transport/...
  • 首先根据数控机械臂系统的自动控制功能要求,通过kinect等双目定位摄像头自动拍摄一段视频或背景图片,然后存储到视频存储器中,通过视频图像信息处理技术系统自动去除视频背景图像信息,可以用于抓取目标物体,可以...
  • halcon结合机械臂抓取

    2020-08-19 00:22:37
    4.移动机械臂至四个不同的位置并拍照保存为tif格式的图片,并且记录下四个位置的xyz坐标。 5.在halcon程序中更新四个位置的坐标,遍历四个图片。对每张图片选择圈出中心圆。 6.验证四个中心点与预估点的偏差。小于...
  • 烧录UNO主板
  • 一、基础入门 ...2.CSDN上一个博主写的抓取、机械臂控制、机械臂抓取的代码解析: https://blog.csdn.net/zzu_seu/article/details/94617814 https://blog.csdn.net/zzu_seu/article/details/89293...
  • 项目实战——基于计算机视觉的物体位姿定位及机械臂抓取(单目标定)         请各位读者朋友注意,这里面很多东西涉及到我的毕设,写作辛苦,请勿滥用,转载请务必注明...
  • 摘要:随着空中机器人技术的快速发展与日益成熟,无人机在越来越多的领域得到了广泛的应用。...而基于深度学习来实现旋翼无人机自主目标检测是机械臂自主抓取的前提和关键。近年来,目标检测技术...
  • 项目实战——基于计算机视觉的物体位姿定位及机械臂抓取(双目标定)         请各位读者朋友注意,这里面很多东西涉及到我的毕设,写作辛苦,请勿滥用,转载请务必注明...
  • 这一篇则是讲最终的问题,虽然前面的步骤都做完了,而且最后这步没什么实质性的编程,但是一直机械臂一直有问题,且运行错误或者到达不了预设的位置,本篇即以此为基础进行总结,分析最终不能成功可能的原因,并给出...
  • Linemod算法 机械臂抓取可乐罐

    千次阅读 2018-12-14 22:34:22
    #rosrun object_recognition_core object_add.py -n "coke " -d "A universal can of coke" #rosrun object_recognition_core mesh_add.py &lt;YOUR_OBJECT_ID&gt;...
  • 移动底盘加dobot机械臂抓取代码

    千次阅读 2018-12-04 11:35:33
    #include &lt;ros/ros.h&gt; #include &lt;tf/transform_listener.h&gt; #include &lt;nav_msgs/Odometry.h&gt; #include &lt;string.h&gt; #include &...actionl...
  • Grasping Planning Finding a gripper configuration that maximizes a success (or quality) metric. Method fall into wo categories based on success criteria: analytic methods empirical (or data-driven) ...
  • 距离上次更新公众号的VREP自学笔记系列已经过去了快2个月,期间各种事情缠身,导致项目进展太慢了。最近也要逐步恢复起来啦,不过项目的代码还存在不少问题,需要一段时间的调试和整理。...
  • 任务主线是深度相机看到物体是什么、在哪儿,接着讲位姿发送给机械臂进行抓取,这两周主要解决了用深度相机检测一个可乐罐以及发布其位置信息。 好了开始这两周的工作内容总结: 运行环境:Ubuntu16.04+ROS ...
  • 【Paper 1】Visual Grasping for a Lightweight Aerial Manipulator Based o NSGA-II and Kinematic Compensation
  • 任务主线是深度相机看到物体是什么、在哪儿,位姿发送给机械臂进行抓取,这两周主要解决了用深度相机检测一个可乐罐以及发布其位置信息。 运行环境:Ubuntu16.04+ROS Kinetic+PCL 目前深度相机主要的方法有: ...
  • 本文章参考博主光头明明的博客进行实现。但是由于明明版本为 kinetic + Gazebo7,实现时存在很多问题不好解决,因此通过此文章记录解决方法。 参考博主链接: ... ...报错: Built target tf_example In file included...
  • 项目实战——基于计算机视觉的物体位姿定位及机械臂矫正(基本原理)         首先让各位关注我的朋友们久等了,这个项目是我本科的毕业设计,写到四之后,我就一直忙于...

空空如也

空空如也

1 2 3 4 5 ... 17
收藏数 328
精华内容 131
关键字:

机械臂抓取