使用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 here
    targetHandle = simGetObjectHandle('ikTarget')


end


if (sim_call_type==sim_childscriptcall_actuation) then

    -- Put your main ACTUATION code here

end



if (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) then

        if (auxiliaryData[1]==119) then
            -- W key
            local p = simGetObjectPosition(targetHandle, -1)
            p[1] = p[1] - 0.001
            simSetObjectPosition(targetHandle, -1, p)
        end
        if (auxiliaryData[1]==115) then
            -- S key
            local p = simGetObjectPosition(targetHandle, -1)
            p[1] = p[1] + 0.001
            simSetObjectPosition(targetHandle, -1, p)
        end


        if (auxiliaryData[1]==2007) then
            -- up key
            local p = simGetObjectPosition(targetHandle, -1)
            p[3] = p[3] + 0.001
            simSetObjectPosition(targetHandle, -1, p)
        end
        if (auxiliaryData[1]==2008) then
            -- down key
            local p = simGetObjectPosition(targetHandle, -1)
            p[3] = p[3] - 0.001
            simSetObjectPosition(targetHandle, -1, p)
        end


        if (auxiliaryData[1]==2009) then
            -- left key
            local p = simGetObjectPosition(targetHandle, -1)
            p[2] = p[2] - 0.001
            simSetObjectPosition(targetHandle, -1, p)
        end
        if (auxiliaryData[1]==2010) then
            -- right key
            local p = simGetObjectPosition(targetHandle, -1)
            p[2] = p[2] + 0.001
            simSetObjectPosition(targetHandle, -1, p)
        end

    end
end


if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here

end

　　測試沒問題后可以使用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=0
    moduleVersion=0
    index=0
    pluginNotFound=true
    while moduleName do
        moduleName,moduleVersion=simGetModuleName(index)
        if (moduleName=='CHAI3D') then
            pluginNotFound=false
        end
        index=index+1
    end

    if (pluginNotFound) then
        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})
    else

        -- Start the device:
        local toolRadius = 0.001 -- the radius of the tool
        local workspaceRadius = 0.2 -- the workspace radius
        if simExtCHAI3D_start(0, toolRadius,workspaceRadius) ~= 1 then
            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})
        else
            CHAI3DPluginInitialized = true

        end
    end

    targetHandle = simGetObjectHandle('ikTarget')

    deltaPos = {0, 0, 0}
    counter = 0
    ratio = 50

end


if (sim_call_type==sim_childscriptcall_actuation) then
    if buttonState ==1 then  -- press the button
        local p = simGetObjectPosition(targetHandle, -1) -- get the target position
        -- add increment of the tool tip
        p[1] = p[1] + deltaPos[1] / ratio                  
        p[2] = p[2] + deltaPos[2] / ratio
        p[3] = p[3] + deltaPos[3] / ratio
        simSetObjectPosition(targetHandle, -1, p)  -- move to the absolute position
    end
end



if (sim_call_type==sim_childscriptcall_sensing) then
    if 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 counter
        
        if counter % 30 == 1 then  -- keep the value
            prevToolPosition = currentToolPosition
        end

        if counter % 30 == 0 then  -- calculate tool tip increment
            deltaPos[1] = currentToolPosition[1] - prevToolPosition[1]  -- X-axis increment
            deltaPos[2] = currentToolPosition[2] - prevToolPosition[2]  -- Y-axis increment
            deltaPos[3] = currentToolPosition[3] - prevToolPosition[3]  -- Z-axis increment
            counter = 0  -- reset counter


        local 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)

        end

    end
end


if (sim_call_type==sim_childscriptcall_cleanup) then
    if 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 = 1

gamma = 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 error
        error = target_pos - x

        # compute Jacobian
        J11 = -math.sin(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.cos(q[0])*math.sin(q[3])
        J12 = 0
        J13 = -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 = 0
        J23 = -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 = 0
        J32 = 1
        J33 = 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 Method
            J_pseudo = np.dot(J.transpose(), np.linalg.inv(J.dot(J.transpose()))) # compute pseudo inverse
            dq = gamma * J_pseudo.dot(error)
            
        if method == 'DLS':
            # Damped Least Squares Method
            f = np.linalg.solve(J.dot(J.transpose())+lamda**2*np.identity(3), error)
            dq = np.dot(J.transpose(), f)

        # update joint position   
        q = q + dq

        nm = np.linalg.norm(error)

        count = count + 1
        if count > ilimit:
            print "Solution wouldn't converge!"
            break
    else:
        # print result
        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)
        print 'Current position: %.2f, %.2f, %.2f' %(x[0],x[1],x[2])    
        print str(count)+' iterations'+'  err:'+str(nm)
        break

 

 

參考：

OpenHaptics編程環境搭建

VREP中的力觸覺設備接口（CHAI3D）

 “逆運動學”——從操作空間到關節空間（上篇）