Coppelia Sim (v-REP)仿真 机器人3D相机手眼标定与实时视觉追踪 (三)

使用标定好的结果进行跟踪标定板的位置

坐标转换的步骤为:
1.图像坐标点转到相机坐标系下的点
2.相机坐标系下的点转为夹爪坐标系下的点
3.夹爪坐标系下的点转为机械手极坐标系下的点
跟踪的方式
1.采用标定板的第一个坐标点作为跟踪点
3.机器人每次移动到该点位,测试姿态是不是正确

# 测试标定的结果:

# 初始拍照位置
moveToConfig(sim, jointHandles, jmaxVel, jmaxAccel, jmaxJerk, targetjoinPos1)

# 10个拍照位置的验证
# for i in range(10):
while True:
    goalTr = targetPos[i].copy()
    # goalTr[2] = goalTr[2] - 0.2
    params = {}
    params['ik'] = {'tip': tipHandle, 'target': targetHandle}
    # params['object'] = targetHandle
    params['targetPose'] = goalTr
    params['maxVel'] = maxVel
    params['maxAccel'] = maxAccel
    params['maxJerk'] = maxJerk
    sim.moveToPose(params)

    img, [resX, resY] = sim.getVisionSensorImg(visionSensorHandle)
    img = np.frombuffer(img, dtype=np.uint8).reshape(resY, resX, 3)

    # In CoppeliaSim images are left to right (x-axis), and bottom to top (y-axis)
    # (consistent with the axes of vision sensors, pointing Z outwards, Y up)
    # and color format is RGB triplets, whereas OpenCV uses BGR:
    img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
    # img=cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

    # 获取深度图像
    Deepdate = sim.getVisionSensorDepth(deepSensorHandle, 1)
    num_floats = Deepdate[1][0] * Deepdate[1][1]
    depth_data = struct.unpack(f'{num_floats}f', Deepdate[0])
    depth_array = np.array(depth_data)
    depth_image = depth_array.reshape((Deepdate[1][1],  Deepdate[1][0]))
    depth_image = np.flipud(depth_image)

    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 找到棋盘格的角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    # 如果找到角点,使用第一个角点来转换为机器人的坐标
    if ret == True:
        u = corners[0][0][0]
        v = corners[0][0][1]
        Z = depth_image[int(v), int(u)]

        img = cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
        ret, rvec, tvec = cv2.solvePnP(objp, corners, mtx, dist)

        # 计算相机坐标系下的三维点
        P_cam = pixel_to_camera_coordinates(u, v, Z, mtx)

        print("相机坐标系下的三维点 P_cam:", P_cam)
        # t_cam2gripper=t_cam2gripper.reshape(-1)
        # 计算物体在手爪坐标系中的位置

        # 计算点在末端坐标系下的坐标 P_end
        P_end = np.dot(R_cam2gripper, P_cam) + t_cam2gripper.reshape(-1)
        # sim.setObjectPosition(targetHandle, P_end,tipHandle)

        # 计算点在基座坐标系下的坐标 P_base
        tip_matrix = sim.getObjectMatrix(tipHandle)
        # 提取旋转矩阵 R_end_to_base (3x3)
        R_end_to_base = np.array([
            [tip_matrix[0], tip_matrix[1], tip_matrix[2]],
            [tip_matrix[4], tip_matrix[5], tip_matrix[6]],
            [tip_matrix[8], tip_matrix[9], tip_matrix[10]]
        ])

        # 提取平移向量 t_end_to_base (3x1)
        t_end_to_base = np.array([
            [tip_matrix[3]],
            [tip_matrix[7]],
            [tip_matrix[11]]
        ])

        P_base = np.dot(R_end_to_base, P_end) + t_end_to_base.reshape(-1)
        # sim.setObjectPosition(targetHandle, P_base)
        Tip_pose = sim.getObjectPose(tipHandle)

        # 将旋转向量转换为旋转矩阵
        R_board_to_camera, _ = cv2.Rodrigues(rvec)

        # 计算标定板相对于末端的旋转矩阵和平移向量
        R_board_to_end = R_cam2gripper @ R_board_to_camera
        t_board_to_end = R_cam2gripper @ tvec.flatten() + t_cam2gripper.flatten()

        # 计算标定板相对于世界坐标系的旋转矩阵和平移向量
        R_board_to_world = R_end_to_base @ R_board_to_end
        t_board_to_world = R_end_to_base @ t_board_to_end + t_end_to_base.flatten()
        chessboard_matrix = sim.getObjectMatrix(targetHandle)

        cal_chessboard_matrix = np.array([R_board_to_world[0][0], R_board_to_world[0][1], R_board_to_world[0][2], t_board_to_world[0],
                                          R_board_to_world[1][0], R_board_to_world[1][1], R_board_to_world[1][2], t_board_to_world[1],
                                          R_board_to_world[2][0], R_board_to_world[2][1], R_board_to_world[2][2], t_board_to_world[2]])

        sim.setObjectMatrix(targetHandle,  cal_chessboard_matrix)

        goalTr = Tip_pose.copy()
        goalTr[0]=P_base[0]
        goalTr[1]=P_base[1]
        goalTr[2]=P_base[2]
        params = {}
        params['ik'] = {'tip': tipHandle, 'target': targetHandle}
        # params['object'] = targetHandle
        params['targetPose'] = goalTr
        params['maxVel'] = maxVel
        params['maxAccel'] = maxAccel
        params['maxJerk'] = maxJerk
        sim.moveToPose(params)

    display.displayUpdated(img, depth_image)

