Joint callback functions

Child scripts, or customization scripts can include a joint callback function, which is one of many system callback functions. When present for a given joint, then it will be called by CoppeliaSim in various situations:

When the joint is in kinematic mode: a call to sim.setJointTargetPosition or sim.setJointTargetVelocity will trigger the joint callback function. If not present in a script attached to the given joint, then the call will be redirected to the main script that offers a default behaviour, i.e. a specific motion profile will be applied

When the joint is in dynamic mode, is dynamically enabled, and is set to custom control: the physics engine will trigger the joint callback function for each dynamics simulation step, which is quite often, normally 10 times per simulation step for a given joint

Joint callback functions enable the user to customize the control loop for specific joints in order to write low-level control algorithms.

Following represents a simple PID joint callback function, for a joint in dynamic mode:


function sysCall_jointCallback(inData)
    -- inData.mode : sim.jointmode_kinematic or sim.jointmode_dynamic
    --
    -- inData.handle : the handle of the joint associated with this script
    -- inData.revolute : whether the joint associated with this script is revolute or prismatic
    -- inData.cyclic : whether the joint associated with this script is cyclic or not
    -- inData.lowLimit : the lower limit of the joint associated with this script (if the joint is not cyclic)
    -- inData.highLimit : the higher limit of the joint associated with this script (if the joint is not cyclic)
    -- inData.currentPos : the current position
    -- inData.targetPos : the desired position (if joint is dynamic, or when sim.setJointTargetPosition was called)
    -- inData.targetVel : the desired velocity (if joint is dynamic, or when sim.setJointTargetVelocity was called)
    -- inData.initVel : the desired initial velocity (if joint is kinematic and when sim.setJointTargetVelocity
    --                  was called with a 4th argument)
    -- inData.errorValue : targetPos-currentPos (with revolute cyclic joints, the shortest cyclic distance)
    -- inData.maxVel : a maximum velocity, taken from sim.setJointTargetPosition or 
    --                 sim.setJointTargetVelocity's 3rd argument)
    -- inData.maxAccel : a maximum acceleration, taken from sim.setJointTargetPosition or
    --                   sim.setJointTargetVelocity's 3rd argument)
    -- inData.maxJerk : a maximum jerk, taken from sim.setJointTargetPosition or
    --                  sim.setJointTargetVelocity's 3rd argument)
    -- inData.first : whether this is the first call from the physics engine, since the joint
    --                was initialized (or re-initialized) in it.
    -- inData.passCnt : the current dynamics calculation pass. 1-10 by default
    -- inData.totalPasses : the number of dynamics calculation passes for each "regular" simulation pass.
    -- inData.effort : the last force or torque that acted on this joint along/around its axis. With Bullet,
    --                 torques from joint limits are not taken into account
    -- inData.dynStepSize : the step size used for the dynamics calculations (by default 5ms)
    -- inData.force : the joint force/torque, as set via sim.setJointTargetForce
    -- inData.velUpperLimit : the joint velocity upper limit

    if inData.first then
        PID_P=0.1
        PID_I=0
        PID_D=0
        pidCumulativeErrorForIntegralParam=0
    end
    
    -- The control happens here:
    -- 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
    
    -- 4. Calculate the velocity needed to reach the position in one dynamic time step:
    local maxVelocity=ctrl/inData.dynStepSize -- max. velocity allowed.
    if (maxVelocity>inData.velUpperLimit) then
        maxVelocity=inData.velUpperLimit
    end
    if (maxVelocity<-inData.velUpperLimit) then
        maxVelocity=-inData.velUpperLimit
    end
    local forceOrTorqueToApply=inData.maxForce -- the maximum force/torque that the joint will be able to exert

    -- 5. Following data must be returned to CoppeliaSim:
    firstPass=false
    local outData={}
    outData.velocity=maxVelocity
    outData.force=forceOrTorqueToApply
    return outData
    
    -- Expected return data:
    -- For kinematic joints:
    -- outData={position=pos, velocity=vel, immobile=false}
    -- 
    -- For dynamic joints:
    -- outData={force=f, velocity=vel}
end

In Python, a joint callback function can only be implemented via a non-threaded script, and it should be explicitly activated with a luaExec command:


#python
#luaExec additionalFuncs={'sysCall_jointCallback'}

def sysCall_jointCallback(inData):
    pass