How to implement my own PID controller?

[Cesarg21]

Hi everyone, my question is relatively trivial I think.

How can I implement the PID control on each robot manipulator joint given that:

1) If I try to implement it inside the sysCall_actuation() function of my robot base's child script, it will only run the PID code once during each simulation step and not during each dynamics step (which is what it should do for correct calculations right?).

2) I tried to add sysCall_joint() in both my robot base and joint's child script to see what would happen, but I have no idea how that function is being called and how should I use it correctly.


Given my failure in the implementation, I even set up a simple pendulum (1 joint) configuration to control the angle but still can't get it :/.

Thanks.

[coppelia]

Hello,

about 1) and 2) you are basically right: you need to handle to joint within a callback from the physics engine, which happens at a higher rate than the simulation step (10x more often by default).

Have a look at the demo scene scenes/motorControllerExamples-lua.ttt, specifically look at the yellow joint named mixedControlledJoint. Basically you need to:

• make sure that your joint is dynamically enabled (in dynamic mode, with correct attachments, i.e. a shape as parent, and a non-static shape as child)
• Make sure the joint is in Custom control mode
• Attach a child script to each of the joints in your arm, or attach a single child script to the base of the arm (better approach)
• Add a sysCall_joint callback function in that script
• The control of the joints happens in that callback function:

Code: Select all

function sysCall_joint(inData)
    -- inData.mode : sim.jointmode_kinematic or sim.jointmode_dynamic
    --
    -- inData.handle : the handle of the joint to control
    -- inData.revolute : whether the joint is revolute or prismatic
    -- inData.cyclic : whether the joint is cyclic or not
    -- inData.lowLimit : the lower limit of the joint (if the joint is not cyclic)
    -- inData.highLimit : the higher limit of the joint (if the joint is not cyclic)
    -- inData.dt : the step size used for the calculations
    -- inData.pos : the current position
    -- inData.vel : the current velocity
    -- 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.error : 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.rk4pass : if Runge-Kutta 4 solver is selected, will loop from 1 to 4 for each inData.passCnt
    -- 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.force : the joint force/torque, as set via sim.setJointTargetForce

    if inData.mode == sim.jointmode_dynamic then
        -- a simple position controller
        local ctrl = inData.error * 20
        local maxVelocity = ctrl
        if (maxVelocity > inData.maxVel) then maxVelocity = inData.maxVel end
        if (maxVelocity < -inData.maxVel) then maxVelocity = -inData.maxVel end
        local forceOrTorqueToApply = inData.maxForce
        local outData = {vel = maxVelocity, force = forceOrTorqueToApply}
        return outData
    end
    -- Expected return data:
    -- For kinematic joints:
    -- outData = {pos = pos, vel = vel, immobile = false}
    -- 
    -- For dynamic joints:
    -- outData = {force = f, vel = vel}
end

Cheers

[Cesarg21]

Thanks for the feedback.

I tried to implement this on the base (for a simple pendulum model):

Code: Select all

#python
import math

def sysCall_init():
    sim = require('sim')
    self.joint=sim.getObject('./Joint')
    # do some initialization here
def sysCall_joint(joint):
    print(type(joint))
    return {'force':1}

def sysCall_actuation():
    # put your actuation code here
    pass

def sysCall_sensing():
    # put your sensing code here
    pass

def sysCall_cleanup():
    # do some clean-up here
    pass

# See the user manual or the available code snippets for additional callback functions and details

And got two problems:

1) When I check the force in the prompt while the simulation is running with sim.getJointForce(sim.getObject('/Base/Joint')), the value does not get changed to 1.

2) Usually, I see that the input sysCall_joint() is inData (i.e., sysCall_joint(inData)). I modified things a little bit:

2.1) I tried to use "joint" (the name of the handle to see what would happen), got curious about its data type and to my surprise I got a dictionary. How exactly this variable works in this script? Isn't the output of a handle an integer? Oh, and when I directly print "joint", I obtain the same dictionary in that would I get if I use "inData". What variable is exactly "inData" calling by default?

2.2) And finally, If I have more than two joints, how I deal with the input? sysCall_joint(inData)? sysCall_joint(joint1, joint2,...)? What about the output? If I use return {'force':1} as in the code for example, which joint would it be modifying?

Sorry for the inconvenience.

[coppelia]

• You always need to return the maximum force/torque and a maximum velocity. With a velocity of e.g. 0, the joint won't exert any force/torque, even if it is non-zero
• the argument to the sysCall_joint callback function is the information about the joint that is currently being handled (remember, that all custom control joints in the given tree will be called in that callback function, unless there is another such callback function located further up the hierarchy tree). The joint being currently handled is identified by joint.handle. Its current pos, is joint.currentPos, its revolute state is joint.revolute, etc. Refer also to the other values in that joint table (or dict).
• If you have several joints in the current hierarchy tree that starts at the location of the script, and if those joints are also in custom control, they will all be handled by the same callback function as described above. If you need to differentiate the controller depending on the joint, simply add a switch on joint.handle

Cheers