V-rep學習筆記：關節力矩控制

• Torque or force mode
  

　　When the joint motor is enabled and the control loop is disabled, then the joint will try to reach the desired target velocity given the maximum torque/force it is capable to deliver. When that maximum torque/force is very high, the target velocity is instantaneously reached and the joint operates in velocity control, otherwise it operates at the specified torque/force until the desired target velocity is reached (torque/force control).

　　若是想要對關節進行力控制VREP中沒有現成的函數，只能經過間接的方式：將目標速度設的足夠大（不可能短期達到），而後在關節控制腳本中修改源代碼，設置並返回關節最大力矩或經過simSetJointForce函數設置關節最大力矩，這樣關節就能夠一直按照設置的最大力矩運轉，從而達到調節關節力矩的目的。

　　When the joint motor is enabled and the control loop is enabled, then the user has 3 control modes available:

• Custom control: a joint control callback script will be in charge of controlling the dynamic behaviour of the joint, allowing you to control the joint with any imaginable algorithm.
• Position control (PID): the joint will be controlled in position via a PID controller that will adjust the joint velocity in following way (the Δt divider is to keep the controller independent of the selected controller time step):

• Spring-damper mode: the joint will act like a spring-damper system via a force/torque modulation:

---

　　根據上面的思路，在作一些底層控制涉及到調節關節力矩時能夠經過修改關節控制回調腳本（腳本默認進行PID控制，控制量爲關節速度），按照需求計算力矩並返回給VREP的物理引擎，這種方式比調用simSetJointForce函數要方便不少。下面是一個簡單的例子：新建一個場景，添加一個連桿並將其用轉動關節鏈接到大地。而後雙擊轉動關節圖標，將其設置爲力矩模式，進行Custom control。修改關節控制腳本（joint control callback scripts）：

-- Following data is handed over from V-REP:
init,revolute,cyclic,jointHandle,passCnt,totalPasses,currentPos,targetPos,errorValue,effort,dynStepSize,lowLimit,highLimit,targetVel,maxForceTorque,velUpperLimit=... --Retrieves the current simulation time
local t = simGetSimulationTime() forceOrTorqueToApply = math.sin(math.pi * t / 2) maxVelocity = 9.0e+04

-- Following data must be returned to V-REP:
return forceOrTorqueToApply,maxVelocity -- forceOrTorqueToApply: the maximum force/torque that the joint will be able to exert -- maxVelocity: max. velocity allowed.

　　添加一個Graph記錄關節轉矩，從下圖能夠看出關節力矩按照正弦函數變化，週期爲4s，與預期一致：

　　下面一個例子中，根據靜力平衡條件計算關節轉矩，使得二連桿機構在重力做用下能保持平衡。兩個密度均勻的連桿長度爲0.5m，初始質量均爲1Kg，能夠經過自定義界面上的兩個滑動條來改變連桿質量，並設置相應的信號，關節控制腳本中讀取信號變化計算力矩。

CustomeUI：

function sliderChange(ui, id, newVal)
    simExtCustomUI_setLabelText(ui, 3000,'Mass set to '..newVal)

    if id == 3001   then
        simSetShapeMassAndInertia(L1_handle, newVal, L1_inertiaMatrix, L1_centerOfMass)
        simSetFloatSignal('L1_mass', newVal)
    elseif id == 3002 then
        simSetShapeMassAndInertia(L2_handle, newVal, L2_inertiaMatrix, L2_centerOfMass)
        simSetFloatSignal('L2_mass', newVal)
    end

end
    
   

if (sim_call_type==sim_childscriptcall_initialization) then
    xml = [[
<ui>
<tabs>
        <tab title="Set link mass">
            <label text="Sliders can be oriented horizontally or vertically, and have optional properties that can be set (in the XML) such as minimum and maximum value." wordwrap="true" />
            <label text="" id="3000" wordwrap="true" />
            <hslider id="3001" tick-position="above" tick-interval="1" minimum="1" maximum="20" onchange="sliderChange" />
            <vslider id="3002" minimum="1" maximum="20" onchange="sliderChange" />
            <label text="Spinboxes allow editing a numeric value using two buttons for increase and decrease, or direct text input. Spinboxes can display a prefix and a suffix text." wordwrap="true" />
            <spinbox minimum="1" maximum="20" suffix="Kg"/>
            <spinbox minimum="1" maximum="20" suffix="Kg" />
            <stretch />
        </tab>
</tabs>
</ui>
]]
    ui = simExtCustomUI_create(xml)

    L1_handle = simGetObjectHandle('L1')
    L2_handle = simGetObjectHandle('L2')
    L1_mass,L1_inertiaMatrix,L1_centerOfMass = simGetShapeMassAndInertia(L1_handle)
    L2_mass,L2_inertiaMatrix,L2_centerOfMass = simGetShapeMassAndInertia(L2_handle)
    simSetFloatSignal('L1_mass', L1_mass)
    simSetFloatSignal('L2_mass', L2_mass)
end


if (sim_call_type==sim_childscriptcall_actuation) then
end

if (sim_call_type==sim_childscriptcall_sensing) then

end

if (sim_call_type==sim_childscriptcall_cleanup) then
    simSetShapeMassAndInertia(L1_handle, 1, L1_inertiaMatrix, L1_centerOfMass)
    simSetShapeMassAndInertia(L2_handle, 1, L2_inertiaMatrix, L2_centerOfMass)
    simExtCustomUI_destroy(ui)
end

View Code

J1 joint control callback scripts:

init,revolute,cyclic,jointHandle,passCnt,totalPasses,currentPos,targetPos,errorValue,effort,dynStepSize,lowLimit,highLimit,targetVel,maxForceTorque,velUpperLimit=...

local m1 = simGetFloatSignal('L1_mass')
local m2 = simGetFloatSignal('L2_mass')


forceOrTorqueToApply = (2.5*m1 + 7.5*m2)

maxVelocity = 9.0e+04

-- Following data must be returned to V-REP:
return forceOrTorqueToApply,maxVelocity

View Code

J2 joint control callback scripts:

init,revolute,cyclic,jointHandle,passCnt,totalPasses,currentPos,targetPos,errorValue,effort,dynStepSize,lowLimit,highLimit,targetVel,maxForceTorque,velUpperLimit=... local m2 = simGetFloatSignal('L2_mass') forceOrTorqueToApply = (2.5 * m2) maxVelocity = 9.0e+04

-- Following data must be returned to V-REP:
return forceOrTorqueToApply,maxVelocity

View Code

　　爲了在仿真過程當中查看信號變量，能夠將模型signal monitor（Models/other/signal monitor.ttm）拖入場景中，顯示信號的變化：

signal monitor.ttm

 

參考：

Joint types and operation

V-rep學習筆記：轉動關節2

How do I simulate a torque motor?

Joints Torque Control