Module navigationTactics
[hide private]
[frames] | no frames]

Source Code for Module navigationTactics

  1  ################################################################################## 
  2  # Copyright (c) 2010, 2011, 2012, 2013, Daniel Urieli, Peter Stone 
  3  # University of Texas at Austin 
  4  # All right reserved 
  5  #  
  6  # Based On: 
  7  #  
  8  # Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam 
  9  # All rights reserved. 
 10  #  
 11  # Redistribution and use in source and binary forms, with or without 
 12  # modification, are permitted provided that the following conditions are met: 
 13  #  
 14  # 1. Redistributions of source code must retain the above copyright notice, this 
 15  # list of conditions and the following disclaimer. 
 16  #  
 17  # 2. Redistributions in binary form must reproduce the above copyright notice, 
 18  # this list of conditions and the following disclaimer in the documentation 
 19  # and/or other materials provided with the distribution. 
 20  #  
 21  # 3. Neither the name of the University of Amsterdam nor the names of its 
 22  # contributors may be used to endorse or promote products derived from this 
 23  # software without specific prior written permission. 
 24  #  
 25  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 26  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 27  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
 28  # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
 29  # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
 30  # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
 31  # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 32  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
 33  # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 34  # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
 35  ################################################################################## 
 36   
 37   
 38   
 39  from vec2d import * 
 40  from lineline import * 
 41  from math import * 
 42   
 43  import geometry 
 44  from actions import * 
 45  import state 
 46  import rrtTree 
 47  import random 
 48   
 49  #TODO: merge into AgentPIDTactic? no good reason to separate them 
