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

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, ,'Mass set to '..newVal)

    if id ==    then
        simSetShapeMassAndInertia(L1_handle, newVal, L1_inertiaMatrix, L1_centerOfMass)
        simSetFloatSignal('L1_mass', newVal)
    elseif id ==  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, , L1_inertiaMatrix, L1_centerOfMass)
    simSetShapeMassAndInertia(L2_handle, , 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