使用Phantom omni力反馈设备控制机器人

　　传统的工业机器人普遍采用电机 、齿轮减速器 、关节轴三者直接连接的传动机构，这种机构要求电机与减速器安装在机械臂关节附近，其缺点是对于多关节机械臂，下一级关节的电机与减速器等驱动装置成为上一级关节的额外负载 。这一额外负载带来的负面影响往往超过机械臂连杆等必要结构件，因此提高了对机械臂动力和驱动元件的要求，由此造成整体重量 、体积 、造价和内部消耗的增加，降低了机械臂对外做功的能力和效率。为了避免这一问题许多主动式医疗机器人采用绳驱动方式，使用柔性绳索远距离的传递运动和力矩。将驱动装置安装到远离相关关节的基座上，使得整个机械臂的结构紧凑 ，在减轻机械臂整体重量的同时提高了对外做功的能力。下图所示的就是达芬奇手术机器人的末端夹子，采用绳驱动方式可以在较小的机械结构内实现多自由度的灵活运动：

　　下面尝试在VREP中使用力反馈设备Phantom omni来控制医疗机器人的末端夹子。机构简图如下图所示，一共有5个自由度：其中移动自由度负责进给，一个整体旋转自由度，还有两个左右、上下弯曲的自由度，最后是控制夹子张合的自由度。

　　删除Solidworks CAD模型中的一些不必要特征，比如倒角、内部孔、螺纹孔等，导出成STL文件，再导入到VREP中。然后还需要进行继续化简，减少网格数量，当网格数减少到不影响几何外观就可以了。接下来在相应的位置添加关节，设置关节运动范围（软件限位），并将其设为Inverse kinematics模式。

　　设置Calculation Modules中的Inverse kinematics：

　　搭建好模型后可以先用键盘进行测试，按方向键移动ikTarget，VREP会根据构型自动计算运动学逆解，然后将ikTip移动到ikTarget处（这样就会带着整个机构运动）。键盘控制的脚本代码如下：

if (sim_call_type==sim_childscriptcall_initialization) then-- Put some initialization code heretargetHandle = simGetObjectHandle('ikTarget')endif (sim_call_type==sim_childscriptcall_actuation) then-- Put your main ACTUATION code hereendif (sim_call_type==sim_childscriptcall_sensing) then-- Put your main SENSING code here-- Read the keyboard messages (make sure the focus is on the main window, scene view):message, auxiliaryData = simGetSimulatorMessage()if (message == sim_message_keypress) thenif (auxiliaryData[1]==119) then-- W keylocal p = simGetObjectPosition(targetHandle, -1)p[1] = p[1] - 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==115) then-- S keylocal p = simGetObjectPosition(targetHandle, -1)p[1] = p[1] + 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2007) then-- up keylocal p = simGetObjectPosition(targetHandle, -1)p[3] = p[3] + 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2008) then-- down keylocal p = simGetObjectPosition(targetHandle, -1)p[3] = p[3] - 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2009) then-- left keylocal p = simGetObjectPosition(targetHandle, -1)p[2] = p[2] - 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2010) then-- right keylocal p = simGetObjectPosition(targetHandle, -1)p[2] = p[2] + 0.001simSetObjectPosition(targetHandle, -1, p)endend
endif (sim_call_type==sim_childscriptcall_cleanup) then-- Put some restoration code hereend
View Code

　　测试没问题后可以使用CHAI3D插件来连接力反馈设备。这里采用增量控制的模式，即计算当前时刻与前一时刻手柄位置在X、Y、Z方向上的差，然后控制VREP中的ikTarget控制点按相应的增量移动。注意在VREP中机器人向前运动是沿X轴负方向、向上运动是沿Z轴正方向、向右运动是沿Y轴正方向，这与CHAI3D中坐标系的定义一致（向前推手柄是沿着X轴负方向...），因此可以使用力反馈设备直观的控制机器人的运动。当然如果坐标系定义不一致，需要进行简单的转换才行。

　　下面的代码在sensing探测部分会使用simExtCHAI3D_readPosition来读取当前操作手柄的位置和按钮状态。按照VREP默认设置，这部分代码会50ms执行一次，这里会出现一个问题：如果采样速率太快，会导致前后两次采集到的位置数据偏差为零（人手的操作频率没那么快，还来不及改变位置），那么输出的控制量就一直是零，这样就没办法用增量控制的方式来操控机器人。解决办法是延迟几个周期再采样，等到有足够大的偏差之后再生成控制量。还有一个问题是使用CHAI3D返回的数据以“米”为单位，而VREP世界中的单位有可能未知，那么使用增量控制时需要对控制量乘一个比例系数，避免因操作端微小的移动造成从动端运动量过大，超出关节限制（无法到达的逆解）。或者可以调节比例系数，用操作端的大位移来控制从动端的小位移，实现精细控制。

