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

Source Code for Module navigationTactics

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