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

　　機器人運動學逆解的問題常常出如今動畫仿真和工業機器人的軌跡規劃中：We want to know how the upper joints of the hierarchy would rotate if we want the end effector to reach some goal.

IK Solutions：

• Analytical solutions are desirable because of their speed and exactness of solution.
• For complex kinematics problems, analytical solutions may not be possible.
• Use iterative methods (迭代法)--Optimization methods (e.g., minimize the distance between end effector and goal point)

　　機器人學的教材上通常只介紹了運動學逆解的解析解法，不多涉及數值解（迭代解法）。其中有兩方面的緣由，一是數值解相對於解析解須要的運算量更大，不太適合實時性要求較高的場合；另外一方面是由於通常在設計機器人的結構時就會考慮其逆解的可解性，好比相鄰的三個關節軸線相交即存在解析解。下面以最簡單的二連桿機構爲例子，研究一下機器人運動學逆解的數值解法。以下圖所示，其運動學正解很容易獲得，根據運動學正解能夠寫出機器人的雅克比矩陣J。

What is Jacobian? A linear approximation to f(x). 雅克比矩陣至關於函數f(x)的一階導數，即線性近似。

Computing the Jacobian Numerically:（雅克比矩陣的計算有解析法和數值法，當難以根據解析法獲得雅克比時能夠用差分法代替微分求解）

下面是Jacobian Transpose方法的主要推導過程：

　　It is recommended that you choose a small positive scalar α < 1 and update the joint angles θ by adding Δθ. Then proceed iteratively by recomputing the Jacobian based on the updated angles and positions, finding new values for Δθ and again updating with a small fraction α. This is repeated until the links are sufficiently close to the desired positions. The question of how small α needs to be depends on the geometry of the links; it would be a good idea to keep α small enough so that the angles are updated by at most 5 or 10° at a time.

　　這一方法運用了最速降法（梯度法）的思想，利用目標函數在迭代點的局部性態，每步搜索都沿着函數值降低最快的方向，即負梯度方向進行搜索。迭代過程的幾何概念較爲直觀，方法和程序簡單，容易實現，不足之處是每次迭代都是沿着迭代點的負梯度方向搜索，搜索路徑較爲曲折，收斂慢。

Operating Principle:  

Project difference vector DX on those dimensions qwhich can reduce it the most. It is a plausible, justifyable approach, because it is related to the method of steepest decent.( It follows a force that pulls the end-effector towards its desired target location).

Advantages:
1. Simple computation (numerically robust)
2. No matrix inversions

Disadvantages:
1. Needs many iterations until convergence in certain configurations 
2. Unpredictable joint configurations
3. Non conservative

 

　　下面以V-rep中官方自帶的例子（在文件夾scenes/ik_fk_simple_examples中）爲基礎進行修改，添加代碼手動實現運動學逆解的求解。爲了實現一樣的功能先將兩個旋轉關節從Inverse kinematics mode設爲Passive mode，而後將target和tip的Linked dummy設爲none，並在Calculation Modules的Inverse kinematics選項卡中取消IK groups enabled。下圖中紅色的dummy爲target dummy，仿真開始後程序會計算連桿末端（tip dummy）與target之間的偏差，而後根據Jacobian Transpose方法不斷計算關節調整量，直到偏差小於允許值。

　　以下圖所示，迭代計算61次收斂後，點擊圖中的Compute IK按鈕，連桿末端能根據計算出的關節角q1,q2移動到target的位置（能夠隨意拖動target的位置，在合理的範圍內通過逆解計算tip都會與target重合）

在child script中由lua API實現了該方法

 

if (sim_call_type==sim_childscriptcall_initialization) then
    ui=simGetUIHandle('UI')
    J1_handle = simGetObjectHandle('j1')
    J2_handle = simGetObjectHandle('j2')
    target_handle = simGetObjectHandle('target')
    consoleHandle = simAuxiliaryConsoleOpen('info', 5, 2+4, {100,100},{800,300})
    
    --link length
    L1 = 0.5
    L2 = 0.5

    gamma = 1       --step size
    stol = 1e-2     --tolerance
    nm = 100        --initial error
    count = 0       --iteration count
    ilimit = 1000   --maximum iteratio
    
    --initial joint value
    q1 = 0   
    q2 = 0
end


if (sim_call_type==sim_childscriptcall_actuation) then
    local a=simGetUIEventButton(ui)
    local target_pos = simGetObjectPosition(target_handle, -1)

    if(nm > stol) 
    then
        simAuxiliaryConsolePrint(consoleHandle, nil)

        local x, y = L1*math.cos(q1)+L2*math.cos(q1+q2), L1*math.sin(q1)+L2*math.sin(q1+q2)
        local delta_x, delta_y = target_pos[1] - x, target_pos[2] - y
        local dq_1 = (-L1*math.sin(q1)-L2*math.sin(q1+q2))*delta_x + (L1*math.cos(q1)+L2*math.cos(q1+q2))*delta_y
        local dq_2 = (-L2*math.sin(q1+q2))*delta_x + (L2*math.cos(q1+q2))*delta_y
        q1, q2 = q1 + gamma*dq_1,  q2 + gamma*dq_2

        nm = math.sqrt(delta_x * delta_x + delta_y * delta_y)

        count = count + 1
        if count > ilimit   then
            simAuxiliaryConsolePrint(consoleHandle,"Solution wouldn't converge\r\n")
        end

        simAuxiliaryConsolePrint(consoleHandle, string.format("q1:%.2f", q1*180/math.pi)..'  '..string.format("q2:%.2f", q2*180/math.pi)..'\r\n') 
        simAuxiliaryConsolePrint(consoleHandle, string.format("x:%.2f",x)..'  '..string.format("y:%.2f", y)..'\r\n') 
        simAuxiliaryConsolePrint(consoleHandle, string.format("%d", count)..'iterations'..'  '..string.format("err:%.4f", nm)..'\r\n')   
    end

    -- if the button(a is the button handle) is pressed
    if a==1 then
        simSetJointPosition(J1_handle, q1+math.pi/2)  -- the angle between L1 and X-axis is 90 degree
        simSetJointPosition(J2_handle, q2)      
    end
end