Strange servo using ServoPIDController

Hi! I am using ServoPIDController() to simulate the consolidation of the cylindrical triaxial test. But I encountered problems that the confining pressure was not able to achieve and a high unbalanced force ratio during the simulation.

The MWE is presented below:
# import essential modules
from __future__ import print_function
from yade import pack, ymport, qt, plot, geom
from yade.gridpfacet import *
import gts, os.path, locale, random, math
locale.setlocale(locale.LC_ALL, 'en_US.UTF-8')

###############################################################
### 1. DEFINE VARIABLES AND MATERIALS ###
###############################################################

# 1.a). define variables
key = 'Triaxial_Undrained' # file name to be saved
young=55e6 # normal contact stiffness
compFricDegree = 1.8 # initial contact friction during the confining phase
finalFricDegree = 43 # contact friction during the deviatoric loading
poisson = 0.3 # shear-to-normal stiffness ratio
isoStress = 110000 # confining stress
conStress = 100000 # confining stress for deviatoric loading stage
width = 1.4e-1 # sample width
height = 2.8e-1 # target sample height(after consolidation)
height_0 = 3.2e-1 # initial sample height
num_spheres=500 # number of spheres
R_p = 0.0084 # mean particle radius
rCoff = 10 # thickness of top and bot sphere cap (based on rParticle)
rParticle = 0.02e-1 # membrane grid seed size
alpha = 8
rate = 0.1 # loading rate (strain rate)
damp = 0.3 # damping coefficient
targetPorosity = 0.45 # target porosity
thresholdvalue = 0.05 # threshold unbalance force
final_rate = 0.1 # strain rate for deviator loading
thresholdstrain = 0.06 # threshold axial strain for terminate
enlargefactor = 1.00

# A.b). create materials for sand spheres and plates
Sand = O.materials.append(CohFrictMat(
young=young,poisson=poisson,frictionAngle=radians(compFricDegree), alphaKr=0.25,alphaKtw=0,etaRoll=0.005,etaTwist=0, normalCohesion=5e6,shearCohesion=5e6, momentRotationLaw=True,density=2650,label='spheres'
))

# A.c). create membrane materials

GridMat = O.materials.append(CohFrictMat( young=100e6,poisson=0.3,density=2650,frictionAngle=radians(0), alphaKr=0,alphaKtw=0,etaRoll=0,etaTwist=0, normalCohesion=1e9,shearCohesion=1e9, momentRotationLaw=True,label='gridNodeMat'
))
pFacetMat = O.materials.append(FrictMat( young=100e6,poisson=0.3,density=2650,frictionAngle=radians(0),label='pFacetMat'
))

# A.d). create TOP & BOT plate materials
frictMat = O.materials.append(FrictMat(
young=100e6,poisson=0.3,density=2650,frictionAngle=radians(0),label='frictMat'
))

###############################################################
### 3. DEFINE GLOBAL ENGINES ###
###############################################################

#**********************************************************************#
O.engines=[
 ForceResetter(),
 InsertionSortCollider([
  Bo1_Sphere_Aabb(),
  Bo1_Facet_Aabb(),
  Bo1_PFacet_Aabb(),
  Bo1_GridConnection_Aabb()
 ]),
 InteractionLoop(
 [
  Ig2_Sphere_Sphere_ScGeom6D(),
  Ig2_GridNode_GridNode_GridNodeGeom6D(),
  Ig2_GridConnection_GridConnection_GridCoGridCoGeom(),
  Ig2_Sphere_PFacet_ScGridCoGeom(),
  Ig2_Facet_Sphere_ScGeom6D()
 ],
 [
  Ip2_FrictMat_FrictMat_FrictPhys(),
  Ip2_CohFrictMat_CohFrictMat_CohFrictPhys(label="cohesiveIp")
 ],
 [
  Law2_ScGeom_FrictPhys_CundallStrack(),
  Law2_ScGeom6D_CohFrictPhys_CohesionMoment(useIncrementalForm=True,always_use_moment_law=False,label='cohesiveLaw'),
  Law2_GridCoGridCoGeom_FrictPhys_CundallStrack(),
  Law2_ScGridCoGeom_FrictPhys_CundallStrack(),
 ],
 label="iloop"
 ),
 GlobalStiffnessTimeStepper(active=1,timeStepUpdateInterval=25,timestepSafetyCoefficient=0.8),
 NewtonIntegrator(gravity=(0,0,0),damping=0.1,label='newton')
]
#**********************************************************************#
O.engines[2].lawDispatcher.functors[1].always_use_moment_law=False
O.engines[2].physDispatcher.functors[1].setCohesionOnNewContacts=False

