Joint control callback scripts
Joint control callback scripts, which are simulation scripts, can be enabled via the joint dynamics properties dialog. When enabled for a given joint (that must be dynamically enabled), then the physics engine will call the callback script with appropriate arguments, allowing the user to customize the control loop of the related joint in order to write low-level control algorithms for specific joints. The joint control callback script might be called quite often, normally 10 times per simulation step for a given joint (remember that the physics engine time step, by default, is 10 times smaller that the simulation time step). For that reason, keep things simple, in order to avoid slowing down the simulation. Following represents a simple PID joint control callback script:
-- Following data is handed over from V-REP: init,revolute,cyclic,jointHandle,passCnt,totalPasses,currentPos,targetPos,errorValue, effort,dynStepSize,lowLimit,hightLimit,targetVel,maxForceTorque,velUpperLimit=... -- init: true when this callback is called for the first time (if the joint is -- dynamically reset during the simulation, this might be true more often) -- revolute: true if the joint is revolute -- cyclic: true if the joint is revolute and cyclic (i.e. no lower/upper limits) -- passCnt: the current dynamics calculation pass. 0-9 by default. See next -- item for details. -- totalPasses: the number of dynamics calculation passes for each "regular" -- simulation pass. 10 by default (i.e. 10*5ms=50ms which is the default -- simulation time step) -- currentPos: the current position of the joint -- targetPos: the desired position of the joint -- errorValue: targetPos-currentPos (with revolute cyclic joints we take the -- shortest cyclic distance) -- 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 -- dynStepSize: the step size used for the dynamics calculations (by default 5ms) -- lowLimit: the joint lower limit -- highLimit: the joint upper limit -- targetVel: the joint target velocity (as set in the user interface) -- maxForceTorque: the joint maximum force/torque (as set in the user interface) -- velUpperLimit: the joint velocity upper limit (as set in the user interface) -- The control happens here: -- 1. PID parameter def: if not PID_P then PID_P=0.1 PID_I=0 PID_D=0 end -- 2. Clear some values when the dynamic joint calls this the first time (this can happen several times, if the joint is reset dynamically): if init then pidCumulativeErrorForIntegralParam=0 end -- 3. Proportional part: ctrl=errorValue*PID_P -- 4. Integral part: if PID_I~=0 then pidCumulativeErrorForIntegralParam=pidCumulativeErrorForIntegralParam+errorValue*dynStepSize else pidCumulativeErrorForIntegralParam=0 end ctrl=ctrl+pidCumulativeErrorForIntegralParam*PID_I -- 5. Derivative part: if not init then ctrl=ctrl+(errorValue-pidLastErrorForDerivativeParam)*PID_D/dynStepSize end pidLastErrorForDerivativeParam=errorValue -- 6. Calculate the velocity needed to reach the position in one dynamic time step: velocityToApply=ctrl/dynStepSize if (velocityToApply > velUpperLimit) then velocityToApply=velUpperLimit end if (velocityToApply < -velUpperLimit) then velocityToApply=-velUpperLimit end forceOrTorqueToApply=maxForceTorque -- Following data must be returned to V-REP: return forceOrTorqueToApply,velocityToApply -- forceOrTorqueToApply: the maximum force/torque that the joint will be able to exert -- velocityToApply: the velocity to apply to the joint.