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

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

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

　　為了在仿真過程中查看信號變量，可以將模型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