测试的结果的
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/912036.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

easyui +vue v-slot 注意事项

https://www.jeasyui.com/demo-vue/main/index.php?pluginDataGrid&themematerial-teal&dirltr&pitemCheckBox%20Selection&sortasc 接口说明 <template><div><h2>Checkbox Selection</h2><DataGrid :data"data" style&…

运动【跑步 03】安踏冠军3的10KM和15KM*2体验(对比必迈PURE LIGHT)

这里写目录标题 1. 前言2. 两双鞋2.1 必迈 PURE LIGHT2.2 安踏 冠军 3 3. 主观对比4. 问题4.1 必迈 PURE LIGHT4.2 冠军 3 5. 总结 1. 前言 我是程序员&#xff0c;并不是专业的运动员&#xff0c;对跑步鞋的研究也不深&#xff0c;至今也就买过两双相对比较专业的跑鞋&#x…

O-RAN Fronthual CU/Sync/Mgmt 平面和协议栈

O-RAN Fronthual CU/Sync/Mgmt 平面和协议栈 O-RAN Fronthual CU/Sync/Mgmt 平面和协议栈O-RAN前端O-RAN 前传平面C-Plane&#xff08;控制平面&#xff09;&#xff1a;控制平面消息定义数据传输、波束形成等所需的调度、协调。U-Plane&#xff08;用户平面&#xff09;&#…

【JavaEE进阶】导读

本节⽬标 了解什么是JavaEE 在JavaEE中, 我们学习什么, 如何学, 难点是什么 一、Java EE 发展历程 Java EE(Java Platform Enterprise Edition), Java 平台企业版. 是JavaSE的扩展, ⽤于解决企业级的开发需求, 所以也可以称之为是⼀组⽤于企业开发的Java技术标准. 所以, 学习…

Javascript事件循环流程分析

基础概念 事件循环&#xff08;Event Loop&#xff09;&#xff1a;事件循环是JavaScript运行时环境中的一个循环机制&#xff0c;它不断地检查调栈用和任务队列。当调用栈为空时&#xff0c;事件循环会首先检查微任务队列&#xff0c;并执行其中的所有任务。只有当微任务队列…

单元/集成测试解决方案

在项目开发的前期针对软件单元/模块功能开展单元/集成测试&#xff0c;可以尽早地发现软件Bug&#xff0c;避免将Bug带入系统测试阶段&#xff0c;有效地降低HIL测试的测试周期&#xff0c;也能有效降低开发成本。单元/集成测试旨在证明被测软件实现其单元/架构设计规范、证明被…

【C++】C++的单例模式、跟踪内存分配的简单方法

二十四、C的单例模式、跟踪内存分配的简单方法 1、C的单例模式 本小标题不是讨论C的语言特性&#xff0c;而是一种设计模式&#xff0c;用于确保一个类在任何情况下都只有一个实例&#xff0c;并提供一个全局访问点来获取这个实例。即C的单例模式。这种模式常用于资源管理&…

LangGPT结构化提示词编写实践

基础任务 如果直接询问大模型strawberry有几个r&#xff0c;大模型会给出错误的答案&#xff1a; 这里我们引入思维连Chain of Thought&#xff0c;我们让大模型遍历一遍单词&#xff0c;每次累加得到最终结果 之前怎么都做不对的题&#xff0c;让大模型一步一步思考&#xf…

【Python系列】使用 Poetry 进行 Python 项目管理

&#x1f49d;&#x1f49d;&#x1f49d;欢迎来到我的博客&#xff0c;很高兴能够在这里和您见面&#xff01;希望您在这里可以感受到一份轻松愉快的氛围&#xff0c;不仅可以获得有趣的内容和知识&#xff0c;也可以畅所欲言、分享您的想法和见解。 推荐:kwan 的首页,持续学…

