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

最新推荐文章于 2024-01-29 20:24:11 发布
最新推荐文章于 2024-01-29 20:24:11 发布
阅读量2.7k 收藏 37
点赞数 3
分类专栏： kinova ROS
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_34897331/article/details/100982041
版权
ROS 同时被 2 个专栏收录
12 篇文章 0 订阅
订阅专栏
kinova
10 篇文章 2 订阅
订阅专栏

实现机械臂抓取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仍然太繁琐，仍然需要精简！
lj2jq
关注
  • 3
    点赞
  • 踩
  • 37
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
【机械臂、无人机规控篇】（8）机械臂轨迹规划、跟踪控制方向
qq_35635374的博客
11-08 3065
机械臂轨迹规划、跟踪控制方向
移动机械臂轨迹跟踪方法
12-28
移动平台 机械臂 轨迹跟踪 Coordinating Locomotion and Manipulation of a Mobile Manipulator
机械臂抓取过程
09-30
这份代码基于stm32芯片结合平衡小车之家的库函数开发的舵机机械臂，抓取过程的。
Android基于高德地图完全自定义Marker的实现方法
08-27
主要给大家介绍了关于Android基于高德地图完全自定义Marker的实现方法，文中通过示例代码介绍的非常详细，对大家的学习或者工作具有一定的参考学习价值，需要的朋友们下面随着小编来一起学习学习吧
机械臂轨迹跟踪,机械臂轨迹跟踪控制,matlab
09-10
模糊规则优化的滑模控制器实现两连杆机械臂的轨迹跟踪控制
使用大象机器人myCobot 280 Jeston Nano协作机械臂进行物体精确识别追踪！
m0_71627844的博客
07-03 1183
仍然还有一些不足的地方，在相机固定的情况下，机械臂的本体可能会遮挡相机的视野，导致没有办法进行下一步跟踪，想到的解决方案是相机换个位置不被遮挡的位置（坐标换算那些都得重新计算）。尝试过多种的方法，物体的识别需要让机器进行学习，我们要识别的目标，这样会增加项目开发的时间，最后决定用aruco码来进行识别，这样可以快速捕捉到aruco码，进行下一步开发。在这之后就是物体检测和机械臂的控制，将识别到的物体的坐标转换成机械臂的运动指令，这里用到的是pymycobot库来进行对机械臂的控制。下图是项目的开发流程图。
机器人轨迹跟随算法
weixin_42178232的博客
04-23 1215
来源：Matlab官方 机械臂算法包括如下：路径规划（RRT及变体等）、轨迹规划（T型、多项式等）、轨迹跟随（角度空间、任务空间和计算力矩）、电机控制 常用三种轨迹跟随算法：
Moveit!入门——古月居机械臂开发笔记（二）
Twilight的博客
02-16 4459
Moveit!入门——古月居机械臂开发笔记（二）引言ROS机械臂开发_机器视觉与物体抓取1、ROS中的运动学插件2、Moveit！碰撞检测规划场景3、Pick and Place 引言 接上次笔记（一）Moveit！和机械臂控制，这一次主要为：ROS机械臂开发_机器视觉与物体抓取。   ROS机械臂开发_机器视觉与物体抓取   1、ROS中的运动学插件        参考https://www.guyuehome.co
[ROS项目]基于语音控制目标跟随与抓取的移动操作平台
weixin_42139267的博客
01-03 1446
Part1: 视觉 CUDA INSTALL CUDA Version: 11.0: 从链接的网址中选择下载runfile模式，并按照提示完成安装 CUDNN INSTALL 根据CUDA版本下载安装文件：CUDNN安装历史版本目录地址 安装CUDNN $ sudo cp cuda/include/cudnn.h /usr/local/cuda/include/ $ sudo cp cuda/lib64/libcudnn* /usr/local/cuda/lib64/ $ sudo chmod
低成本Hand Tracking机械臂手势识别与跟踪
qq_42500340的博客
04-17 2143
低成本50R的eps32-cam与esp8266双主控，结合OpenCV的机械臂。基于arduino开发，适合新手。
MGS-Mechinery（Unity 机械机构关节脚本）
01-25
实现虚拟现实常用到的机械结构的关机绑定脚本，支持曲柄摇杆机构，曲柄滑块机构，四杆铰链机构，以及同步伸缩，序列伸缩机构等，可实现诸如挖掘机，起重机等常见机器的关节绑定。
基于视觉的PUMA560机械臂的抓取路径规划问题
04-23
基于视觉的PUMA560机械臂的抓取路径规划问题，包括图像采集与处理，机械臂建模，三维重建，视觉伺服控制等内容 (2013-04-02, matlab, 21KB, 0次).zip
Unity3D 虚拟手DEMO
01-26
本资源使用unity制作的虚拟手控制例子，通过按钮控制虚拟手的每个部件，内含unity完整工程文件（5.0）和运行效果gif图片。
JavaScript添加marker并连线
12-13
SuperMap iClient 6R for JavaScript 本实例演示了如何添加marker的点，线，面
基于JS抓取某高校附近共享单车位置 使用web方式展示位置变化代码实例
10-16
在本实例中，我们探讨如何使用JavaScript（JS）来抓取特定高校附近的共享单车位置，并通过Web方式实时展示这些车辆的位置变化。这个应用结合了地图API，例如高德地图API，来实现地理信息的可视化。 首先，为了抓取...
基于Arcgis for javascript实现百度地图ABCD marker的效果
10-23
本篇文章由脚本之家小编给大家分享的基于Arcgis for javascript实现百度地图ABCD marker的效果，需要的朋友一起学习吧
基于OL2实现百度地图ABCD marker的效果
11-23
上文中提到了在Arcgis for JS中实现百度地图ABCD的marker效果，在本文，讲述如何在OpenLayers2中实现类似的效果。 效果图展示如下： 为直观期间，先将效果贴出来。 联动展示 思路： 1、列表与地图的互动 鼠标经过...
LabVIEW机械臂轨迹跟踪控制
最新发布
bjcyck的博客
01-29 1280
为了实现LabVIEW环境下的机械臂轨迹跟踪控制，以下是建议的硬件配置：伺服电机和驱动器：选择具有高精度和响应速度的伺服电机，例如Delta、松下或汇川的伺服电机。相应的伺服驱动器应支持EtherCAT或类似的高速通信协议，以确保与LabVIEW的顺畅通信。运动控制器：考虑使用支持多轴控制的运动控制器，如NI的PXI系列或CompactRIO系列，这些控制器能与LabVIEW紧密集成。编码器：对于精确的位置反馈，选择高分辨率的旋转编码器，如海德汉、SICK或奥托尼克斯品牌。
机械臂：物体跟踪
lperooo的博客
03-21 2897
机械臂：人脸跟踪总体结构一、机器人舵机1.硬件连接2.驱动程序二、人脸检测1.引入库2.加载模型3.检测并框出人脸三、小球检测1.引入库2.处理图像3.检测并框出小球四、控制机械臂1.引入库2.计算物体中心和转动角度3.发出指令 总体结构 提示：这里可以添加本文要记录的大概内容： 例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。 提示：以下是本篇文章正文内容，下面案例可供参考 一、机器人舵机 1.硬件连接 通过官方Debugger.
android中使用高德地图的marker类，并实现给每个marker实例绑定id
03-27
在使用高德地图的Marker类时，可以通过设置Marker的Object属性来实现给每个Marker实例...通过这种方式，可以方便地实现给每个Marker实例绑定一个id值，并在Marker的点击事件中获取该id值，从而实现对Marker的更多操作。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值