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

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

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

设置Calculation Modules中的Inverse kinematics：

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

键盘控制的脚本代码如下：

1. if (sim_call_type==sim_childscriptcall_initialization) then  
2.  
3.     -- Put some initialization code here  
4.     targetHandle = simGetObjectHandle('ikTarget')  
5.  
6.  
7. end  
8.  
9.  
10. if (sim_call_type==sim_childscriptcall_actuation) then  
11.  
12.     -- Put your main ACTUATION code here  
13.  
14. end  
15.  
16.  
17.  
18. if (sim_call_type==sim_childscriptcall_sensing) then  
19.  
20.     -- Put your main SENSING code here  
21.     -- Read the keyboard messages (make sure the focus is on the main window, scene view):  
22.     message, auxiliaryData = simGetSimulatorMessage()  
23.  
24.     if (message == sim_message_keypress) then  
25.  
26.         if (auxiliaryData[1]==119) then  
27.             -- W key  
28.             local p = simGetObjectPosition(targetHandle, -1)  
29.             p[1] = p[1] - 0.001  
30.             simSetObjectPosition(targetHandle, -1, p)  
31.         end  
32.         if (auxiliaryData[1]==115) then  
33.             -- S key  
34.             local p = simGetObjectPosition(targetHandle, -1)  
35.             p[1] = p[1] + 0.001  
36.             simSetObjectPosition(targetHandle, -1, p)  
37.         end  
38.  
39.  
40.         if (auxiliaryData[1]==2007) then  
41.             -- up key  
42.             local p = simGetObjectPosition(targetHandle, -1)  
43.             p[3] = p[3] + 0.001  
44.             simSetObjectPosition(targetHandle, -1, p)  
45.         end  
46.         if (auxiliaryData[1]==2008) then  
47.             -- down key  
48.             local p = simGetObjectPosition(targetHandle, -1)  
49.             p[3] = p[3] - 0.001  
50.             simSetObjectPosition(targetHandle, -1, p)  
51.         end  
52.  
53.  
54.         if (auxiliaryData[1]==2009) then  
55.             -- left key  
56.             local p = simGetObjectPosition(targetHandle, -1)  
57.             p[2] = p[2] - 0.001  
58.             simSetObjectPosition(targetHandle, -1, p)  
59.         end  
60.         if (auxiliaryData[1]==2010) then  
61.             -- right key  
62.             local p = simGetObjectPosition(targetHandle, -1)  
63.             p[2] = p[2] + 0.001  
64.             simSetObjectPosition(targetHandle, -1, p)  
65.         end  
66.  
67.     end  
68. end  
69.  
70.  
71. if (sim_call_type==sim_childscriptcall_cleanup) then  
72.  
73.     -- Put some restoration code here  
74.  
75. end 

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

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

1. if (sim_call_type==sim_childscriptcall_initialization) then  
2.     -- Check if the plugin is loaded:  
3.     moduleName=0  
4.     moduleVersion=0  
5.     index=0  
6.     pluginNotFound=true 
7.     while moduleName do 
8.         moduleName,moduleVersion=simGetModuleName(index)  
9.         if (moduleName=='CHAI3D') then  
10.             pluginNotFound=false 
11.         end  
12.         index=index+1  
13.     end  
14.  
15.     if (pluginNotFound) then  
16.         simDisplayDialog('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})  
17.     else 
18.  
19.         -- Start the device:  
20.         local toolRadius = 0.001 -- the radius of the tool  
21.         local workspaceRadius = 0.2 -- the workspace radius  
22.         if simExtCHAI3D_start(0, toolRadius,workspaceRadius) ~= 1 then  
23.             simDisplayDialog('Error','Device failed to initialize.',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})  
24.         else 
25.             CHAI3DPluginInitialized = true 
26.  
27.         end  
28.     end  
29.  
30.     targetHandle = simGetObjectHandle('ikTarget')  
31.  
32.     deltaPos = {0, 0, 0}  
33.     counter = 0  
34.     ratio = 50  
35.  
36. end  
37.  
38.  
39. if (sim_call_type==sim_childscriptcall_actuation) then  
40.     if buttonState ==1 then  -- press the button  
41.         local p = simGetObjectPosition(targetHandle, -1) -- get the target position  
42.         -- add increment of the tool tip  
43.         p[1] = p[1] + deltaPos[1] / ratio                    
44.         p[2] = p[2] + deltaPos[2] / ratio  
45.         p[3] = p[3] + deltaPos[3] / ratio  
46.         simSetObjectPosition(targetHandle, -1, p)  -- move to the absolute position  
47.     end  
48. end  
49.  
50.  
51.  
52. if (sim_call_type==sim_childscriptcall_sensing) then  
53.     if CHAI3DPluginInitialized then  
54.         -- Read the current position of the cursor:  
55.         local currentToolPosition = simExtCHAI3D_readPosition(0)  
56.           
57.         -- Read the buttons of the device:  
58.         buttonState = simExtCHAI3D_readButtons(0)  
59.  
60.         counter = counter + 1      -- increase the counter  
61.           
62.         if counter % 30 == 1 then  -- keep the value  
63.             prevToolPosition = currentToolPosition  
64.         end  
65.  
66.         if counter % 30 == 0 then  -- calculate tool tip increment  
67.             deltaPos[1] = currentToolPosition[1] - prevToolPosition[1]  -- X-axis increment  
68.             deltaPos[2] = currentToolPosition[2] - prevToolPosition[2]  -- Y-axis increment  
69.             deltaPos[3] = currentToolPosition[3] - prevToolPosition[3]  -- Z-axis increment  
70.             counter = 0  -- reset counter  
71.  
72.  
73.         local info = string.format("CurrentPosition:%.2f,%.2f,%.2f  DeltaPosition:%.2f,%.2f,%.2f",  
74.                             currentToolPosition[1],currentToolPosition[2],currentToolPosition[3],  
75.                             deltaPos[1],deltaPos[2],deltaPos[3])  
76.         simAddStatusbarMessage(info)  
77.  
78.         end  
79.  
80.     end  
81. end  
82.  
83.  
84. if (sim_call_type==sim_childscriptcall_cleanup) then  
85.     if CHAI3DPluginInitialized then  
86.  
87.         -- Disconnects all devices and removes all objects from the scene  
88.         simExtCHAI3D_reset()  
89.     end  
90. end 

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

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

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