if (sim_call_type==sim_childscriptcall_initialization) then-- Check if the plugin is loaded:moduleName=0moduleVersion=0index=0pluginNotFound=truewhile moduleName domoduleName,moduleVersion=simGetModuleName(index)if (moduleName=='CHAI3D') thenpluginNotFound=falseendindex=index+1endif (pluginNotFound) thensimDisplayDialog('Error','CHAI3D plugin was not found, or was not correctly initialized (v_repExtCHAI3D.dll).',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})else-- Start the device:local toolRadius = 0.001 -- the radius of the toollocal workspaceRadius = 0.2 -- the workspace radiusif simExtCHAI3D_start(0, toolRadius,workspaceRadius) ~= 1 thensimDisplayDialog('Error','Device failed to initialize.',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})elseCHAI3DPluginInitialized = trueendendtargetHandle = simGetObjectHandle('ikTarget')deltaPos = {0, 0, 0}counter = 0ratio = 50endif (sim_call_type==sim_childscriptcall_actuation) thenif buttonState ==1 then  -- press the buttonlocal p = simGetObjectPosition(targetHandle, -1) -- get the target position-- add increment of the tool tipp[1] = p[1] + deltaPos[1] / ratio                  p[2] = p[2] + deltaPos[2] / ratiop[3] = p[3] + deltaPos[3] / ratiosimSetObjectPosition(targetHandle, -1, p)  -- move to the absolute positionend
endif (sim_call_type==sim_childscriptcall_sensing) thenif CHAI3DPluginInitialized then-- Read the current position of the cursor:local currentToolPosition = simExtCHAI3D_readPosition(0)-- Read the buttons of the device:buttonState = simExtCHAI3D_readButtons(0)counter = counter + 1      -- increase the counterif counter % 30 == 1 then  -- keep the valueprevToolPosition = currentToolPositionendif counter % 30 == 0 then  -- calculate tool tip incrementdeltaPos[1] = currentToolPosition[1] - prevToolPosition[1]  -- X-axis incrementdeltaPos[2] = currentToolPosition[2] - prevToolPosition[2]  -- Y-axis incrementdeltaPos[3] = currentToolPosition[3] - prevToolPosition[3]  -- Z-axis incrementcounter = 0  -- reset counterlocal info = string.format("CurrentPosition:%.2f,%.2f,%.2f  DeltaPosition:%.2f,%.2f,%.2f",currentToolPosition[1],currentToolPosition[2],currentToolPosition[3],deltaPos[1],deltaPos[2],deltaPos[3])simAddStatusbarMessage(info)endend
endif (sim_call_type==sim_childscriptcall_cleanup) thenif CHAI3DPluginInitialized then-- Disconnects all devices and removes all objects from the scene
        simExtCHAI3D_reset()end
end

 　　将力反馈设备手柄移动到合适的位置之后就可以按住按钮开始操控机器人，松开按钮会停止控制。如果在VREP虚拟场景中添加其它物体（比如障碍物），则还可以模拟环境中的力（接触力、重力、摩擦力、弹簧力等）让操控着“感觉”到。如果实际机器人上装有力传感器，则在用Phantom omni控制机器人的同时也能读取力的信息，反馈给操作者。

　　下面是使用Phantom omni来控制机器人的动态图，黄色的轨迹为使用Graph记录的控制点的空间位置：

 　　对于该机构也可以自己实现运动学逆解的数值算法，下面给出伪逆矩阵法和阻尼最小二乘法的参考：

