Servo control of cylindrical triaxial test

Hi! I want to simulate the cylindrical triaxial test but can't achieve the target confining pressure. Can someone help me with this？
The MWE is presented as 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.1). define variables
young=550e6 # normal contact stiffness
compFricDegree = 1.8 # initial contact friction during the confining phase
finalFricDegree = 38 # contact friction during the deviatoric loading
poisson = 0.3 # shear-to-normal stiffness ratio
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.43 # 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
tszz = 50000
tsrr = 50000

# 1.2). create materials for sand spheres and plates
Sand = O.materials.append(FrictMat(young=young,poisson=poisson,frictionAngle=radians(compFricDegree),density=2650,label='spheres'))

# 1.3). 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'))

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

###############################################################
### 2. SAMPLE GENERATION ###
###############################################################
# 2.1). 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)

# 2.2). create facet wall around particle packing
facets = []
nw = 45
nh = 1
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)
for b in wall:
 O.bodies[b].state.blockedDOFs = 'xyzXYZ'
 O.bodies[b].state.vel = (0,0,0)

# 2.3). create bot facet plate
facets3 = []
nw=45
rCyl2 = (0.75*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)), 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)
bot_id = 0
for s in botcap:
 bot_id = s

# 2.4). create top facet plate
facets3 = []
nw=45
rCyl2 = (0.75*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)
for s in topcap:
 top_id = s

# 2.5). calculate porosity
V_sand = 0
num_sand = 0
for b in sand:
 r = O.bodies[b].shape.radius
 V_sand += 4/3*math.pi*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)

# apply velocity for loading plates
vel_ini_a = 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)

###############################################################
### 3. DEFINE GLOBAL ENGINES ###
###############################################################
#**********************************************************************#
O.engines=[
 ForceResetter(),
 InsertionSortCollider([
  Bo1_Sphere_Aabb(),
  Bo1_Facet_Aabb(),
 ]),
 InteractionLoop(
 [
  Ig2_Sphere_Sphere_ScGeom6D(),
  Ig2_Facet_Sphere_ScGeom6D()
 ],
 [
  Ip2_FrictMat_FrictMat_FrictPhys(),
 ],
 [
  Law2_ScGeom_FrictPhys_CundallStrack(),
 ],
 label="iloop"
 ),
 GlobalStiffnessTimeStepper(active=1,timeStepUpdateInterval=25,timestepSafetyCoefficient=0.8),
 NewtonIntegrator(gravity=(0,0,0),damping=0.4,label='newton'),
 PyRunner(command='N1_sampleGen()',iterPeriod=1000,label='N11'),
 PyRunner(command='N1_sampleStable()',iterPeriod=1000,label='N12',dead=True),
 PyRunner(command='N4_stopisotropicLoad()',iterPeriod=100,label='N41',dead=True),
 PyRunner(command='N4_isotropicLoad()',iterPeriod=1,label='N42',dead=True),
]
#**********************************************************************#

###############################################################
### 4. DEFINE FUNCTIONS ###
###############################################################
# 4.1.1). generate sample using gravity deposition
def N1_sampleGen():
 f4 = 0 # total confining force act on the flexible membrane toward center of circle
 wdz = O.bodies[top_id].state.pos[2] - O.bodies[bot_id].state.pos[2] # maximum height of sample after gravity deposition
 for f in facets:
  f_local = O.forces.f(f.id)
  n = f.shape.normal
  a = f.shape.area
  f4 += (n[0]*f_local[0]+n[1]*f_local[1]+n[2]*f_local[2])
 # calculate resulant force on 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
 wAz = math.pi*width*width/4 # area of loading plate
 wAr = math.pi*width*wdz # area of flexible membrane
 # calculate axial and radial stress
 wszz = -0.5*(f2-f1)/wAz/1000 # average axial stress in the Z direction (kPa)
 wsrr = f4/wAr/1000 # average axial stress in the centripetal direction (kPa)
 # fix cylindrical wall
 for b in wall:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,0)
 # check stop criterion
 global V_sand
 V = wdz*0.25*width*width*math.pi
 porosity = 1-V_sand/V
 print('wszz:', wszz, 'wsrr:', wsrr, 'porosity:', porosity, 'height:', wdz,'unbF:', unbalancedForce())
 if porosity <= 0.44:
  N11.dead = True
  N12.dead = False