###############################################################
### 2. IMPORT CT-BASED PACKING ###
###############################################################
# C.a). generate random dense sphere pack
pred = pack.inCylinder((0,0,0),(0,0,height_0),.5*width)
sp = pack.randomDensePack(pred,spheresInCell=num_spheres,radius=R_p,rRelFuzz=0.3, returnSpherePack=True,memoDbg=True,memoizeDb='/tmp/loosePackings11.sqlite')
sand=sp.toSimulation(color=(0,1,1),material=Sand)

# C.b). define different sections of sphere pack
bot = [O.bodies[s] for s in sand if O.bodies[s].state.pos[2]<rParticle*rCoff]
top = [O.bodies[s] for s in sand if O.bodies[s].state.pos[2]>height_0-rParticle*rCoff]
tot = [O.bodies[s] for s in sand if O.bodies[s].state.pos[2]<=height_0]

# C.c). detect the position of particles in top & bot layer
top_limit = 0
top_id = 0
for s in top:
 if s.state.pos[2]>=top_limit:
  top_limit = s.state.pos[2]
  top_id = s.id
bot_limit = height_0
bot_id = 0
for s in bot:
 if s.state.pos[2]<=bot_limit:
  bot_limit = s.state.pos[2]
  bot_id = s.id

# C.d). create facet wall around particle packing
facets = []
nw = 45
nh = 15
rCyl2 = 0.5*width / cos(pi/float(nw))
for r in range(nw):
 for h in range(nh):
  v1 = Vector3( rCyl2*cos(2*pi*(r+0)/float(nw)), rCyl2*sin(2*pi*(r+0)/float(nw)), height_0*(h+0)/float(nh) )
  v2 = Vector3( rCyl2*cos(2*pi*(r+1)/float(nw)), rCyl2*sin(2*pi*(r+1)/float(nw)), height_0*(h+0)/float(nh) )
  v3 = Vector3( rCyl2*cos(2*pi*(r+1)/float(nw)), rCyl2*sin(2*pi*(r+1)/float(nw)), height_0*(h+1)/float(nh) )
  v4 = Vector3( rCyl2*cos(2*pi*(r+0)/float(nw)), rCyl2*sin(2*pi*(r+0)/float(nw)), height_0*(h+1)/float(nh) )
  f1 = facet((v1,v2,v3),color=(0,0,1),material=frictMat)
  f2 = facet((v1,v3,v4),color=(0,0,1),material=frictMat)
  facets.extend((f1,f2))
wall = O.bodies.append(facets)

# C.e). define different sections of facet wall
for b in wall:
 O.bodies[b].state.blockedDOFs = 'xyzXYZ'
 O.bodies[b].state.vel = (0,0,0)

# C.f). create top & bot facet plate
facets3 = []
nw=45
rCyl2 = (.6*width+2*rParticle) / cos(pi/float(nw))
for r in range(nw):
 if r%2==0:
  v1 = Vector3( rCyl2*cos(2*pi*(r+0)/float(nw)), rCyl2*sin(2*pi*(r+0)/float(nw)), height_0 )
  v2 = Vector3( rCyl2*cos(2*pi*(r+1)/float(nw)), rCyl2*sin(2*pi*(r+1)/float(nw)), height_0 )
  v3 = Vector3( rCyl2*cos(2*pi*(r+2)/float(nw)), rCyl2*sin(2*pi*(r+2)/float(nw)), height_0 )
  v4 = Vector3( 0, 0, height_0 )
  f1 = facet((v1,v2,v4),color=(0,0,0),material=frictMat)
  f2 = facet((v2,v3,v4),color=(0,0,0),material=frictMat)
  facets3.extend((f1,f2))
topcap = O.bodies.append(facets3)
facets3 = []
for r in range(nw):
 if r%2==0:
  v1 = Vector3( rCyl2*cos(2*pi*(r+0)/float(nw)), rCyl2*sin(2*pi*(r+0)/float(nw)), 0 )
  v2 = Vector3( rCyl2*cos(2*pi*(r+1)/float(nw)), rCyl2*sin(2*pi*(r+1)/float(nw)), 0 )
  v3 = Vector3( rCyl2*cos(2*pi*(r+2)/float(nw)), rCyl2*sin(2*pi*(r+2)/float(nw)), 0 )
  v4 = Vector3( 0, 0, 0 )
  f1 = facet((v1,v2,v4),color=(0,0,0),material=frictMat)
  f2 = facet((v2,v3,v4),color=(0,0,0),material=frictMat)
  facets3.extend((f1,f2))
