Enabling spring-damper mode through regular API?

Typically: "How do I... ", "How can I... " questions
coppelia
Site Admin
Posts: 7255
Joined: 14 Dec 2012, 00:25

Re: Enabling spring-damper mode through regular API?

Post by coppelia » 29 Jan 2018, 21:26

What I meant to say is following: if you need to switch between a position control, a spring/damper, and maybe something else, then you just implement it in a joint callback script. This could look like this (in V-REP V3.5.0 and later):

Code: Select all

function sysCall_init()
end

function sysCall_jointCallback(inData)
    if inData.first then
        K=250
        C=6.25
        P=0.1
        I=0
        D=0
        pidCumulativeErrorForIntegralParam=0
        springDamperMode=true -- Switch betweem position ctrl and spring/damper
    end

    if springDamperMode then
        PID_P=K/inData.maxForce
        PID_I=0
        PID_D=C/inData.maxForce
    else
        PID_P=P
        PID_I=I
        PID_D=D
    end

    -- 1. Proportional part:
    local ctrl=inData.errorValue*PID_P;

    -- 2. Integral part:
    if PID_I~=0 then
        pidCumulativeErrorForIntegralParam=pidCumulativeErrorForIntegralParam+inData.errorValue*inData.dynStepSize
    else
        pidCumulativeErrorForIntegralParam=0
    end
    ctrl=ctrl+pidCumulativeErrorForIntegralParam*PID_I
    
    -- 3. Derivative part:
    if not inData.first then
        ctrl=ctrl+(inData.errorValue-pidLastErrorForDerivativeParam)*PID_D/inData.dynStepSize
    end
    pidLastErrorForDerivativeParam=inData.errorValue

    local outData={}

    if springDamperMode then
        -- We modulate the force/torque:
        outData.velocity=math.abs(inData.targetVel)
        if ctrl<0 then
            outData.velocity=-outData.velocity
        end
        outData.force=math.abs(ctrl)*inData.maxForce
        if outData.force>inData.maxForce then
            outData.force=inData.maxForce
        end
    else
        -- we modulate the velocity:
        outData.velocity=ctrl/inData.dynStepSize -- max. velocity allowed.
        if (outData.velocity>inData.velUpperLimit) then
            outData.velocity=inData.velUpperLimit
        end
        if (outData.velocity<-inData.velUpperLimit) then
            outData.velocity=-inData.velUpperLimit
        end
        outData.force=inData.maxForce -- the maximum force/torque that the joint will be able to exert
    end

    return outData
end
Cheers

Post Reply