V-rep學習筆記：機器人逆運動學數值解法（The Pseudo Inverse Method）

　　There are two ways of using the Jacobian matrix to solve kinematics. One is to use the transpose of the Jacobian J^T. The other is to calculate the inverse of the Jacobian J^-1. J is most likely redundant and non square,thus an ordinary inverse is not possible. We can try using the pseudo inverse to find a matrix that effectively inverts a non square matrix. J⁺ is the pseudoinverse of J, also called the Moore-Penrose inverse of J . It is defined for all matrices J , even ones which are not square or not of full row rank.

　　雅可比矩陣將關節空間速度映射到直角座標空間：ΔP = J(θ)Δθ。對於機器人運動學逆解來講能夠考慮求雅克比矩陣J的逆，而後根據Δθ=J(θ)^-1ΔP計算出關節角變更量並反覆迭代。然當不少狀況下J不可逆，所以能夠考慮求其廣義逆（Moore-Penrose逆）來求解方程。設A∈C^m×n，b∈C^m，則線性方程組Ax=b有解的充分必要條件是AA⁺b=b，且通解爲x=A⁺b + (I-A⁺A)y (y∈Cⁿ任意)，而且它的惟一極小範數解爲x₀ = A⁺b（矩陣論簡明教程 P150 A⁺在解線性方程組中的應用）。根據矩陣論（《矩陣論簡明教程 第二版》 科學出版社 第6章 廣義逆矩陣），設J爲m×n階實矩陣，當rankJ=m時，有J⁺ = J^T(J J^T)^-1；而當rankJ=n時，有J⁺ = (J^TJ )^-1 J^T，此時方程組J(θ)Δθ=ΔP的解惟一。 對通常機器人來講n≥m，且rankJ=m，即有J⁺ = J^T(J J^T)^-1。當n>m，即機構驅動數目多於末端自由度時，會出現多解的狀況，pseudo inverse方法會尋找解向量中長度最小的一個（無窮多個解中2範數最小的解，即||Δθ₀||₂=min||Δθ||₂，稱爲極小範數解）

　Let  ΔP be e = t - P,where t is the target position,P is the end effector position and e is the desirable change of the end effector. The first iteration will result in a new θ from equation Δθ=J(θ)^-1ΔP. By using forward kinematics a new position P of the end effector is acquired and a new iteration begins. This is done until either e is small enough or the end effector does not move. 使用Pseudo Inverse方法求機器人逆解的基本步驟以下所示：

Operating Principle:
1. Shortest path in q-space
Advantages:
1. Computationally fast (second order method)
Disadvantages:
1. Matrix inversion necessary (numerical problems)
2. Unpredictable joint configurations
3. Non conservative

　　The pseudoinverse tends to have stability problems in the neighborhoods of singularities. At a singularity, the Jacobian matrix no longer has full row rank, corresponding to the fact that there is a direction of movement of the end effectors which is not achievable. If the configuration is exactly at a singularity, then the pseudoinverse method will not attempt to move in an impossible direction, and the pseudoinverse will be well-behaved. However, if the configuration is close to a singularity, then the pseudoinverse method will lead to very large changes in joint angles, even for small movements in the target position. In practice, roundoff errors mean that true singularities are rarely reached and instead singularity have to be detected by checking values for being near-zero. 對於平面二連桿機構，當θ₂趨近於0°或180°時，機械手接近奇異形位，關節J2速度將趨於無窮大。（參考John J.Craig. Introduction to Robotics: Mechanics and Control Chapter 5-->Section 5.8 Singularities）

　　下面使用一樣的模型驗證Pseudo Inverse方法。從輸出窗口能夠看出，該方法迭代次數相比Jacobian Transpose法明顯減小（迭代5次就達到精度要求）。The Jacobian pseudoinverse method is equivalent to solving by Newton's method.(至關於牛頓法)。Jacobian transpose is also related to solution by the method of steepest descent.（至關於最速降法或梯度法）。牛頓法是梯度法的進一步發展，梯度法在肯定搜索方向時只考慮目標函數在迭代點的局部性質，即只利用一階偏導數的信息，而牛頓法進一步利用了目標函數的二階偏導數，考慮了梯度的變化趨勢，於是能夠更爲全面的肯定合適的搜索方向，以便很快的搜索到極小點。

import vrep             #V-rep library
import sys      
import time
import math  
import numpy as np