botcap = O.bodies.append(facets3)

# C.g). define top & bot wall id
for s in topcap:
 top_id = s
bot_id = 0
for s in botcap:
 bot_id = s

# D.h). calculate porosity
V_sand = 0
num_sand = 0
for b in sand:
 r = O.bodies[b].shape.radius
 V_sand += 1.3333333*3.1416*r*r*r
 num_sand +=1
porosity = 1-V_sand/(.25*width*width*3.1416*height_0)
print('v_sand= ',V_sand,' number of sand: ',num_sand,'porosity is: ',porosity)
O.pause()
###############################################################
### 4. DEFINE FUNCTIONS ###
###############################################################

####################################################################### # D. DEFINING ADD-ON FUNCTIONS # #######################################################################

# D.c). a function for recording data
def checkrecord():
 plot.saveDataTxt('results_'+key)

def compress():
 for b in wall:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,0)

####################################################################### # E. APPLYING CONFINING STRESS TO FLEXIBLE MEMBRANE # #######################################################################
# E.a). adding corrosponding python function
O.engines=O.engines+[
 PyRunner(command='compress()',iterPeriod=1)
]

# E.b). compress until target porosity
vel_ini_a = rate*height_0
vel_ini_r = rate*height_0
for b in topcap:
 O.bodies[b].state.blockedDOFs = 'xyzXYZ'
 O.bodies[b].state.vel = (0,0,-vel_ini_a)
for b in botcap:
 O.bodies[b].state.blockedDOFs = 'xyzXYZ'
 O.bodies[b].state.vel = (0,0,vel_ini_a)
while 1:
 O.run(100,True)
 h = (O.bodies[top_id].state.pos[2]-O.bodies[bot_id].state.pos[2])
 V = h*0.25*width*width*3.1416
 porosity = 1-V_sand/V
 print('porosity: ',porosity, ' height: ', h)
 if (porosity <= targetPorosity):
  print( 'compression stage finished!')
  break
h_ini = h # record height after compression

# E.c). ADD CONFINING STRESS
# E.c.1). remove facet wall
for b in wall:
 O.bodies.erase(b)

# E.c.2). create membrane around particle packing by reading mesh file
def membraneCylinderGenerator(width, height, nw, nh, rNode, posC, materialNodes, materialPfacets):
 '''
 width: diameter of cylinder
 height: height of cylinder
 nw: number of facets along perimeter
 nh: number of facets along height
 rNode: radius of grid node
 posC: center of cylindrical bottom surface-->Vector3(x,y,z)
 materialNodes: material of grid nodes
 materialPfacets: material of PFacets
 '''
 nodesIds = [] # ID list of gridnodes
 cylIds = [] # ID list of cylinders
 pfIds = [] # ID list of pfacets
 colornode = [255/255,102/255,0/255]
 colorcylinder = [0,0,0]
 colorpfacet1 = [248/255,248/255,168/255]
 colorpfacet2 = [156/255,160/255,98/255]
 # create gridnodes
 rCyl2 = 0.5*width / cos(pi/float(nw))
 for r in range(nw):
   for h in range(nh+1):
       v = Vector3(rCyl2*cos(2*pi*r/float(nw)),rCyl2*sin(2*pi*r/float(nw)), height*h/float(nh))+posC
       V = (O.bodies.append(gridNode(v,rNode,wire=False,fixed=False, material=materialNodes,color=colornode)) )
       nodesIds.append(V)
 # create grid connection
 nodeIdsMin = min(nodesIds)
 for r in range(nw):
  for h in range(nh):
   if r == nw-1:
    V1 = nodeIdsMin+(r+0)*(nh+1)+h+0
    V2 = nodeIdsMin+(r+0)*(nh+1)+h+1
    V3 = nodeIdsMin+(0+0)*(nh+1)+h+1
    V4 = nodeIdsMin+(0+0)*(nh+1)+h+0
   else:
    V1 = nodeIdsMin+(r+0)*(nh+1)+h+0
    V2 = nodeIdsMin+(r+0)*(nh+1)+h+1
    V3 = nodeIdsMin+(r+1)*(nh+1)+h+1
    V4 = nodeIdsMin+(r+1)*(nh+1)+h+0
   #create grid connection
   cylIds.append(O.bodies.append(gridConnection(V1,V2,rNode,material=materialNodes,color=colorcylinder)))
   cylIds.append(O.bodies.append(gridConnection(V1,V4,rNode,material=materialNodes,color=colorcylinder)))
   cylIds.append(O.bodies.append(gridConnection(V1,V3,rNode,material=materialNodes,color=colorcylinder)))
   if h == nh-1:
    cylIds.append(O.bodies.append(gridConnection(V2,V3,rNode,material=materialNodes,color=colorcylinder)))
 # create PFacet
 for r in range(nw):
  for h in range(nh):
   if r == nw-1:
    V1 = nodeIdsMin+(r+0)*(nh+1)+h+0
    V2 = nodeIdsMin+(r+0)*(nh+1)+h+1
    V3 = nodeIdsMin+(0+0)*(nh+1)+h+1
    V4 = nodeIdsMin+(0+0)*(nh+1)+h+0
   else:
    V1 = nodeIdsMin+(r+0)*(nh+1)+h+0
    V2 = nodeIdsMin+(r+0)*(nh+1)+h+1
    V3 = nodeIdsMin+(r+1)*(nh+1)+h+1
    V4 = nodeIdsMin+(r+1)*(nh+1)+h+0
   #create Pfacets
   pfIds.append(O.bodies.append(pfacet(V1,V3,V2,wire=True,material=materialPfacets,color=colorpfacet1)))
   pfIds.append(O.bodies.append(pfacet(V1,V4,V3,wire=True,material=materialPfacets,color=colorpfacet2)))
 return nodesIds,cylIds,pfIds
