def __init__(self, name, sim, G, head='BC', nCranes=2, startPos=[12.5, -4], bundler=False, twigCrack=False):
print "head:", head, "cranes:", nCranes, "bundler:", bundler, "twigcracker:", twigCrack
self.driver=Operator(sim,preemptable=1) #add driver
sim.activate(self.driver, self.driver.work())
Machine.__init__(self, name, sim, G=G, driver=self.driver, mass=21000)
s=self.G.simParam
self.velocities={'machine': s['velocityOfMachine'],#1
'crane angular': s['angularVelocityOfCrane'],#0.35
'crane radial': s['radialVelocityOfCrane']}#2.5 #radians/sec,m/s
self.color='#CD0000'
self.moveEvent=SimEvent(name='machine moves', sim=self.sim) #signals BEFORE movement
self.movedEvent=SimEvent(name='machine moves', sim=self.sim) #signals AFTER movement
self.times={'crane const':s['moveCraneConst'],#1.5,
'move const':s['moveConst'],#5,
'switchFocus': s['switchFocusTime']}#3 #change before activating. May be changed from craneHead constructor
self.craneMaxL=s['maxCraneLength']#11
self.craneMinL=s['minCraneLength']#3
self.automaticMove=s['moveMachine']#False
self.hasBundler=bundler
self.prio=0
self.length=6.939
self.width=2.720
self.pos=startPos
self.trees=[]
self.mainRoadTrees=[]
self.corridorTrees=[]
self.heads={} #dictionary with keys 'left', 'right'
self.nCranes=nCranes
#Here is the bundler initiation
if bundler == True:
self.bundler=Bundler(sim=self.sim, driver=self.driver, machine=self)
self.sim.activate(self.bundler,self.bundler.run())
#Here is the head initiation
if head=='BC':
for i in range(nCranes):
h=BCHead(sim=self.sim, driver=self.driver, machine=self, twigCrack=twigCrack) #adds to above list.
elif head=='convAcc':
for i in range(nCranes):
h=ConventionalHeadAcc(sim=self.sim, driver=self.driver, machine=self, twigCrack=twigCrack) #adds to above list.
else:
raise Exception('ThinningMachine did not recognize head %s'%str(head))
for h in self.heads.values():
self.sim.activate(h, h.run())
self.treeMoni=Monitor(name='trees harvested')
self.treeMoni.observe(len(self.trees), self.sim.now())
self.roadList=[] #simply a list of roads.
self.roads={} #a dictionary, more sophisticated structure with simplifies finding the road for a spec. pos.
self.setUpCorridors()
class ThinningMachine(Machine, UsesDriver):
"""
A thinningmachine that thins a 40m long road
All measurements are taken from the komatsu 901.4 harvester with 620/55 x 30,5 wheels on the back and 650/45 x 22,5 wheels in the front.
"""
def __init__(self, name, sim, G, head='BC', nCranes=2, startPos=[12.5, -4], bundler=False, twigCrack=False):
print "head:", head, "cranes:", nCranes, "bundler:", bundler, "twigcracker:", twigCrack
self.driver=Operator(sim,preemptable=1) #add driver
sim.activate(self.driver, self.driver.work())
Machine.__init__(self, name, sim, G=G, driver=self.driver, mass=21000)
s=self.G.simParam
self.velocities={'machine': s['velocityOfMachine'],#1
'crane angular': s['angularVelocityOfCrane'],#0.35
'crane radial': s['radialVelocityOfCrane']}#2.5 #radians/sec,m/s
self.color='#CD0000'
self.moveEvent=SimEvent(name='machine moves', sim=self.sim) #signals BEFORE movement
self.movedEvent=SimEvent(name='machine moves', sim=self.sim) #signals AFTER movement
self.times={'crane const':s['moveCraneConst'],#1.5,
'move const':s['moveConst'],#5,
'switchFocus': s['switchFocusTime']}#3 #change before activating. May be changed from craneHead constructor
self.craneMaxL=s['maxCraneLength']#11
self.craneMinL=s['minCraneLength']#3
self.automaticMove=s['moveMachine']#False
self.hasBundler=bundler
self.prio=0
self.length=6.939
self.width=2.720
self.pos=startPos
self.trees=[]
self.mainRoadTrees=[]
self.corridorTrees=[]
self.heads={} #dictionary with keys 'left', 'right'
self.nCranes=nCranes
#Here is the bundler initiation
if bundler == True:
self.bundler=Bundler(sim=self.sim, driver=self.driver, machine=self)
self.sim.activate(self.bundler,self.bundler.run())
#Here is the head initiation
if head=='BC':
for i in range(nCranes):
h=BCHead(sim=self.sim, driver=self.driver, machine=self, twigCrack=twigCrack) #adds to above list.
elif head=='convAcc':
for i in range(nCranes):
h=ConventionalHeadAcc(sim=self.sim, driver=self.driver, machine=self, twigCrack=twigCrack) #adds to above list.
else:
raise Exception('ThinningMachine did not recognize head %s'%str(head))
for h in self.heads.values():
self.sim.activate(h, h.run())
self.treeMoni=Monitor(name='trees harvested')
self.treeMoni.observe(len(self.trees), self.sim.now())
self.roadList=[] #simply a list of roads.
self.roads={} #a dictionary, more sophisticated structure with simplifies finding the road for a spec. pos.
self.setUpCorridors()
def run(self):
"""
PEM of thinning machine
"""
#first, take the trees in the main road:
while True:
r=self.roads['main']
self.heads['left'].road=r #only one head in the two armed case because of balance. left is default 1a.
print "Mainroad assigned"
yield waituntil, self, self.headsDoneAtSite
#check if road is clear until the next position.
p=self.getRoadClearPos()
while p != self.getNextPos(): #if not clear, move some more and clear the road.
for c in self.setPos(p, cmnd=True): yield c
for a in self.releaseDriver(): yield a
self.movedEvent.signal()
self.heads['left'].road=r
yield waituntil, self, self.headsDoneAtSite
p=self.getRoadClearPos()
#p is self.getNextPos()
for c in self.setPos(p, cmnd=True): yield c
if self.getNextPos()==self.pos:
if self.hasBundler==True and self.bundler.currentBundle is not None:
self.bundler.forceBundler=True
for c in self.releaseDriver(): yield c
yield waituntil, self, self.bundlerDone
self.sim.stopSimulation()
yield hold, self, 0.001 #give it time to stop..
r=self.roads[self.pos[1]] #current roads.. should be like 10 of them..
for h in self.heads.values():
if len(r[h.side])>0: h.road=r[h.side][0]
for c in self.releaseDriver(): yield c
yield waituntil, self, self.headsDoneAtSite
def bundlerDone(self):
"""
Checks if the bundler is done
"""
if self.bundler.forceBundler==True: return False
else: return True
def headsDoneAtSite(self):
"""are the heads done?"""
for h in self.heads.values():
if h.road: return False #if road is assigned, head is still working.
return True
def setUpCorridors(self):
"""
Sets up
Note:
this is a VERY (indeed...=) inefficient way of doing it, using an iterative method.
However, it is sufficient for current use and can be improved if needed.
"""
#main road
print "sets up roads"
startX=self.pos[0]
tic=time.clock()
W=4.0 #Standard width of the strip road
cart=self.getCartesian
h1=self.heads['left']
origin=[startX, 0]
c1=cart([-W/2., 0],origin=origin, fromLocalCart=True)
c2=cart([-W/2., 40],origin=origin, fromLocalCart=True)
c3=cart([W/2., 40],origin=origin, fromLocalCart=True)
c4=cart([W/2., 0],origin=origin, fromLocalCart=True)
mainRoad=ThinningRoad([startX, 20], [c1,c2,c3,c4], G=self.G, direction=-pi/2., machine=self, main=True)
mainRoad.add()
self.roads['main']=mainRoad #each new point will get a new instance here.
#the key for each corridor set is first the y-pos of the vehicle, then the side, e.g. corridor[5.5]['left'] gives three corridors
P=[startX,5.5] #first stop by default
self.positions=[self.pos, P]
w=self.heads['left'].corridorWidth #meters
L=sqrt(self.craneMaxL**2-W**2)
nPerSide=self.heads['left'].corrPerSide
sigma=pi/nPerSide/3. #allowed deviation from optimal angle.
points=10 #integer no of points is the double
angMin=pi/4. #no "vertical" corridors
rad=sqrt(w**2/4+L**2/4) #radius of corridor.. saves time
while True:
self.roads[P[1]]={}
for side in ['left', 'right']:
cTemp=[]
mod=1
if side is 'right': mod=-1 #distinguishes between the two sides.
for ang in [mod*(angMin+(pi-2*angMin)/(nPerSide-1)*(i)) for i in range(nPerSide)]: #e.g. [pi/4., pi/2., 2/4.*pi]:
most=0
best=None
for sig in [x * sigma/(points*2) for x in range(-points, points)]:
#define the coordinates.
direction=pi/2.+ang+sig
sp=cart([0, abs(W/2./sin(direction-pi/2.))],origin=P, direction=direction, fromLocalCart=True) #startPos
w2=w/cos(-direction) #to compensate for the angular effect on the side.
if side is 'left':
w2=-w2
c1=[sp[0], sp[1]-w2/2.]
c4=[sp[0], sp[1]+w2/2.]
else:
c4=[sp[0], sp[1]-w2/2.]
c1=[sp[0], sp[1]+w2/2.]
c2=cart([-w/2., L],origin=P, direction=direction, fromLocalCart=True)
c3=cart([w/2., L],origin=P, direction=direction, fromLocalCart=True)
pos=cart([0, L/2.],origin=P, direction=direction, fromLocalCart=True)
c=ThinningRoad(pos, [c1,c2,c3,c4], G=self.G, direction=direction, machine=self, radius=rad) #radius not completely correct.. but speeds things up.
c.startPoint=sp
if len(c.trees)==0: c=None #no trees in c, no meaning to have..
else:
for t in c.trees:
if not h1.treeChopable(t):
c=None #don't use this road.. bad trees in the way.
break
if c and c.harvestTrees>most:
best=c
most=c.harvestTrees
if best:
best.add()
cTemp.append(best)
#print best.direction*180/pi
self.roads[P[1]][side]=cTemp
P=copy.copy(P) #there are references to the old P.
P[1]=P[1]+cos(angMin)*self.craneMaxL
if P[1]>self.G.terrain.ylim[1]: break
self.positions.append(P)
self.positions.append([self.positions[0][0], self.G.terrain.ylim[1]]) #the last spot.
print "time to set up roads: %f"%(time.clock()-tic)
def getNextPos(self):
"""
returns the next stop
"""
i=0
for p in self.positions:
if p[1]>self.pos[1]: return p
if len(self.positions)<i+2: return self.pos
i+=1
raise Exception('getNextPos is not expected to come this far.')
def getTreeDumpSpot(self, side):
"""
returns the position to dump the trees
"""
cart=self.getCartesian
W=2
L=self.length/3.
if side=='left':
return cart([-W,-L], fromLocalCart=True)
elif side=='right':
return cart([W, -L], fromLocalCart=True)
else: raise Exception('getTreeDumpSpot does not recognize side %s'%side)
def getNodes(self, pos=None):
"""
if position is not current position, it is expected that vechicle goes there from the current
position, which gives the angle.
"""
direction=self.direction
if not pos:
pos=self.pos
elif pos != self.pos:
[r,direction]=self.getCartesian(pos, direction=pi/2)
#get the nodes going!
W=1
L=3
c=self.getCartesian
nodes=[]
nodes.append(c([W/2., L/2.], origin=pos, direction=direction, fromLocalCart=True))
nodes.append(c([-W/2., L/2.], origin=pos, direction=direction, fromLocalCart=True))
nodes.append(c([-W/2., -L/2.], origin=pos, direction=direction, fromLocalCart=True))
nodes.append(c([W/2., -L/2.], origin=pos, direction=direction, fromLocalCart=True))
return nodes
def setPos(self, pos, cmnd=False):
time=super(ThinningMachine,self).setPos(pos)+self.times['move const']
for h in self.heads.values():
h.pos=h.getStartPos() #crane are always in this pos while moving
if self.hasBundler:
self.bundler.pos=self.bundler.getBPos() #sets the position of the bundler to before machine
if cmnd: return self.cmnd([],time, self.automaticMove)
else: return time
def getRoadClearPos(self):
"""used for the mainRoad. Check if the road is clear or if we have to take a small movement first and clear it."""
p=self.getNextPos()
closest=None
r=self.roads['main']
for t in r.trees:
if not closest or t.pos[1]<closest.pos[1]:
closest=t
if closest and p[1]+self.radius>=closest.pos[1]-closest.dbh:
pnew= [p[0], closest.pos[1]-closest.dbh-self.radius]
return pnew
else:
return p
def cmndWithDriver(self, commands, time):
"""
a method to set up the yield command, for use of the driver for a specific time.
overrides superclass method
priority not added here. If you want that, see how it's implemented in the same
method for the planting machine.
"""
if self.usesDriver: #don't need to reserve driver..
commands.extend([(hold, self, time)])
else:
commands.extend([(request, self, self.driver), (hold, self, time)])
self.usesDriver=True
switchTime=self.times['switchFocus']
if self.driver.isIdle(): #check for how long he's been idle
switchTime-=self.driver.idleTime()
if switchTime<0: switchTime=0
commands.extend([(hold, self, switchTime)]) #add time to switch focus
commands.extend([(hold, self, time)])
return commands
def draw(self,ax):
"""draws the machine at current poistion and with current direction.
All measurements are taken from the komatsu 901.4 harvester with 620/55 x 30,5 wheels on the back and 650/45 x 22,5 wheels in the front."""
cart=self.getCartesian
#draw roads:
for road in [r for r in self.roadList if (r.radius>self.G.terrain.ylim[1]/2. or r.harvestTrees==0)]+[h.road for h in self.heads.values() if h.road and h.road.harvestTrees != 0 and h.road.radius<self.G.terrain.ylim[1]/2.]:
road.draw(ax) #main roads have r>c+3. Current roads and visited roads
#wheels. first the four front wheels.
w=0.225
r=0.650
W=2.650
f=1.850+0.650+0.1
o1=cart([W/2.-w/2., f], fromLocalCart=True)
o2=cart([-(W/2.-w/2.), f], fromLocalCart=True)
o3=cart([-(W/2.-w/2.), 1.850-0.1-r], fromLocalCart=True)
o4=cart([W/2.-w/2., 1.850-0.1-r], fromLocalCart=True)
for origin in [o1,o2,o3,o4]:
c1=cart([w/2., r], origin=origin, fromLocalCart=True)
c2=cart([-w/2., r], origin=origin, fromLocalCart=True)
c3=cart([-w/2., -r], origin=origin, fromLocalCart=True)
c4=cart([w/2., -r], origin=origin, fromLocalCart=True)
p=mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k')
ax.add_patch(p)
#back wheels:
W=2.720
w=0.305
r=0.620
o1=cart([W/2.-w/2.,-1.650],fromLocalCart=True)
o2=cart([-W/2.+w/2.,-1.650],fromLocalCart=True)
for origin in [o1,o2]:
c1=cart([w/2., r], origin=origin, fromLocalCart=True)
c2=cart([-w/2., r], origin=origin, fromLocalCart=True)
c3=cart([-w/2., -r], origin=origin, fromLocalCart=True)
c4=cart([w/2., -r], origin=origin, fromLocalCart=True)
p=mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k')
ax.add_patch(p)
#base structure
L=6.939
W=2.720
a=1.850+2*0.650+0.1
f=1.850+0.650+0.1
c=[cart([(W-2*0.305)/2., f],fromLocalCart=True)]
c.append(cart([-(W-2*0.305)/2., f],fromLocalCart=True))
#fix a less wide end.
w=(W-2*0.305)/2.*0.70 #width at the back
#some coordinates are for the red parts.. others for the base structure
diff=0.1 #difference between red and black parts
r2=cart([-(W-2*0.305)/2.+diff, -1.650+r-0.05],fromLocalCart=True)
c2=cart([-(W-2*0.305)/2., -1.650],fromLocalCart=True)
r21=cart([-(W-2*0.305)/2.+diff, -1.650],fromLocalCart=True)
r5=cart([(W-2*0.305)/2.-diff, -1.650+r-0.05],fromLocalCart=True)
c5=cart([(W-2*0.305)/2., -1.650],fromLocalCart=True)
r41=cart([(W-2*0.305)/2.-diff, -1.650],fromLocalCart=True)
r3=cart([-w+diff, a-L+diff],fromLocalCart=True)
c3=cart([-w, a-L],fromLocalCart=True)
r4=cart([w-diff, a-L+diff],fromLocalCart=True)
c4=cart([w, a-L],fromLocalCart=True)
c.extend([c2,c3,c4,c5])
p=mpl.patches.Polygon(np.array(c), closed=True, facecolor='k', alpha=90)
ax.add_patch(p)
p=mpl.patches.Polygon(np.array([r2, r21,r3,r4, r41,r5]), closed=True, facecolor=self.color)
ax.add_patch(p)
if self.hasBundler: #draw the bundler
self.bundler.draw(ax)
for h in self.heads.values():#draw the cranes and heads
h.draw(ax)
#cabin:
l=1.8 #pure estimation for these three variables
w=W-2*r
f=0.4
smooth=1/5.*min(l,w)
origin=cart([0,f],fromLocalCart=True)
c2=cart([w/2., l/2.-smooth], origin=origin, fromLocalCart=True)
c3=cart([w/2.-smooth, l/2.], origin=origin, fromLocalCart=True)
c5=cart([-w/2.+smooth, l/2.], origin=origin, fromLocalCart=True)
c6=cart([-w/2., l/2.-smooth], origin=origin, fromLocalCart=True)
c8=cart([-w/2., -l/2.+smooth], origin=origin, fromLocalCart=True)
c9=cart([-w/2.+smooth, -l/2.], origin=origin, fromLocalCart=True)
c11=cart([w/2.-smooth, -l/2.], origin=origin, fromLocalCart=True)
c12=cart([w/2., -l/2.+smooth], origin=origin, fromLocalCart=True)
a=[c2,c3,c5,c6,c8,c9,c11,c12]
ax.add_patch(mpl.patches.Polygon(np.array(a), closed=True, facecolor=self.color))
class PlantMachine(Machine):
"""
The planting machine. A Volvo EC210C. Has one or two planting devices which in turn have two planting heads each.
This machine is not really intelligent, the sofisticated behavious is programmed in the planting devices. Machine does not move.
"""
def __init__(self, name, sim, G, mtype='1a2h', craneLim=None):
if not craneLim: craneLim=[4.0,9.0]
Machine.__init__(self, name, sim, G=G, mass=21000)
self.driver=Operator(sim=self.sim, delay=10000) #does not go on breaks..
self.sim.activate(self.driver, self.driver.work())
return None
self.type=mtype
if self.type=='1a2h' or self.type=='2a4h':
self.headType='Mplanter'
else:
self.headType='Bracke'
self.pos=[0,0]
self.craneMaxL=G.craneLim[1]
self.craneMinL=G.craneLim[0]
if self.craneMaxL <= self.craneMinL: raise Exception('crane maximum is smaller than crane minimum...')
self.pos[0]=random.uniform(G.terrain.xlim[0]+self.craneMaxL, G.terrain.xlim[1]- self.craneMaxL)
self.pos[1]=random.uniform(G.terrain.ylim[0]+self.craneMaxL, G.terrain.ylim[1]- self.craneMaxL)
if self.G.PMattach:
self.craneIntersect=self.G.PMattach
else:
self.craneIntersect=3.0
if self.craneMinL<=self.craneIntersect: self.craneMinL=self.craneIntersect-0.01 # of course..
self.visual={'upperStructLength':4.3, 'upperStructWidth':2.540 ,'trackWidth':0.6,'trackLength':4.460 }
po1=[self.visual['upperStructWidth']/2.+(2.990-2.540)/4., 0]
po2=[-self.visual['upperStructWidth']/2.-(2.990-2.540)/4., 0]
self.visual['trackCntrPosLoc']=[po1,po2] #just for plotting..
self.workingArea=pow(self.craneMaxL,2)*pi/2.-pow(self.craneMinL,2)*pi/2.
if self.G.PMstockingRate:
self.stockingRate=self.G.PMstockingRate
else:
self.stockingRate=2000 # plants/ha
self.plantMinDist=G.plantMinDist
self.dibbleDepth=0.1
self.nSeedlingsPWArea=floor(self.stockingRate/10000.*self.workingArea)
print "sPerWorkarea:", self.nSeedlingsPWArea, "cranemax:", self.craneMaxL, "cranemin:",self.craneMinL, "attach:", self.craneIntersect
#self.direction=random.uniform(0,2*pi)
self.direction=0
self.times={'diggTime': 3, 'heapTime': 2,'moundAndHeapTime': 5, 'dibbleDownTime': 1, 'relSeedlingTime': 1, 'dibbleUpTime':1, 'haltTime': 3, 'searchTime': 0, 'switchFocus':0}
self.timeConsumption={'diggTime': 0, 'heapTime': 0,'moundAndHeapTime': 0, 'dibbleDownTime': 0, 'relSeedlingTime': 0, 'haltTime': 0, 'searchTime': 0, 'switchFocus':0, 'machineMovement':0}
self.type=mtype
self.pDevs=[]
self.treesPlanted=[]
self.automatic=G.automatic
self.velocities={'machine': 0.5, 'angMachine':11.8*2*pi/60., 'angCranes': 15*pi/180, 'radial': 1.6} #radians/sec and m/s
self.timeConstants={'machine': 5, 'maincrane':1.5, 'subcrane':0.1} #time taken to initiate movements.
if self.G.PMradialCraneSpeed: self.velocities['radial']=self.G.PMradialCraneSpeed
self.angleLim=self.G.PMangleLim #angle limit between the two arms/planting devices.
self.treeMoni=Monitor(name="Trees planted")
self.leftAngleMoni=Monitor(name="angle for left crane")
self.rightAngleMoni=Monitor(name="angle for right crane")
self.treeMoni.observe(len(self.treesPlanted), self.sim.now())
p1=PlantingDevice('rightDevice', sim=self.sim, belongToMachine=self, G=self.G)
self.sim.activate(p1,p1.run())
self.pDevs.append(p1)
if mtype[0:2]=='2a': #add another one
self.times['switchFocus']=2
self.timeConstants['machine']*=1.5 #longer time for 2a
self.velocities['machine']*=0.75
p2=PlantingDevice('leftDevice', sim=self.sim, belongToMachine=self, G=self.G)
self.sim.activate(p2,p2.run())
self.pDevs.append(p2)
if self.exceedsAngleLim(self.pDevs[0], self.pDevs[0].pos, self.pDevs[1]):
#angle is too big, needs to adjust.
it=0
while self.exceedsAngleLim(self.pDevs[0], self.pDevs[0].pos, self.pDevs[1]):
newcyl=[self.pDevs[0].posCyl[0], self.pDevs[0].posCyl[1]+pi/50.]
print self.pDevs[0].posCyl, newcyl
newpos=getCartesian(newcyl, origin=self.pos, direction=self.direction)
if collide(self.pDevs[0], self.pDevs[1], o1pos=newpos) or it>20:
raise Exception('exceeds initially and cannot adjust.', it, newcyl)
self.pDevs[0].setPos(newpos)
self.mass+=4000. #the other planting head has a mass of 4 tons.
self.inPlaceEvent=SimEvent('machine has moved in place', sim=self.sim) #event fired when machine has moved
self.calcStumpsInWA() #updates some statistics
if self.headType=='Mplanter':
self.times['searchTime']=0.1*self.sim.stats['stumps in WA'] #specifics for this head..0 otherwise
elif self.headType=='Bracke': pass
else:
raise Exception('could not identify head type') #safety first..
def run(self): #the method has to be here in order to be counted as an entity
#get machine in place. Assumes that machine comes from last position in half-cricle pattern.
while True:
yield hold,self,1e5
distance=self.craneMaxL #not exact, half circle may overlap more or less than this.
time=self.timeConstants['machine']+distance/self.velocities['machine']
yield request, self, self.driver
yield hold, self, time
yield release, self, self.driver
self.timeConsumption['machineMovement']=time
self.inPlaceEvent.signal()
while True:
yield hold, self, 1
self.stopControl()
def exceedsAngleLim(self, p1in, pos1in,p2in, pos2in='undefined'):
"""move p1 to pos1, do we exceed the angular limits and do the cranes intersect?
i.e is it possible to place the intersection point of the two small cranes at a position so that the angle does not exceed the limit.
Also checks if point is too far for crane to reach."""
tic=time.clock()
if pos2in == 'undefined': pos2in=p2in.pos
if p1in.mountPoint == 'left' and p2in.mountPoint == 'right':
p1=p1in
pos1=pos1in
p2=p2in
pos2=pos2in
elif p1in.mountPoint == 'right' and p2in.mountPoint == 'left':
p1=p2in
pos1=pos2in
p2=p1in
pos2=pos1in
else:
raise Exception("ERROR, exceedsAngleLim only defined for 2a with names 'left' and 'right'")
#p1 is to the left, p2 to the right..
#some first checks: if we could classify as exceeds in this stage, a lot of time is saved.
D=getDistance(pos1, pos2)
p1cyl=self.getCylindrical(pos1)
p2cyl=self.getCylindrical(pos2)
if p2cyl[1] <=p1cyl[1]: alpha=abs(self.angleLim[1]) #no crossing.
else: alpha=abs(self.angleLim[0]) #cranes cross!
if abs(p1cyl[1]-p2cyl[1])>alpha:
PlantingDevice.timesProf[3]+=time.clock()-tic
return True #there is no way that the angle that counts can be smaller than this angle. Think about it.
if alpha<pi/2. and alpha>0.4636: #pretty long derivation, number comes from arctan(0.5)...
rmax=max(p1cyl[0], p2cyl[0])
if D/2.>(rmax-self.craneIntersect)*tan(alpha):
PlantingDevice.timesProf[3]+=time.clock()-tic
return True #saves a lot of time, but does not cover all the cases
#algorithm: test angles from pi to 0.. brute force and time consuming, and solutions could be missed.
if collide(o1=p1in, o2=p2in, o1pos=pos1in, o2pos=pos2in):
PlantingDevice.timesProf[3]+=time.clock()-tic
return True #collides..
a=self.getAngles(pos1,pos2)
PlantingDevice.timesProf[3]+=time.clock()-tic
return a
def getAngles(self, pos1, pos2, optimize=False, dth=pi/200):
"""if optimize, this function returns the angles at the position of smallest angles. otherwise it checks if this position is possible.
function assumes that there is two cranes."""
th=0
lim=self.angleLim
ang=[0,0]
r=[0,0]
angmin=1000
angCrane=th
rI=self.craneIntersect
#determine angle max/min span:
direction=self.direction
[r1,th1]=self.getCylindrical(pos1)
[r2,th2]=self.getCylindrical(pos2)
#this gives us some marginal. More analysis can be performed to optimize, analytical solution might even exist.
thmin=min(th1, th2)
thmax=max(th1, th2)
if lim[0]<0:
thmin-=pi/7.
thmax+=pi/7.
span=thmax-thmin
thMiddle=min(th1,th2)+(max(th1,th2)-min(th1,th2))/2.
if thmin<-abs(lim[0]): thmin=-abs(lim[0])
if thmax>pi+abs(lim[0]): thmax=pi+abs(lim[0])
angles=[thMiddle]+list(np.arange(thMiddle-span/4., thMiddle+span/4., dth))+ \
list(np.arange(thmin, thMiddle-span/4. ,dth))+list(np.arange(thMiddle+span/4., thmax, dth))
for th in angles: #scan through the possible angles for the crane, look for angle exceeds.
spreadPoint=self.getCartesian([rI, th])
direction=self.direction+th-pi/2. #direction of boom
[r1,th1]=self.getCylindrical(pos1, origin=spreadPoint, direction=direction)
[r2,th2]=self.getCylindrical(pos2, origin=spreadPoint, direction=direction)
th1=th1-pi/2.
th2=pi/2.-th2
if lim[0]<th1<lim[1] and lim[0]<th2<lim[1] and rI+r1<self.craneMaxL and rI+r2<self.craneMaxL:
#angles agree and cranes are not too extended. If craneIntersect+remaineing crane > craneMax, too extended.
if optimize:
angle=abs(th1)+abs(th2)
if angle<angmin:
angmin=angle
ang=[th1,th2]
r=[r1,r2]
angCrane=th
else:
return False
elif th1>lim[1] and th2>lim[1] and not optimize: #positive angle means point is in between. If this point exist, a solution can impossibly be found.
return True
if optimize:
if angmin is not 1000:
return ang+[angCrane]+r
else:
#this happens very rarely.. but ..
if dth < pi/4000.: #to avoid infinite loop.. below return has "spun" several times..
raise Exception("ERROR, getAngles: the current position is not allowed, exceeds angle limits %f, %f............%f,%f"%(pos1[0], pos1[1], pos2[0], pos2[1]))
return self.getAngles(pos1, pos2, optimize=optimize, dth=dth/5.)
else:
return True #angles exceed..
#assume that p1 is left and p2 right.
def stopControl(self):
"""
Checks if simulations should be stopped
"""
reason=None
if self.driver.resting:
return False #wait until he's not at rest, should not result in an infinite loop.
elif len(self.treesPlanted)>=floor(self.stockingRate/10000.0*self.workingArea):
#last is to ensure that machine is above productivity maximum
reason="the desired no. of trees are planted"
elif len(self.pDevs)==1 and self.pDevs[0].noMoreSpots:
reason="pDev out of spots."
else:
reason="plantDevs are passive and so are all the plantheads."
for p in self.pDevs:
if not p.passive():
return False
else:
for pH in p.plantHeads:
if pH.cause is not None: return False #machine is waiting for
print "Stop Reason:", reason
print self.sim.stats['mound attempts'],len( self.treesPlanted)
self.sim.stopSimulation()
def isWithinPlantingBorders(self, pos, c='cartesian'):
#plantingarea is approximated as circle.
w=self.pDevs[0].plantAreaW #2.5 def
L=self.pDevs[0].plantAreaL # 1 def
l=self.pDevs[0].plantHeads[0].length
#transform to cylindrical if necessary
if c=='cartesian' or c=='Cartesian':
cyl=self.getCylindrical(pos)
elif c=='cylindrical' or c=='Cylindrical':
cyl=pos
else:
raise Exception("error, isWithingPlantingBorders did not recognize %s"%c)
r=cyl[0]
th=cyl[1]
#is radius too small or too big?
if r<self.craneMinL+L-l/2 or r > self.craneMaxL-l/2.:
return False
if self.headType=='Mplanter': thInner=asin((w/2.)/r)
else: thInner=0
#is angle too small or too big?
if th>pi-thInner or th<thInner:
return False
return True
def calcStumpsInWA(self):
"""
required statistics. Roots are included
"""
#make a polygon that approximates out half circle
cart=getCartesian
cyl=getCylindrical
polynodes=[]
Lmin=self.craneMinL
Lmax=self.craneMaxL
points=15 #how many point in a half circle arc
thlim=[0,pi]
for th in np.linspace(thlim[0], thlim[1],points):
polynodes.append(cart([Lmax,th], direction=self.direction, origin=self.pos))
for th in np.linspace(thlim[1], thlim[0],points):
polynodes.append(cart([Lmin,th], direction=self.direction, origin=self.pos))
pol=Polygon(self.pos, polynodes)
potentialStumps=self.G.terrain.GetStumps(self.pos, self.craneMaxL)
sno=0
sdiam=0
for s in potentialStumps:
if collide(s,pol):
sno+=1
sdiam+=s.dbh
self.sim.stats['stumps in WA']=sno
self.sim.stats['stumps in WA sum diamater']=sdiam
def draw(self, ax):
cart=self.getCartesian
v=self.visual
#plot the machine tracks.
p1=v['trackCntrPosLoc'][0]
p2=v['trackCntrPosLoc'][1]
for p in [p1,p2]:
verts=[]
codes=[]
c1=cart([p[0]+v['trackWidth']/2., p[1]+v['trackLength']/2.],origin=self.pos, direction=self.direction, local=False, fromLocalCart=True)
c2=cart([p[0]-v['trackWidth']/2., p[1]+v['trackLength']/2.],origin=self.pos, direction=self.direction, local=False, fromLocalCart=True)
c3=cart([p[0]-v['trackWidth']/2., p[1]-v['trackLength']/2.],origin=self.pos, direction=self.direction, local=False, fromLocalCart=True)
c4=cart([p[0]+v['trackWidth']/2., p[1]-v['trackLength']/2.],origin=self.pos, direction=self.direction, local=False, fromLocalCart=True)
ax.add_patch(mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k'))
#plot the machine
if len(self.pDevs)>1:
a=self.getAngles(self.pDevs[1].pos, self.pDevs[0].pos, optimize=True) #angles not allowed may accur during movements.
craneDir=self.direction-pi/2.+a[2]
else:
a=[0,0,self.direction] #first two are not relevant for 1a
craneDir=self.direction+self.pDevs[0].posCyl[1]-pi/2.
D=2.850 #yposition of back
front=v['upperStructLength']-D #y-position of front
c1=cart([v['upperStructWidth']/2., front],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c2=cart([-v['upperStructWidth']/2., front],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c3=cart([-v['upperStructWidth']/2., -D],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c4=cart([v['upperStructWidth']/2., -D],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
ax.add_patch(mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='y'))
#plot cranes
alpha=1
if len(self.pDevs)>1:
#first, plot the first part of the crane:
L=self.craneIntersect
W=0.3 #3dm
c1=cart([W/2., 0],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c2=cart([W/2., L],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c3=cart([-W/2., L],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c4=cart([-W/2., 0],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
ax.add_patch(mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k', alpha=alpha))
#add the "bar" at the point of separation, let the separation be 0.7m
origin=cart([0, L],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
L=W
W2=0.7
c1=cart([W2/2., -L/2.],origin=origin, direction=craneDir, local=False, fromLocalCart=True)
c2=cart([W2/2., L/2.],origin=origin, direction=craneDir, local=False, fromLocalCart=True)
c3=cart([-W2/2., L/2.],origin=origin, direction=craneDir, local=False, fromLocalCart=True)
c4=cart([-W2/2., -L/2.],origin=origin, direction=craneDir, local=False, fromLocalCart=True)
ax.add_patch(mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k', alpha=alpha))
#add the two outer cranes:
W=W/2.
orig1=cart([W2/2., 0],origin=origin, direction=craneDir, local=False, fromLocalCart=True) #left
orig2=cart([-W2/2., 0],origin=origin, direction=craneDir, local=False, fromLocalCart=True) #right
pos1=self.pDevs[0].pos #left
pos2=self.pDevs[1].pos #right
for p in [(orig1, pos1), (orig2, pos2)]:
L=getDistance(p[0],p[1])
[r,th]=self.getCylindrical(p[1], origin=p[0], direction=craneDir)
#redefine cartesian, use direction th.
direction=craneDir-pi/2.+th
c1=cart([W/2., 0],origin=p[0], direction=direction, local=False, fromLocalCart=True)
c2=cart([W/2., L],origin=p[0], direction=direction, local=False, fromLocalCart=True)
c3=cart([-W/2., L],origin=p[0], direction=direction, local=False, fromLocalCart=True)
c4=cart([-W/2., 0],origin=p[0], direction=direction, local=False, fromLocalCart=True)
ax.add_patch(mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k', alpha=alpha))
else:
L=self.pDevs[0].posCyl[0]
W=0.3 #3dm
c1=cart([W/2., 0],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c2=cart([W/2., L],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c3=cart([-W/2., L],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
c4=cart([-W/2., 0],origin=self.pos, direction=craneDir, local=False, fromLocalCart=True)
ax.add_patch(mpl.patches.Polygon(np.array([c1,c2,c3,c4]), closed=True, facecolor='k', alpha=alpha))
def __init__(self, name, sim, G, mtype='1a2h', craneLim=None):
if not craneLim: craneLim=[4.0,9.0]
Machine.__init__(self, name, sim, G=G, mass=21000)
self.driver=Operator(sim=self.sim, delay=10000) #does not go on breaks..
self.sim.activate(self.driver, self.driver.work())
return None
self.type=mtype
if self.type=='1a2h' or self.type=='2a4h':
self.headType='Mplanter'
else:
self.headType='Bracke'
self.pos=[0,0]
self.craneMaxL=G.craneLim[1]
self.craneMinL=G.craneLim[0]
if self.craneMaxL <= self.craneMinL: raise Exception('crane maximum is smaller than crane minimum...')
self.pos[0]=random.uniform(G.terrain.xlim[0]+self.craneMaxL, G.terrain.xlim[1]- self.craneMaxL)
self.pos[1]=random.uniform(G.terrain.ylim[0]+self.craneMaxL, G.terrain.ylim[1]- self.craneMaxL)
if self.G.PMattach:
self.craneIntersect=self.G.PMattach
else:
self.craneIntersect=3.0
if self.craneMinL<=self.craneIntersect: self.craneMinL=self.craneIntersect-0.01 # of course..
self.visual={'upperStructLength':4.3, 'upperStructWidth':2.540 ,'trackWidth':0.6,'trackLength':4.460 }
po1=[self.visual['upperStructWidth']/2.+(2.990-2.540)/4., 0]
po2=[-self.visual['upperStructWidth']/2.-(2.990-2.540)/4., 0]
self.visual['trackCntrPosLoc']=[po1,po2] #just for plotting..
self.workingArea=pow(self.craneMaxL,2)*pi/2.-pow(self.craneMinL,2)*pi/2.
if self.G.PMstockingRate:
self.stockingRate=self.G.PMstockingRate
else:
self.stockingRate=2000 # plants/ha
self.plantMinDist=G.plantMinDist
self.dibbleDepth=0.1
self.nSeedlingsPWArea=floor(self.stockingRate/10000.*self.workingArea)
print "sPerWorkarea:", self.nSeedlingsPWArea, "cranemax:", self.craneMaxL, "cranemin:",self.craneMinL, "attach:", self.craneIntersect
#self.direction=random.uniform(0,2*pi)
self.direction=0
self.times={'diggTime': 3, 'heapTime': 2,'moundAndHeapTime': 5, 'dibbleDownTime': 1, 'relSeedlingTime': 1, 'dibbleUpTime':1, 'haltTime': 3, 'searchTime': 0, 'switchFocus':0}
self.timeConsumption={'diggTime': 0, 'heapTime': 0,'moundAndHeapTime': 0, 'dibbleDownTime': 0, 'relSeedlingTime': 0, 'haltTime': 0, 'searchTime': 0, 'switchFocus':0, 'machineMovement':0}
self.type=mtype
self.pDevs=[]
self.treesPlanted=[]
self.automatic=G.automatic
self.velocities={'machine': 0.5, 'angMachine':11.8*2*pi/60., 'angCranes': 15*pi/180, 'radial': 1.6} #radians/sec and m/s
self.timeConstants={'machine': 5, 'maincrane':1.5, 'subcrane':0.1} #time taken to initiate movements.
if self.G.PMradialCraneSpeed: self.velocities['radial']=self.G.PMradialCraneSpeed
self.angleLim=self.G.PMangleLim #angle limit between the two arms/planting devices.
self.treeMoni=Monitor(name="Trees planted")
self.leftAngleMoni=Monitor(name="angle for left crane")
self.rightAngleMoni=Monitor(name="angle for right crane")
self.treeMoni.observe(len(self.treesPlanted), self.sim.now())
p1=PlantingDevice('rightDevice', sim=self.sim, belongToMachine=self, G=self.G)
self.sim.activate(p1,p1.run())
self.pDevs.append(p1)
if mtype[0:2]=='2a': #add another one
self.times['switchFocus']=2
self.timeConstants['machine']*=1.5 #longer time for 2a
self.velocities['machine']*=0.75
p2=PlantingDevice('leftDevice', sim=self.sim, belongToMachine=self, G=self.G)
self.sim.activate(p2,p2.run())
self.pDevs.append(p2)
if self.exceedsAngleLim(self.pDevs[0], self.pDevs[0].pos, self.pDevs[1]):
#angle is too big, needs to adjust.
it=0
while self.exceedsAngleLim(self.pDevs[0], self.pDevs[0].pos, self.pDevs[1]):
newcyl=[self.pDevs[0].posCyl[0], self.pDevs[0].posCyl[1]+pi/50.]
print self.pDevs[0].posCyl, newcyl
newpos=getCartesian(newcyl, origin=self.pos, direction=self.direction)
if collide(self.pDevs[0], self.pDevs[1], o1pos=newpos) or it>20:
raise Exception('exceeds initially and cannot adjust.', it, newcyl)
self.pDevs[0].setPos(newpos)
self.mass+=4000. #the other planting head has a mass of 4 tons.
self.inPlaceEvent=SimEvent('machine has moved in place', sim=self.sim) #event fired when machine has moved
self.calcStumpsInWA() #updates some statistics
if self.headType=='Mplanter':
self.times['searchTime']=0.1*self.sim.stats['stumps in WA'] #specifics for this head..0 otherwise
elif self.headType=='Bracke': pass
else:
raise Exception('could not identify head type') #safety first..
def __init__(self, name, sim, G, mtype='2h', craneLim=None):
if not mtype in self.allowedTypes:
raise Exception('Machine type %s no allowed'%str(mtype))
if not craneLim: craneLim=[4.0,9.0]
Machine.__init__(self, name, sim, G=G, mass=21000)
self.driver=Operator(sim=self.sim, delay=10000) #does not go on breaks..
self.sim.activate(self.driver, self.driver.work())
self.type=mtype
if '2h' in self.type:
self.headType='Mplanter'
elif '3h' in self.type or '4h' in self.type:
self.headType='MultiHead' #several heads, don't know how many yet.
else:
self.headType='Bracke'
self.times={'heapTime': 2,'dibbleDownTime': 1, 'relSeedlingTime': 1, 'dibbleUpTime':1, 'haltTime': 1, 'searchTime': 0, 'switchFocus':0, 'invertTime':G.simParam['tCWhenInvKO'], 'invert':None} #all in seconds, invert is determined later
self.timeConsumption={'heapTime': 0,'moundAndHeapTime': 0, 'dibbleDownTime': 0, 'relSeedlingTime': 0, 'haltTime': 0, 'searchTime': 0, 'switchFocus':0, 'machineMovement':0} #new function for digTime, see plant method.
self.pos=[0,0]
self.craneMaxL=G.craneLim[1]
self.craneMinL=G.craneLim[0]
if self.craneMaxL <= self.craneMinL: raise Exception('crane maximum is smaller than crane minimum...')
self.pos[0]=random.uniform(G.terrain.xlim[0]+self.craneMaxL, G.terrain.xlim[1]- self.craneMaxL)
self.pos[1]=random.uniform(G.terrain.ylim[0]+self.craneMaxL, G.terrain.ylim[1]- self.craneMaxL)
if self.G.PMattach:
self.craneIntersect=self.G.PMattach
else:
self.craneIntersect=3.0
if self.craneMinL<=self.craneIntersect: self.craneMinL=self.craneIntersect-0.01 # of course..
self.visual={'upperStructLength':4.3, 'upperStructWidth':2.540 ,'trackWidth':0.6,'trackLength':4.460 }
po1=[self.visual['upperStructWidth']/2.+(2.990-2.540)/4., 0]
po2=[-self.visual['upperStructWidth']/2.-(2.990-2.540)/4., 0]
self.visual['trackCntrPosLoc']=[po1,po2] #just for plotting..
self.workingArea=pow(self.craneMaxL,2)*pi/2.-pow(self.craneMinL,2)*pi/2.
self.stockingRate=self.G.simParam['TSR']
self.plantMinDist=G.simParam['dibbleDist']
self.dibbleDepth=0.1
self.moundingFailureProb=G.simParam['moundFailureProb']=0.05
self.inverting=G.simParam['inverting']
if self.inverting:
assert G.simParam['ExcavatorInverting']!=G.simParam['KOInverting'] #can't use both
if G.simParam['KOInverting']:
self.invertingMethod='KO'
self.invertFailureProb=G.simParam['invertKOFailureProb']
else:
self.invertingMethod='Excavator'
self.invertFailureProb=G.simParam['invertExcFailureProb']
if 'ObAv' in self.type:
self.rootDegreesOK=self.G.simParam['angRootObAv']
if self.G.simParam['wMB']==0.6:
self.immobilePercent=self.G.simParam['vertOccStoneObAv60']
elif self.G.simParam['wMB']==0.5:
self.immobilePercent=self.G.simParam['vertOccStoneObAv50']
else:
self.immobilePercent=self.G.simParam['vertOccStoneObAv40']
else:
self.rootDegreesOK=pi/180.0*G.simParam['angRoot']
self.immobilePercent=G.simParam['vertOccStone']
self.nSeedlingsPWArea=max(floor(self.stockingRate/10000.*self.workingArea),1)
print "sPerWorkarea:", self.nSeedlingsPWArea, "cranemax:", self.craneMaxL, "cranemin:",self.craneMinL, "attach:", self.craneIntersect
self.direction=random.uniform(0,2*pi)
self.pDevs=[]
self.treesPlanted=[]
self.automatic=G.automatic
self.velocities={'machine': 0.5, 'angMachine':11.8*2*pi/60., 'angCranes': 15*pi/180, 'radial': 1.6} #radians/sec and m/s
self.timeConstants={'machine': 5, 'maincrane':1.5, 'subcrane':0.1} #time taken to initiate movements.
if self.G.PMradialCraneSpeed: self.velocities['radial']=self.G.PMradialCraneSpeed
self.angleLim=self.G.PMangleLim #angle limit between the two arms/planting devices.
self.treeMoni=Monitor(name="Trees planted")
self.leftAngleMoni=Monitor(name="angle for left crane")
self.rightAngleMoni=Monitor(name="angle for right crane")
self.treeMoni.observe(len(self.treesPlanted), self.sim.now())
p1=PlantingDevice('rightDevice', sim=self.sim, belongToMachine=self, G=self.G)
self.sim.activate(p1,p1.run())
self.pDevs.append(p1)
if '2a' in self.type: #add another one. 1a is default
self.times['switchFocus']=2
self.timeConstants['machine']*=1.5 #longer time for 2a
self.velocities['machine']*=0.75 #heavier, takes more time to move. Part of BTE model
p2=PlantingDevice('leftDevice', sim=self.sim, belongToMachine=self, G=self.G)
self.sim.activate(p2,p2.run())
self.pDevs.append(p2)
if self.exceedsAngleLim(self.pDevs[0], self.pDevs[0].pos, self.pDevs[1]):
#angle is too big, needs to adjust.
it=0
while self.exceedsAngleLim(self.pDevs[0], self.pDevs[0].pos, self.pDevs[1]):
newcyl=[self.pDevs[0].posCyl[0], self.pDevs[0].posCyl[1]+pi/50.]
print self.pDevs[0].posCyl, newcyl
newpos=getCartesian(newcyl, origin=self.pos, direction=self.direction)
if collide(self.pDevs[0], self.pDevs[1], o1pos=newpos) or it>20:
raise Exception('exceeds initially and cannot adjust.', it, newcyl)
self.pDevs[0].setPos(newpos)
self.mass+=4000. #the other planting head has a mass of 4 tons.
self.inPlaceEvent=SimEvent('machine has moved in place', sim=self.sim) #event fired when machine has moved
self.calcVisibleObstInWA() #some statistics updates needed
if self.headType=='Mplanter' or self.headType=='MultiHead':
multiplier=self.G.simParam['multiplierFindMuSite']
self.times['searchTime']=multiplier*self.sim.stats['visible obstacles in WA'] #specifics for this head..0 otherwise
elif self.headType!='Bracke':
raise Exception('could not identify head type %s'%self.headType) #safety first..