# 4.1.2). stable sample
def N1_sampleStable():
 # fix cylindrical wall
 for b in wall:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,0)
 # fix bot and top wall
 for b in topcap:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,0)
 for b in botcap:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,0)
 print('unbF:', unbalancedForce())
 if unbalancedForce() <= 0.002:
  print( 'sample generation finished!')
  global height
  height = O.bodies[top_id].state.pos[2] - O.bodies[bot_id].state.pos[2] # maximum height of sample after gravity deposition
  global V_ini
  V_ini = width*width*height/4*math.pi
  global zvel, rvel
  zvel=0
  rvel=0
  global max_zvel,max_rvel
  max_zvel = 0.5*height
  max_rvel = 0.5*height
  global wb,wt,wc
  wb = [O.bodies[b] for b in botcap]
  wt = [O.bodies[b] for b in topcap]
  wc = [O.bodies[b] for b in wall]
  N12.dead = True
  N41.dead = False
  N42.dead = False

# 4.4.1). measure stress and strain
def measureStressStrain():
 # 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 resulant force on rigid walls
 f4 = 0 # total confining force act on the rigid wall toward center of circle
 r_cum = 0 # cumulative radius of flexbile membrane
 count = 0 # number of gridnodes
 for b in wall:
  x,y,z = O.bodies[b].state.pos
  dist = math.sqrt(x*x+y*y)
  n = Vector3(x/dist,y/dist,0)
  f_local = O.forces.f(b)
  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 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 # average radius of flexible membrane
 wAz = math.pi*r_avg*r_avg # area of loading plate
 wAr = math.pi*wdz*r_avg*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
 # calculate axial strain and volume strain
 global height, V_ini, width
 VV = wdz*r_avg*r_avg*math.pi
 dV = VV-V_ini
 ev = -dV/V_ini
 ea = -(wdz-height)/height
 # stress tensor
 lwStress = getStress(volume=VV)
 wslw = Vector3(lwStress[0][0],lwStress[1][1],lwStress[2][2])
 return wszz, wsrr, ev, ea, wdz, wAz, wAr, count, lwStress, wslw

# 4.4.2). isotropic loading (servo-controlled of vertical and lateral wall)
def N4_isotropicLoad():
 # calculate stress and strain
 wszz, wsrr, ev, ea, wdz, wAz, wAr, count, lwStress, wslw = measureStressStrain()
 # calculate velocity for loading plates
 global gz, tszz, max_zvel, zvel
 zvel = gz * (wszz - tszz)
 if zvel>0:
  zvel = min(max_zvel,abs(zvel))
 else:
  zvel = -min(max_zvel,abs(zvel))
 # calculate velocity for membrane grids
 global gr, tsrr, max_rvel, rvel
 rvel = gr * (wsrr - tsrr)
 if rvel>0:
  rvel = min(max_rvel,abs(rvel))
 else:
  rvel = -min(max_rvel,abs(rvel))
 # assign velocity for loading plates
 for b in topcap:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,zvel)
 for b in botcap:
  O.bodies[b].state.blockedDOFs = 'xyzXYZ'
  O.bodies[b].state.vel = (0,0,-zvel)
 # assign velocity for membrane grids
 for f in wall:
  x,y,z = O.bodies[b].state.pos
  dist = math.sqrt(x*x+y*y)
  n = Vector3(x/dist,y/dist,0)
  O.bodies[b].state.vel = rvel*n

# 4.4.3). requirement for stop isotropic loading
def N4_stopisotropicLoad():
 # calculate stress and strain
 wszz, wsrr, ev, ea, wdz, wAz, wAr, count, lwStress, wslw = measureStressStrain()
 # calculate relaxation factor
 global gz, gr
 gz = 0
 gr = 0
 intrsr = [i for b in wc for i in b.intrs()]
 idsr = set([(i.id1,i.id2) for i in intrsr])
 intrsr = [O.interactions[id1,id2] for id1,id2 in idsr]
 for i in intrsr:
  gr = gr + i.phys.kn
 intrst = [i for b in wt for i in b.intrs()]
 idst = set([(i.id1,i.id2) for i in intrst])
 intrst = [O.interactions[id1,id2] for id1,id2 in idst]
 for i in intrst:
  gz = gz + i.phys.kn
 intrsb = [i for b in wb for i in b.intrs()]
 idsb = set([(i.id1,i.id2) for i in intrsb])
 intrsb = [O.interactions[id1,id2] for id1,id2 in idsb]
 for i in intrsb:
  gz = gz + i.phys.kn
 gz1=gz
 gr1=gr
 if gr1 < 1.0:
  gr1=1.0
 if gz1 < 1.0:
  gz1=1
 gz = 0.5 * wAz / (gz1 * PWaveTimeStep())
 gr = 0.5 * wAr / (gr1 * PWaveTimeStep())
 # check stop requirement
 unb=unbalancedForce()
 global zvel, rvel
 print( 'Servoing: wszz= ',wszz,' wsrr= ',wsrr,' unbalanced force= ',unb, 'zvel=',zvel,'rvel=',rvel, 'wslw=', wslw)
 if abs(wszz-tszz)/tszz<=0.01 and abs(wsrr-tsrr)/tsrr<=0.01 and unbalancedForce()<=0.01:
  O.pause()