# 4.2.2). generate flexible membrane
shiftfactor = O.bodies[bot_id].state.pos[2]-((height-h_ini)/2)
nodesIds,cylIds,pfIds = membraneCylinderGenerator(width=width, height=O.bodies[top_id].state.pos[2]-O.bodies[bot_id].state.pos[2], nw=80, nh=50, rNode=rParticle,
posC=Vector3(0,0,O.bodies[bot_id].state.pos[2]), materialNodes=GridMat, materialPfacets=pFacetMat)

# E.c.3). define different sections of membrane
membrane_grid = [O.bodies[s] for s in nodesIds ]
membrane_pfacet = [O.bodies[s] for s in pfIds]

# E.c.4). run one interaction
for f in membrane_grid:
 f.shape.color = Vector3(0,0,0)
 x = f.state.pos[0]
 y = f.state.pos[1]
 z = f.state.pos[2]
 dist = math.sqrt(x*x+y*y)
 n = Vector3(x/dist,y/dist,0)
 if z<=O.bodies[top_id].state.pos[2] and z>=O.bodies[bot_id].state.pos[2]:
  f.state.vel = -vel_ini_r*n*5
O.engines[2].physDispatcher.functors[1].setCohesionNow=True
O.engines[5].dead=True
while 1:
 O.run(1,True)
 break

# 4.2). redefine engine
#**********************************************************************#
# some initial parameters
'''

tsrr = 110000
wszz = 0
wsrr = 0
r_avg = 0
# velocity
zvel = 0.05*rate*height
rvel = 0.05*rate*height
max_vel = 0.5*height
gz = 0
gr = 0
rParticle = width/100
'''
tszz = 110000
tsrr = 110000
max_vel = 0.5*height
axialforce = tszz*width*width*math.pi/4