50 -class AgentPIDTacticImpl:
51 """ 52 Implements a tactic in which an agent uses a PID controler 53 to reach its goal position. 54 """ 55
56 - def __init__(self, agentIndex, goalPos, rulesOfTheSea):
57 58 self.shipIndex = agentIndex 59 self.goalPos = goalPos 60 self.rulesOfTheSea = rulesOfTheSea 61 # TODO: send these constants as params 62 self.anglePID = 0.5 #1 #0.2 63 self.distPID = 0.1# 0.2 64 # Added integral term to the controller because of strong winds 65 self.distIConst = 0.00001#0.01 66 self.distIntergral = 0
67 68
69 - def composeAction(self, agentWorldModel):
70 """ 71 Implements the abstract method. 72 """ 73 # Rules of the sea - check for potential colisions with other ships 74 if self.rulesOfTheSea: 75 shipsToAvoid = self.getShipsOnACollisionPath(agentWorldModel) 76 if len(shipsToAvoid) > 0: 77 return self.composeAvoidingAction(shipsToAvoid, agentWorldModel) 78 else: 79 # If no collision danger, compose action as usual 80 return self.composePIDAction(agentWorldModel) 81 else: 82 # If no collision danger, compose action as usual 83 return self.composePIDAction(agentWorldModel)
84
85 - def composePIDAction(self, agentWorldModel):
86 """ 87 Composes a PID-controller-based action in order to reach the goal. 88 This function is by default used in unless there is a collision danger. 89 """ 90 estimatedState = agentWorldModel.getEstimatedExtShipState() 91 estimatedPos = (estimatedState.x, estimatedState.y) 92 93 distanceToGoal = geometry.distance2D(estimatedPos, self.goalPos) 94 angleFixToGoal = self.getAngleFixToGoal(estimatedState, self.goalPos) 95 96 97 # If at goal - stop 98 if distanceToGoal <= 10: 99 action = CompositeAction() 100 action.appendAction(PrimitiveAction("stop", [])) 101 return action 102 103 # accumulate data for the integral part of the PI controller 104 self.distIntergral += distanceToGoal 105 106 action = CompositeAction() 107 # Steering - (Proportinal controller) 108 if abs(angleFixToGoal) > 10: 109 requiredSteering = angleFixToGoal * self.anglePID 110 # TODO: agent should have a pointer to the ship so he could 111 # know the max speed rather than hand cropping it here to between [-90, 90] 112 steering = max(min(requiredSteering, 90), -90) 113 action.appendAction(PrimitiveAction("setSteering", [steering])) 114 115 116 # Speed - PI (no D) controller 117 requiredSpeed = distanceToGoal * self.distPID + self.distIConst * self.distIntergral 118 # TODO: agent should have a pointer to the ship so he could know the max speed and so on 119 speed = max(min(requiredSpeed, 40), -10) 120 121 122 action.appendAction(PrimitiveAction("setEngineSpeed", [speed])) 123 124 return action
125 126
127 - def getAngleFixToGoal(self, estimatedState, goal):
128 """ 129 According to the estimation, how much should we turn 130 to point at the goal 131 132 Keyword arguments: 133 estimatedState -- estimated ShipExternalState 134 135 returns: 136 The angle we should turn 137 """ 138 estimatedPos = (estimatedState.x, estimatedState.y) 139 desiredOrientation = geometry.orientation2D(estimatedPos, goal) 140 estimatedOrientation = estimatedState.orientation 141 return geometry.computeShortestTurn(estimatedOrientation, desiredOrientation)
142
143 - def getShipsOnACollisionPath(self, agentWorldModel):
144 """ 145 Compute a list of ships that are on path to potential collision 146 with the agent's ship. 147 148 @type agentWorldModel: AgentWorldModel 149 @param agentWorldModel: The belief state of the agent 150 151 @rtype: a list of ShipExternalState 152 @return: External states (position, orientation...) of ships that are on a path to potential collision with the agent's ship. 153 """ 154 # extract my position, orientation, speed 155 # print 'shipIndex', self.shipIndex 156 estimatedSelfState = agentWorldModel.getEstimatedExtShipState() 157 myPos = Vec2d(estimatedSelfState.x, estimatedSelfState.y) 158 # print 'myPos', myPos 159 myOrient = estimatedSelfState.orientation 160 # print 'myOrient', myOrient 161 mySpeed = estimatedSelfState.speed 162 # print 'mySpeed', mySpeed 163 164 # create my line of motion 165 myDirection = Vec2d(cos(radians(myOrient)), sin(radians(myOrient))) 166 # print 'myDirection', myDirection 167 myLineP1 = myPos - myDirection * 10000 168 myLineP2 = myPos + myDirection * 10000 169 170 171 # check potential collisions with any other ship 172 dangerShipsStates = [] 173 externalShipStates = agentWorldModel.getEstimatedStatesOfOtherShips(self.shipIndex) 174 for state in externalShipStates: 175 # extract other agent's position, orientation, speed 176 otherPos = Vec2d(state.x, state.y) 177 # print 'otherPos', otherPos 178 otherOrient = state.orientation 179 # print 'otherOrient', otherOrient 180 otherSpeed = state.speed 181 # print 'otherSpeed', otherSpeed 182 183 # create the other ship's line of motion 184 otherDirection = Vec2d(cos(radians(otherOrient)), sin(radians(otherOrient))) 185 # print 'otherDirection', otherDirection 186 otherLineP1 = otherPos - otherDirection * 10000 187 otherLineP2 = otherPos + otherDirection * 10000 188 189 190 191 # check if coming towards each other 192 globalAngleToOtherShip = (otherPos - myPos).get_angle() 193 relativeAngleToOtherShip = geometry.computeShortestTurn(myOrient, globalAngleToOtherShip) 194 # print 'relativeAngleToOtherShip', relativeAngleToOtherShip 195 if 45 > relativeAngleToOtherShip > -45: 196 relativeOrient = geometry.computeShortestTurn(myOrient, otherOrient) 197 # print 'relativeOrient', relativeOrient 198 if abs(relativeOrient) > 135: 199 distToOther = (otherPos - myPos).get_length() 200 # print 'distToOther', distToOther 201 sumSpeeds = mySpeed + otherSpeed 202 # print 'sumSpeeds', sumSpeeds 203 timeToCollision = distToOther / sumSpeeds if sumSpeeds > 0 else 999999 204 # print 'timeToCollision', timeToCollision 205 # TODO: tune threshold and merge with threshold from below 206 if timeToCollision < 60: 207 dangerShipsStates.append(state) 208 continue 209 210 211 212 # check if lines intersects in any way 213 intersection = lineline(myLineP1, myLineP2, otherLineP1, otherLineP2) 214 # print 'intersection', intersection 215 # if intersection is None then there's no intersection => nothing to do 216 if intersection: 217 myDistToIntersect= (intersection - myPos).get_length() 218 # print 'myDistToIntersect', myDistToIntersect 219 otherDistToIntersect = (intersection - otherPos).get_length() 220 # print 'otherDistToIntersect', otherDistToIntersect 221 # if intersection is ahead of us or not too far behind us 222 # TODO: parametrize tune threshold of -50 223 if myDistToIntersect > -50 and otherDistToIntersect > -50: 224 myTimeToIntersect = myDistToIntersect / mySpeed if mySpeed > 0 else 999999 225 # print 'myTimeToIntersect', myTimeToIntersect 226 otherTimeToIntersect = otherDistToIntersect / otherSpeed if otherSpeed > 0 else 999999 227 # print 'otherTimeToIntersect', otherTimeToIntersect 228 229 # TODO: tune thresholds 230 if myTimeToIntersect < 60 and otherTimeToIntersect < 60 and abs(myTimeToIntersect - otherTimeToIntersect) < 30: 231 # collision danger 232 dangerShipsStates.append(state) 233 234 # print 'dangerShipsStates', dangerShipsStates 235 return dangerShipsStates
236 237 238 239
240 - def composeAvoidingAction(self, shipsToAvoid, agentWorldModel):
241 """ 242 Compute an action to take when there is a collision danger. 243 244 @type shipsToAvoid: a list of ShipExternalState 245 @param shipsToAvoid: External states (position, orientation...) of ships that are on a path to potential collision with the agent's ship. 246 247 @type agentWorldModel: AgentWorldModel 248 @param agentWorldModel: The belief state of the agent 249 250 @rtype: CompositeAction 251 @return: Action to take in order to avoid collision 252 """ 253 # In general, if no ships on the right, turn right. Otherwise stop. 254 estimatedSelfState = agentWorldModel.getEstimatedExtShipState() 255 myPos = Vec2d(estimatedSelfState.x, estimatedSelfState.y) 256 myOrient = estimatedSelfState.orientation 257 258 turnRight = False 259 stop = False 260 for state in shipsToAvoid: 261 otherShipPos = Vec2d(state.x, state.y) 262 otherShipOrientation = state.orientation 263 globalAngleToOtherShip = (otherShipPos - myPos).get_angle() 264 relativeAngleToOtherShip = geometry.computeShortestTurn(myOrient, globalAngleToOtherShip) 265 relativeOrient = geometry.computeShortestTurn(myOrient, otherShipOrientation) 266 if abs(relativeAngleToOtherShip) < 45: 267 if abs(relativeOrient) > 135: 268 # facing each other, turn right 269 turnRight = True 270 elif abs(relativeOrient) < 45: 271 # behind the other, stop 272 stop = True 273 elif 45 < relativeAngleToOtherShip < 135: 274 # elif -135 < relativeAngleToOtherShip < -45: # TODO: as the 0,0 is top left (upside down screen) "left" and "right" are opposite, so if find solution to this problem, restore this line 275 # on my right, stop 276 stop = True 277 elif -135 < relativeAngleToOtherShip < -45: 278 # elif 45 < relativeAngleToOtherShip < 135: # TODO: as the 0,0 is top left (upside down screen) "left" and "right" are opposite, so if find solution to this problem, restore this line 279 turnRight = True 280 else: 281 # behind me, do nothing 282 pass 283 284 285 if stop: 286 # on my right, stop 287 action = CompositeAction() 288 # print 'STOPPING' 289 # TODO: instead of a fixed steering, we should have some kind 290 # of PID based on the distance to collision point 291 action.appendAction(PrimitiveAction("setEngineSpeed", [-10])) 292 return action 293 294 if turnRight: 295 # on my front or left, turn right 296 action = CompositeAction() 297 # TODO: instead of a fixed steering, we should have some kind 298 # of PID based on the distance to collision point 299 # print 'TURNING TO THE RIGHT' 300 action.appendAction(PrimitiveAction("setSteering", [30])) 301 return action 302 303 # if reached here, all ships are behind us, nothing special todo 304 return self.composePIDAction(agentWorldModel)
305 306 307 #TODO: merge into AgentRRTTactic? no good reason to separate them
308 -class AgentRRTTacticImpl:
309 """ 310 This class implements an RRT based partolling agent tactic. 311 Given a start position and a goal position, it runs a simplistic RRT 312 algorithm, and extracts the resulting path. 313 """
314 - def __init__(self, agentIndex, startPos, goalPos, obstacleList, rulesOfTheSea):
315 """ 316 @type agentIndex: int 317 @param agentIndex: the index of the patroling agent (like an agent-id) 318 319 @type startPos: 2-tuple 320 @param startPos: start position for the RRT computation 321 322 @type goalPos: 2-tuple 323 @param goalPos: goal position for the RRT computation 324 325 @type rulesOfTheSea: boolean 326 @param rulesOfTheSea: Tells whether to respect the rules of the sea 327 """ 328 329 self.agentIndex = agentIndex 330 self.startPos = state.ShipExternalState(startPos[0], startPos[1], 0, 0, 0) 331 self.goalPos = state.ShipExternalState(goalPos[0], goalPos[1], 0, 0, 0) 332 self.obstacleList = obstacleList 333 self.rulesOfTheSea = rulesOfTheSea 334 self.actionIndex = 0 335 336 337 338 339 # TODO: Some hard-coded constants that should probably be sent as params 340 341 # Threshold to determine whether at goal 342 self.angleThreshold = 10 343 self.distanceThreshold = 10 344 345 # borders for sampling 346 xStart, yStart = startPos 347 xEnd, yEnd = goalPos 348 # randint expects an int 349 self.minX = int(round(min(xStart, xEnd) - 200)) 350 self.maxX = int(round(max(xStart, xEnd) + 200)) 351 self.minY = int(round(min(yStart, yEnd) - 200)) 352 self.maxY = int(round(max(yStart, yEnd) + 200)) 353 self.minAng = 0 354 self.maxAng = 360 355 356 # step-fraction towards a new sampled state 357 self.epsilon = 0.1 358 359 360 361 # Compute the RRT path 362 # init a tree that is filled inside self.rrtPath() 363 #(TODO: change distanceUnit and angleUnit?) 364 self.tree = rrtTree.RRTTreeXYOrientationEuclidDist(self.startPos, distanceUnit=10, angleUnit=5) 365 self.path = self.rrtPath(self.startPos, self.goalPos)
366 367
368 - def getPath(self):
369 """ 370 Returns the path of points along which the agent patrols. 371 372 @rtype: list of 2-tuples 373 @return: a list of (x, y) points representing the patrol path 374 """ 375 return self.path
376
377 - def rrtPath(self, startPos, goalPos):
378 """ 379 Computes an RRT path from startPos to goalPos. 380 381 @type startPos: 2-tuple 382 @param startPos: start position for the RRT computation 383 384 @type goalPos: 2-tuple 385 @param goalPos: goal position for the RRT computation 386 387 @rtype: list of 2-tuples 388 @return: a list of (x, y) points representing the patrol path 389 """ 390 # create initial node 391 initialExternalState = startPos 392 393 394 # search 395 currentState = initialExternalState 396 while not self.isGoalState(currentState): 397 currentState = self.extend(self.tree, self.chooseRandomState()) 398 399 nodesPath = self.tree.getNodesPathTo(currentState) 400 path = [(n.state.x, n.state.y) for n in nodesPath] 401 return path
402 403
404 - def extend(self, tree, randomState):
405 """ 406 The EXTEND function from the paper. 407 Here, in this simple version of the algorithms, we just 408 take an epsilon step towards the randomState, without checking 409 the all intermediate states are collision-free. 410 We also assume holonomic problem in which the computation 411 of the edge (the action that takes us to the new state) 412 is just a simple vector calculation. 413 414 415 @type randomState: ShipExternalState 416 @param randomState: A state that was randomly sampled in space, towards which we take an epsilon step. 417 418 @rtype: ShipExternalState 419 @return: The new state that was added to the tree. 420 """ 421 closestNode = tree.findClosestNode(randomState) 422 closestState = closestNode.state 423 424 # if intersects some obstacle - reject point 425 for obst in self.obstacleList: 426 A = Vec2d(closestState.x, closestState.y) 427 B = Vec2d(randomState.x, randomState.y) 428 C, D = obst.getBorder() 429 C = Vec2d(C) 430 D = Vec2d(D) 431 432 if lineline(A, B, C, D): 433 #intersects! 434 return None 435 436 437 # Take an epsilon step towards randomState 438 # TODO: need to normalize attributes with variance 439 closestStateX = closestState.x 440 closestStateY = closestState.y 441 closestStateOrient = closestState.orientation 442 newX = (randomState.x - closestStateX) * self.epsilon + closestStateX 443 newY = (randomState.y - closestStateY) * self.epsilon + closestStateY 444 newOrient = (randomState.orientation - closestStateOrient) * self.epsilon + closestStateOrient 445 446 # Compute the edge - the action to take in order to get to this state 447 # TODO: check that the path to this state is legal 448 xDiff = newX - closestStateX 449 yDiff = newY - closestStateY 450 451 if closestNode.parent != None: 452 oldOrientation = degrees( atan2(closestStateY - closestNode.parent.state.y, 453 closestStateX - closestNode.parent.state.x) ) 454 else: 455 oldOrientation = closestStateOrient 456 # Turn from the old direction to the new direction 457 steeringToApply = degrees( atan2(yDiff, xDiff) ) - oldOrientation 458 459 speedToApply = sqrt( xDiff * xDiff + yDiff * yDiff ) 460 action = CompositeAction() 461 action.appendAction(PrimitiveAction("setEngineSpeed", [speedToApply])) 462 action.appendAction(PrimitiveAction("setSteering", [steeringToApply])) 463 464 # Add the node to the tree 465 newState = state.ShipExternalState(newX, newY, newOrient, speedToApply, 0) 466 parent = closestNode 467 edge = action 468 newNode = rrtTree.RRTNode(newState, parent, edge) 469 tree.addNode(newNode) 470 471 return newState
472
473 - def isGoalState(self,shipState):
474 "Is the ship within some threshold from the goal." 475 476 if not shipState: 477 return False 478 479 point1 = (shipState.x, shipState.y) 480 point2 = (self.goalPos.x, self.goalPos.y) 481 distance = geometry.distance2D(point1, point2) 482 if distance > self.distanceThreshold: 483 return False 484 485 # # ignore angle for now 486 # angle1 = shipState.orientation 487 # angle2 = self.goalPos.orientation 488 # if abs(computeShortestTurn(angle2,angle1)) > self.angleThreshold: 489 # return False 490 491 return True
492 493
494 - def chooseRandomState(self):
495 "Samples a random state with a bias towards the goal." 496 if random.random() < 0.05: 497 return self.goalPos 498 else: 499 return state.ShipExternalState(random.randint(self.minX, self.maxX), 500 random.randint(self.minY, self.maxY), 501 random.randint(self.minAng, self.maxAng), 502 speed = -1, angularSpeed = -1) # speed should be ignored
503 504 505 506 507 ########################################################################## 508 # An abstract class for low-level navigation technique. 509 # While the AgentStrategy class encapsulates the agent's 510 # complete strategy, the NavigateToPointTactic class 511 # only cares about how to reach the next goal point. 512 ########################################################################## 513 514 538 539 540 #class AgentNullTactic(NavigateToPointTactic): 541 # """ 542 # Do nothing, always return done() == True. Used for initialization. 543 # """ 544 # def composeAction(self, agentWorldModel): 545 # "Implements abstract method." 546 # # should have no action implemented 547 # abstract 548 # 549 # def done(self, agentWorldModel): 550 # "Implements abstract method." 551 # return True 552 553
554 -class AgentPIDTactic(NavigateToPointTactic):
555 """ 556 Low level tactic for PID based navigation to a point 557 """
558 - def __init__(self, agentIndex, goalPoint, rulesOfTheSea):
559 # TODO: eliminate self.worker and merge AgentPIDTacticImpl 560 # to here? 561 self.goalPoint = goalPoint 562 self.worker = AgentPIDTacticImpl(agentIndex, goalPoint, rulesOfTheSea)
563
564 - def composeAction(self, agentWorldModel):
565 "Implements abstract method." 566 return self.worker.composeAction(agentWorldModel)
567
568 - def done(self, agentWorldModel):
569 "Implements abstract method." 570 estimatedState = agentWorldModel.getEstimatedExtShipState() 571 estimatedPos = (estimatedState.x, estimatedState.y) 572 return self.atPoint(estimatedPos, self.goalPoint)
573
574 - def getGoalPos(self):
575 "Implements abstract method." 576 return self.goalPoint
577
578 -class AgentRRTTactic(NavigateToPointTactic):
579 """ 580 Low level tactic for RRT based navigation to a point 581 """
582 - def __init__(self, agentIndex, 583 estimatedPos, 584 goalPoint, 585 estimatedObstaclesList, 586 rulesOfTheSea):
587 588 self.agentIndex = agentIndex 589 self.rrtBuilder = AgentRRTTacticImpl(agentIndex, 590 estimatedPos, 591 goalPoint, 592 estimatedObstaclesList, 593 rulesOfTheSea) 594 self.rrtPath = self.rrtBuilder.path 595 # the two atPoint() tests should be consistent 596 # and therefore we set the goal point to 597 # the last point in the rrt rather than the real goal 598 self.goalPoint = self.rrtPath[-1]# goalPoint 599 self.nextRRTPointIndex = 0 600 nextPoint = self.rrtPath[self.nextRRTPointIndex] 601 self.pidTactic = AgentPIDTactic(agentIndex, nextPoint, rulesOfTheSea) 602 self.rulesOfTheSea = rulesOfTheSea
603
604 - def composeAction(self, agentWorldModel):
605 "Implements abstract method." 606 estimatedState = agentWorldModel.getEstimatedExtShipState() 607 estimatedPos = (estimatedState.x, estimatedState.y) 608 nextPoint = self.rrtPath[self.nextRRTPointIndex] 609 if self.atPoint(estimatedPos, nextPoint): 610 # set next point as target 611 self.nextRRTPointIndex += 1 612 nextPoint = self.rrtPath[self.nextRRTPointIndex] 613 self.pidTactic = AgentPIDTactic(self.agentIndex, 614 nextPoint, 615 self.rulesOfTheSea) 616 return self.pidTactic.composeAction(agentWorldModel)
617
618 - def done(self, agentWorldModel):
619 "Implements abstract method." 620 estimatedState = agentWorldModel.getEstimatedExtShipState() 621 estimatedPos = (estimatedState.x, estimatedState.y) 622 return self.atPoint(estimatedPos, self.goalPoint)
623
624 - def getGoalPos(self):
625 "Implements abstract method." 626 return self.goalPoint
627