# Starts a communication thread with the server (i.e. V-REP). 
clientID=vrep.simxStart('127.0.0.1', 20001, True, True, 5000, 5)

# clientID: the client ID, or -1 if the connection to the server was not possible
if clientID!=-1:  #check if client connection successful
    print 'Connected to remote API server'
else:
    print 'Connection not successful'
    sys.exit('Could not connect')    # Exit from Python


# Retrieves an object handle based on its name.
errorCode,J1_handle = vrep.simxGetObjectHandle(clientID,'j1',vrep.simx_opmode_oneshot_wait)
errorCode,J2_handle = vrep.simxGetObjectHandle(clientID,'j2',vrep.simx_opmode_oneshot_wait)
errorCode,target_handle = vrep.simxGetObjectHandle(clientID,'target',vrep.simx_opmode_oneshot_wait)
errorCode,consoleHandle = vrep.simxAuxiliaryConsoleOpen(clientID,'info',5,1+4,None,None,None,None,vrep.simx_opmode_oneshot_wait)

uiHandle = -1
errorCode,uiHandle = vrep.simxGetUIHandle(clientID,"UI", vrep.simx_opmode_oneshot_wait)
buttonEventID = -1
err,buttonEventID,aux = vrep.simxGetUIEventButton(clientID,uiHandle,vrep.simx_opmode_streaming)


L1 = 0.5    # link length
L2 = 0.5
gamma = 1       # step size
stol = 1e-2     # tolerance
nm = 100        # initial error
count = 0       # iteration count
ilimit = 1000   # maximum iteration


# initial joint value
# note that workspace-boundary singularities occur when q2 approach 0 or 180 degree
q = np.array([0,1])   


while True:
    retcode, target_pos = vrep.simxGetObjectPosition(clientID, target_handle, -1, vrep.simx_opmode_streaming)


    if(nm > stol):
        vrep.simxAuxiliaryConsolePrint(clientID, consoleHandle, None, vrep.simx_opmode_oneshot_wait) # "None" to clear the console window

        x = np.array([L1*math.cos(q[0])+L2*math.cos(q[0]+q[1]), L1*math.sin(q[0])+L2*math.sin(q[0]+q[1])])
        error = np.array([target_pos[0],target_pos[1]]) - x

        J = np.array([[-L1*math.sin(q[0])-L2*math.sin(q[0]+q[1]), -L2*math.sin(q[0]+q[1])],\
                      [L1*math.cos(q[0])+L2*math.cos(q[0]+q[1]), L2*math.cos(q[0]+q[1])]])

        J_pseudo = np.dot(J.transpose(), np.linalg.inv(J.dot(J.transpose())))
        dq = J_pseudo.dot(error)
        q = q + dq

        nm = np.linalg.norm(error)

        count = count + 1
        if count > ilimit:
            vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,"Solution wouldn't converge\r\n",vrep.simx_opmode_oneshot_wait)
        vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,'q1:'+str(q[0]*180/math.pi)+' q2:'+str(q[1]*180/math.pi)+'\r\n',vrep.simx_opmode_oneshot_wait)  
        vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,str(count)+' iterations'+'  err:'+str(nm)+'\r\n',vrep.simx_opmode_oneshot_wait)   


    err, buttonEventID, aux = vrep.simxGetUIEventButton(clientID,uiHandle,vrep.simx_opmode_buffer)    
    if ((err==vrep.simx_return_ok) and (buttonEventID == 1)):
        '''A button was pressed/edited/changed. React to it here!'''
        vrep.simxSetJointPosition(clientID,J1_handle, q[0]+math.pi/2, vrep.simx_opmode_oneshot )  
        vrep.simxSetJointPosition(clientID,J2_handle, q[1], vrep.simx_opmode_oneshot )
        
        '''Enable streaming again (was automatically disabled with the positive event):'''
        err,buttonEventID,aux=vrep.simxGetUIEventButton(clientID,uiHandle,vrep.simx_opmode_streaming)

    time.sleep(0.01)

　　上面使用Python Remote API來進行逆解計算並控制V-rep中的模型（由於涉及到矩陣求逆等運算，而我不太熟悉Lua的相關數值計算庫）。須要注意的是要先在V-rep模型中調用函數simExtRemoteApiStart(portNumber)開啓通訊服務端，而後在Python程序的客戶端進行鏈接。