O.engines=[
 ForceResetter(),
 InsertionSortCollider(
 [
  Bo1_Sphere_Aabb(),
  Bo1_Facet_Aabb(),
  Bo1_PFacet_Aabb(),
  Bo1_GridConnection_Aabb()
 ]
 ),
 InteractionLoop(
 [
  Ig2_Sphere_Sphere_ScGeom6D(),
  Ig2_GridNode_GridNode_GridNodeGeom6D(),
  Ig2_GridConnection_GridConnection_GridCoGridCoGeom(),
  Ig2_Sphere_PFacet_ScGridCoGeom(),
  Ig2_Facet_Sphere_ScGeom6D(),
  Ig2_Sphere_GridConnection_ScGridCoGeom(),
  Ig2_GridConnection_PFacet_ScGeom(),
  Ig2_PFacet_PFacet_ScGeom()
 ],
 [
  Ip2_FrictMat_FrictMat_FrictPhys(),
  Ip2_CohFrictMat_CohFrictMat_CohFrictPhys(label="cohesiveIp")
 ],
 [
  Law2_ScGeom_FrictPhys_CundallStrack(),
  Law2_ScGeom6D_CohFrictPhys_CohesionMoment(useIncrementalForm=True,always_use_moment_law=False,label='cohesiveLaw'),
  Law2_GridCoGridCoGeom_FrictPhys_CundallStrack(),
  Law2_ScGridCoGeom_FrictPhys_CundallStrack(),
 ],
 label="iloop"
 ),
    NewtonIntegrator(gravity=(0,0,0),damping=0.7,label='newton'),
 ServoPIDController(axis=[0, 0, 1], maxVelocity=0.3*max_vel, iterPeriod=1, ids=topcap, target=axialforce, kP=2.0, kI=0.0, kD=0.0, label='top'),
 ServoPIDController(axis=[0, 0, -1], maxVelocity=0.3*max_vel, iterPeriod=1, ids=botcap, target=axialforce, kP=2.0, kI=0.0, kD=0.0, label='bot'),
 #GlobalStiffnessTimeStepper(active=1,timeStepUpdateInterval=25,timestepSafetyCoefficient=0.8),
 PyRunner(command='plotAddData()',iterPeriod=10),
]
# set final friction angle, enable cohesion
setContactFriction(radians(finalFricDegree))
O.engines[2].lawDispatcher.functors[1].always_use_moment_law=True
O.engines[2].physDispatcher.functors[1].setCohesionNow=True
O.engines[2].physDispatcher.functors[1].setCohesionOnNewContacts=False

def plotAddData():
# axial stress
 f4 = 0 # total confining force act on the flexible membrane toward center of circle
 r_cum = 0 # cumulative radius of flexbile membrane
 count = 0 # number of gridnodes
 for b in membrane_grid:
  x = b.state.pos[0]
  y = b.state.pos[1]
  z = b.state.pos[2]
  dist = math.sqrt(x*x+y*y)
  n = Vector3(x/dist,y/dist,0)
  if z<=O.bodies[top_id].state.pos[2] and z>=O.bodies[bot_id].state.pos[2]:
   f_local = O.forces.f(b.id) # resultant force on the gridnode
   f_normal = n[0]*f_local[0]+n[1]*f_local[1]+n[2]*f_local[2]
   f4 -= f_normal
   r_cum += dist
   count += 1
 # calculate resulant force on top and bottom loading plates
 f1 = sum(O.forces.f(b)[2] for b in topcap) # axial force act on the top loading plate
 f2 = sum(O.forces.f(b)[2] for b in botcap) # axial force act on the bottom loading plate
 # calculate height of sample and area of cylindrical walls
 wdz = O.bodies[top_id].state.pos[2] - O.bodies[bot_id].state.pos[2] # height of sample
 r_avg = r_cum/count-rParticle # average radius of flexible membrane
 wAz = math.pi*r_avg*r_avg # area of loading plate
 wAr = math.pi*r_avg*wdz*2 # area of flexible membrane
 # calculate axial and radial stress
 wszz = 0.5*(f2-f1)/wAz # average axial stress in the Z direction (kPa)
 wsrr = f4/wAr
 unbF = unbalancedForce()
 print( 'Servoing: wszz= ',wszz,' wsrr= ',wsrr,' unbalanced force: ',unbF)
 plot.addData(z=O.iter, swa=wszz, swc = wsrr, unbF = unbalancedForce())

def node2servo(node, area):
 global tsrr, max_vel
 x, y, z = node.state.pos
 f = tsrr * area
 dist = math.sqrt(x*x+y*y)
 return ServoPIDController(ids=[node.id], maxVelocity=max_vel, axis=Vector3(-x/dist, -y/dist, 0),iterPeriod=10,target=f,kP=5.0,kI=0.0,kD=0.0)

#nodes, nodesmiddle, nodestop, nodesbottom = findnodes(0, height)
nAPFacet = math.pi*width*height/(nw*nh)
servosnode = [node2servo(node, nAPFacet) for node in membrane_grid]
O.engines = O.engines + servosnode

qt.View()
r = qt.Renderer()
r.bgColor = 1, 1, 1

plot.plots = {'z': ('swa', 'swc'), 'z ': ('unbF',)}
plot.plot()
O.dt = 0.1*PWaveTimeStep()
O.run()