import math  
import numpy as np# link length
L1 = 1  
L2 = 1gamma = 1       # step size
lamda = 0.2     # damping constant (DLS-method)
stol = 1e-3     # tolerance
nm = 10         # initial error
count = 0       # iteration count
ilimit = 20     # maximum iteration# numerical method for inverse kinematics
method = 'Pseudo Inverse'  # 'Pseudo Inverse', 'DLS', ...# initial joint value
q = np.array([0, 0, math.pi/2, 0]) # [theta1, d1, theta2, theta3]# target position  
target_pos = np.array([1, 0, 2])   # [x,y,z]while True:if(nm > stol):# forward kinematics:x = np.array([math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3]),\math.cos(q[2])*math.sin(q[0])*(L1+L2*math.cos(q[3]))-L2*math.cos(q[0])*math.sin(q[3]),\q[1]+(L1+L2*math.cos(q[3]))*math.sin(q[2])])# compute errorerror = target_pos - x# compute JacobianJ11 = -math.sin(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.cos(q[0])*math.sin(q[3])J12 = 0J13 = -math.sin(q[2])*math.cos(q[0])*(L1+L2*math.cos(q[3]))J14 = L2*(math.sin(q[0])*math.cos(q[3])-math.cos(q[0])*math.cos(q[2])*math.sin(q[3]))J21 = math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3])J22 = 0J23 = -math.sin(q[0])*math.sin(q[2])*(L1+L2*math.cos(q[3]))J24 = -L2*(math.cos(q[0])*math.cos(q[3])+math.sin(q[0])*math.cos(q[2])*math.sin(q[3]))J31 = 0J32 = 1J33 = math.cos(q[2])*(L1+L2*math.cos(q[3]))J34 = -L2*math.sin(q[2])*math.sin(q[3])J = np.array([[J11,J12,J13,J14],[J21,J22,J23,J24],[J31,J32,J33,J34]])if method == 'Pseudo Inverse': # Pseudo Inverse MethodJ_pseudo = np.dot(J.transpose(), np.linalg.inv(J.dot(J.transpose()))) # compute pseudo inversedq = gamma * J_pseudo.dot(error)if method == 'DLS':# Damped Least Squares Methodf = np.linalg.solve(J.dot(J.transpose())+lamda**2*np.identity(3), error)dq = np.dot(J.transpose(), f)# update joint position   q = q + dqnm = np.linalg.norm(error)count = count + 1if count > ilimit:print "Solution wouldn't converge!"breakelse:# print resultprint 'theta1=' + str(q[0]*180/math.pi)+' d1='+str(q[1]) + ' theta2=' + str((q[2]-math.pi/2)*180/math.pi)+' theta3=' + str(q[3]*180/math.pi)print 'Current position: %.2f, %.2f, %.2f' %(x[0],x[1],x[2])    print str(count)+' iterations'+'  err:'+str(nm)break

 

 

参考：

OpenHaptics编程环境搭建

VREP中的力触觉设备接口（CHAI3D）

 “逆运动学”——从操作空间到关节空间（上篇）

转载于:https://www.cnblogs.com/21207-iHome/p/7338216.html

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

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

Go_配置系统环境MacOS(M1)

Go_配置系统环境MacOS(M1)

在MacOS下和JDK一样，配不配环境其实MacOS都是可以检测的到的，安装好以后直接输入go version是一样可以的，因为都是使用开发工具的，在开发工具里配置的话是样的，如果有习惯的话就配置一下吧 下载安装及配置环境&#xf…
阅读更多...
Android setBackgroundResource和setBackgroundDrawable和用法

Android setBackgroundResource和setBackgroundDrawable和用法

两个方法的效果是一样，只是区别于效率！playBtn.setBackgroundResource(R.drawable.pause_selecor); 从上面可以看出来是从资源文件中获取drawable中的pause_selecor对象，如果是经常要用到的drawable对象，就可以让它成为一个成员变…
阅读更多...
Go_变量使用详解

Go_变量使用详解

变量 变量的本质是计算机分配的一块内存，用于存放数据，变量是在程序运行过程中，其值可以发生改变的数据，变量属于临时存储，当程序运行结束，存放该数据的内存就会释放，该变量就会随着内存的释放…
阅读更多...
硬币找零问题，动态规划基础，百度面试题

硬币找零问题，动态规划基础，百度面试题

问题描述：给出几种面值的硬币，要求用这几种硬币找零出所给零钱数，用的硬币数要最少。 过去我们用过贪心法解决此类问题，包括本人在百度面试时，也是用的贪心法（面试官对这个解答不满意）&#xff…
阅读更多...
Go_数据类型

Go_数据类型

数据类型： 计算机存储设备最小信息单位是位（bit），最小的存储单元是字节（byte），占用字节的不同，所表示能存储的数据长度不同。数据类型用来说明数据的数据的结构，便于后面…
阅读更多...
HashMap之扰动函数和低位掩码

HashMap之扰动函数和低位掩码

我们都知道，hashMap在实现的时候，为了寻找在数组上的位置，主要做了两件事 int hash hash(key); int i indexFor(key, table.length); 这个时候得到i才是数组上的位置。 这两个方法详解如下 JDK8对扰动函数的修改，只进行了一次移…
阅读更多...
Go_运算符及优先级

Go_运算符及优先级

运算符： 很久以前，流传程序算法数据这样的说法。算法是什么？通俗点说就是解决问题的过程。小到加法指令，大到成千上万台服务器组成的分布式计算集群。抛去抽象概念和宏观架构，最终都由最基础的机器指令过程去处理不同层…
阅读更多...
mvn 修改所有子项目pom版本

mvn 修改所有子项目pom版本

mvn versions:set -DgenerateBackupPomsfalse -DnewVersion1.3.0 转载于:https://www.cnblogs.com/sidesky/p/7344713.html
阅读更多...
Go_流程、跳转控制语详解

Go_流程、跳转控制语详解

