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))