Linux内核USB2.0驱动框架分析--USB设备枚举过程

一 USB特点 1.1 USB协议版本介绍&#xff1a; USB1.0/1.1&#xff08;low/fullspeed&#xff09;&#xff1a;传输速率最大为12Mbps&#xff0c;是较早的USB协议版本。 USB2.0&#xff08;highspeed&#xff09;&#xff1a;传输速率最大为480Mbps&#xff0c;相比USB1.0/1.1…

解决ultralytics的YOLO模型训练中验证集Loss为NaN(或mAP为0)的问题

前言 在使用ultralytics库的YOLO模型时&#xff0c;比如YOLOv8进行目标检测模型训练&#xff0c;遇到一个非常奇怪的问题&#xff1a;训练过程中的验证损失&#xff08;loss&#xff09;出现了NaN&#xff0c;而验证的评价指标如mAP50却能正常计算&#xff08;有时mAP都也为0&…

微信支付现金红包,实现转账到零钱包功能

大家好&#xff0c;我是小悟。 上次说到微信商家转账到零钱要出新玩法&#xff0c;可能会对某些特定的业务产生影响&#xff0c;详细请阅读【微信商家转账到零钱新玩法&#xff0c;却是个不好接受的消息】。 微信支付还有个现金红包的产品&#xff0c;也可以实现转账到用户零…

掌握均值回归,外汇交易盈利新视角

外汇交易是全球金融市场的重要组成部分&#xff0c;它不仅用于国际间结算债权债务&#xff0c;还提供了一个充满盈利机会的金融市场。在这个市场中&#xff0c;货币价格的波动为投资者带来了丰富的交易机会。本文&#xff0c;EagleTrader将详细介绍外汇交易中的一种常用策略——…

mac-泛洪

泛洪攻击的类型 TCP SYN Flood&#xff1a; 攻击者向目标服务器发送大量的 TCP SYN 请求&#xff0c;但不完成握手过程。服务器为每个请求分配资源&#xff0c;最终可能耗尽其连接表&#xff0c;导致无法处理正常请求。 UDP Flood&#xff1a; 攻击者向目标发送大量的 UDP 数据…

【Windows修改Docker Desktop(WSL2)内存分配大小】

记录一下遇到使用Docker Desktop占用内存居高不下的问题 自从使用了Docker Desktop&#xff0c;电脑基本每天都需要重启&#xff0c;内存完全不够用&#xff0c;从16g扩展到24&#xff0c;然后到40G&#xff0c;还是不够用&#xff1b;打开Docker Desktop 运行时间一长&#x…

【06】A-Maven项目SVN设置忽略文件

做Web项目开发时&#xff0c;运用的是Maven管理工具对项目进行管理&#xff0c;在项目构建的过程中自动生成了很多不需要SVN进行管理的文件&#xff0c;SVN在对源码进行版本管理时&#xff0c;需要将其忽略&#xff0c;本文给出了具体解决方案。 SVN设置忽略Maven项目中自动生成…

【数据分享】2024年我国省市县三级的生活服务设施数量(46类设施/Excel/Shp格式)

人才市场、售票处、旅行社等生活服务设施的配置情况是一个城市公共基础设施完善程度的重要体现&#xff0c;一个城市生活服务设施种类越丰富&#xff0c;数量越多&#xff0c;通常能表示这个城市的公共服务水平越高&#xff01; 本次我们为大家带来的是我国各省份、各地级市、…

【算法】Floyd多源最短路径算法

目录 一、概念 二、思路 三、代码 一、概念 在前面的学习中&#xff0c;我们已经接触了Dijkstra、Bellman-Ford等单源最短路径算法。但首先我们要知道何为单源最短路径&#xff0c;何为多源最短路径 单源最短路径&#xff1a;从图中选取一点&#xff0c;求这个点到图中其他…

【商用存储】希捷磁盘阵列部署实践

文章目录 一、前言1、盘阵类型2、性能规格 二、功能说明1、RAID配置1.1、虚拟池&#xff08;virtual pool&#xff09;1.2、线性池&#xff08;linear pool&#xff09; 2、磁盘休眠2.1、RBOD2.1.1、功能说明2.1.2、规格限制 3、ADAPT配置3.1、说明3.2、规格限制3.3、配置建议3…

Android Room框架使用指南

Room框架使用指南 项目效果创建应用,配置Gradle1、在app Module的build.gradle配置kapt插件2、配置依赖:3、配置依赖包版本号创建实体类创建DAO1、DAO简介2、WordDao设计以及相关注解说明3、监听数据变化添加Room数据库1、Room数据库简介2、实现Room数据库实现存储库实现View…