流程控制语句分类： 流程控制语句可以控制代码的执行顺序和条件 顺序结构：普通的代码，由上而下依次执行分支结构(if, switch)循环结构(for) if语句： 条件表达式值必须是bool类型，不可省略括号，且左大括号…
阅读更多...
HDU 4027 Can you answer these queries?（线段树/区间不等更新）

HDU 4027 Can you answer these queries?（线段树/区间不等更新）

传送门 Can you answer these queries? Time Limit: 4000/2000 MS (Java/Others) Memory Limit: 65768/65768 K (Java/Others)Total Submission(s): 18290 Accepted Submission(s): 4308 Description A lot of battleships of evil are arranged in a line before the b…
阅读更多...
Go_秒懂函数、参数、可变参数、匿名函数、回调函数、内置函数

Go_秒懂函数、参数、可变参数、匿名函数、回调函数、内置函数

函数是将具有独立功能的代码块组成一个整体，使其具有特殊功能的代码集。它将复杂的算法过程分解为若干个小任务，隐藏相关细节，使得程序结构更加清晰，易于维护。通过接收输入参数完成一段算法指令，输出或存储相关结果。…
阅读更多...
NE2018届校招内推笔试——数据挖掘

NE2018届校招内推笔试——数据挖掘

【单选题|2分/题】 1、在只有两类的情况下，二维特征向量通过共享相同的协方差矩阵的正态分布生成，其中协方差矩阵为： 均值向量分别为：，则根据贝叶斯分类，样本分类为：（） A…
阅读更多...
Go_数组遍历、最大值、求和、多维数组

Go_数组遍历、最大值、求和、多维数组

数组： 数组就是用来存储数据的容器，存储多个数据时数据类型要一致。如果想要保存任意类型数据，需要声明为接口类型数组数组定义完成后，可以对数组进行赋值操作。数组是通过下标来进行操作的，下标的范围是从0开始到数组…
阅读更多...
全局变量和环境变量的区别

全局变量和环境变量的区别

全局变量:启动脚本了.在各线程,以及主程序中.可以互相传递值.每次启动脚本,初始值都一样.环境变量:启动脚本了.在各线程,以及主程序中.可以互相传递值.每次启动脚本,初始值是上次停止脚本时的值. 例子:Global 全局变量Dimenv 环境变量全局变量 全局变量 1环境变量 环境变量 …
阅读更多...
不满足依赖关系

不满足依赖关系

今晚上脑残，替换了实体，把报错的也都替换完成了，但是运行报错： 大概的意思就是说不满足XXXXXX依赖关系，但是找了半天都没有找到，最后是mapper的实体类全路径替换的时候，脑残在后面加上了.java。…
阅读更多...
Go_切片（初始化、遍历、截取、修改、append、copy、切片作为函数参数、切片求和、切片求最大值）

Go_切片（初始化、遍历、截取、修改、append、copy、切片作为函数参数、切片求和、切片求最大值）

切片： 切片的长度是不固定的，可以追加数据，可以理解是一个动态数组，切片的底层是一个结构体切片类型（slice）本身并不是动态数组或数组指针。它内部通过指针引用底层数组，设定相关属性将操作限定…
阅读更多...
Golang——递归的使用

Golang——递归的使用

递归指的是函数定义中调用函数本身的现象（自己调自己）把一个复杂的问题层层转化为一个与原问题相似的规模较小的问题来求解递归策略只需少量的程序就可描述出解题过程所需要的多次重复计算 递归注意事项： 递归一定要有出口。否则内存溢出(出口…
阅读更多...
阿里巴巴Java开发手册——速读记录

阿里巴巴Java开发手册——速读记录

本随笔基于阿里巴巴Java开发手册V1.2，陆陆续续记录一些现阶段能理解的，有启发的内容，并将持续更新 最佳实践——插件使用已经发布为随笔！http://www.cnblogs.com/jiangbei/p/7668654.html 一、编程规范 1.命名规范 （1&…
阅读更多...
Go_map集合初始化、键值操作、map作为函数参数的使用

Go_map集合初始化、键值操作、map作为函数参数的使用

map： map是key-value的数据结构，类似于Java的集合，又称为字典，像新华字典就是key:value类型展示的map是无序的，其中key是不允许重复的，key不存在相当于添加，存在相当于修改map的key必须可以进行…
阅读更多...
Angular 小试牛刀[1]:Getting Started

Angular 小试牛刀[1]:Getting Started

首先，Angular2 与 Angular1.x 版本没有多大关系，甚至可以说是两个完全不一样的框架，故 Angular 指的是 Angular2 及以上的版本。而 Angular 与 TypeScript 的完美结合，对于一个 .NET 开发者来说，实在是找不到不用它的理…
阅读更多...
最新文章

Copyright @ 2022~2023