1. import math    
2. import numpy as np
3.  
4. # link length  
5. L1 = 1    
6. L2 = 1  
7.  
8. gamma = 1       # step size  
9. lamda = 0.2     # damping constant (DLS-method)  
10. stol = 1e-3     # tolerance  
11. nm = 10         # initial error  
12. count = 0       # iteration count  
13. ilimit = 20     # maximum iteration
14.  
15. # numerical method for inverse kinematics  
16. method = 'Pseudo Inverse'  # 'Pseudo Inverse', 'DLS', ...
17.  
18. # initial joint value  
19. q = np.array([0, 0, math.pi/2, 0]) # [theta1, d1, theta2, theta3]
20.  
21. # target position    
22. target_pos = np.array([1, 0, 2])   # [x,y,z]  
23.  
24.  
25. while True:  
26.     if(nm > stol):
27.         # forward kinematics:  
28.         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]),\  
29.         math.cos(q[2])*math.sin(q[0])*(L1+L2*math.cos(q[3]))-L2*math.cos(q[0])*math.sin(q[3]),\  
30.         q[1]+(L1+L2*math.cos(q[3]))*math.sin(q[2])])
31.  
32.         # compute error  
33.         error = target_pos - x
34.  
35.         # compute Jacobian  
36.         J11 = -math.sin(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.cos(q[0])*math.sin(q[3])  
37.         J12 = 0  
38.         J13 = -math.sin(q[2])*math.cos(q[0])*(L1+L2*math.cos(q[3]))  
39.         J14 = L2*(math.sin(q[0])*math.cos(q[3])-math.cos(q[0])*math.cos(q[2])*math.sin(q[3]))  
40.         J21 = math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3])  
41.         J22 = 0  
42.         J23 = -math.sin(q[0])*math.sin(q[2])*(L1+L2*math.cos(q[3]))  
43.         J24 = -L2*(math.cos(q[0])*math.cos(q[3])+math.sin(q[0])*math.cos(q[2])*math.sin(q[3]))  
44.         J31 = 0  
45.         J32 = 1  
46.         J33 = math.cos(q[2])*(L1+L2*math.cos(q[3]))  
47.         J34 = -L2*math.sin(q[2])*math.sin(q[3])  
48.  
49.         J = np.array([[J11,J12,J13,J14],[J21,J22,J23,J24],[J31,J32,J33,J34]])  
50.           
51.         if method == 'Pseudo Inverse': 
52.             # Pseudo Inverse Method  
53.             J_pseudo = np.dot(J.transpose(), np.linalg.inv(J.dot(J.transpose()))) # compute pseudo inverse  
54.             dq = gamma * J_pseudo.dot(error)  
55.               
56.         if method == 'DLS':
57.             # Damped Least Squares Method  
58.             f = np.linalg.solve(J.dot(J.transpose())+lamda**2*np.identity(3), error)  
59.             dq = np.dot(J.transpose(), f)
60.  
61.         # update joint position     
62.         q = q + dq  
63.  
64.         nm = np.linalg.norm(error)  
65.  
66.         count = count + 1  
67.         if count > ilimit:  
68.             print "Solution wouldn't converge!" 
69.             break 
70.     else:
71.         # print result  
72.         print '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)  
73.         print 'Current position: %.2f, %.2f, %.2f' %(x[0],x[1],x[2])      
74.         print str(count)+' iterations'+'  err:'+str(nm)  
75.         break