def determineCircle(self, angleBetweenP1AndP2, p1, p2): distanceBetweenP1andP2 = Vector.length(Vector.buildFromTwoPoints(p1, p2)) radius = distanceBetweenP1andP2 / 2 / math.sin(math.radians(angleBetweenP1AndP2)) circle1 = Circle(p1, radius) circle2 = Circle(p2, radius) intersectionPoints = Circle.intersect(circle1, circle2) center = intersectionPoints[0] if len(intersectionPoints) == 2: v1 = Vector.buildFromTwoPoints(intersectionPoints[0], p1) v2 = Vector.buildFromTwoPoints(intersectionPoints[0], p2) angle1 = Vector.angleBetween(v1, v2) v1 = Vector.buildFromTwoPoints(intersectionPoints[1], p1) v2 = Vector.buildFromTwoPoints(intersectionPoints[1], p2) angle2 = Vector.angleBetween(v1, v2) if angle1 > angle2: center = intersectionPoints[0] else: center = intersectionPoints[1] return Circle(center, radius)
def planMovement(self, currentPose, nextNodes, finalAbsoluteAngle): currentX = currentPose[0] currentY = currentPose[1] currentTheta = currentPose[2] moves = [] for nextNode in nextNodes: currentPoseVector = Vector.buildUnitaryVectorFromAngle(currentTheta) destinationVector = Vector.buildFromTwoPoints((currentX, currentY), (nextNode[0], nextNode[1])) rotationAngle = Vector.angleBetween(currentPoseVector, destinationVector) if rotationAngle: moves.append(Rotate(rotationAngle)) distanceToAdvance = Vector.length(destinationVector) if distanceToAdvance: moves.append(Advance(distanceToAdvance)) currentX = nextNode[0] currentY = nextNode[1] currentTheta += rotationAngle currentRobotVector = Vector.buildUnitaryVectorFromAngle(currentTheta) finalRobotVector = Vector.buildUnitaryVectorFromAngle(finalAbsoluteAngle) rotationAngle = Vector.angleBetween(currentRobotVector, finalRobotVector) if math.fabs(rotationAngle) > 0.01: moves.append(Rotate(rotationAngle)) return moves
def planMovement(self, currentPose, nextNodes): currentX = currentPose[0] currentY = currentPose[1] currentTheta = currentPose[2] currentPoseVector = Vector.buildUnitaryVectorFromAngle(currentTheta) moves = [] print "current pose in shuffle: " + str(currentPose) print "path to be planned in shuffle: " + str(nextNodes) for nextNode in nextNodes: destinationVector = Vector.buildFromTwoPoints((currentX, currentY), (nextNode[0], nextNode[1])) shuffleDistance = Vector.length(destinationVector) shuffleAngle = Vector.angleBetween(currentPoseVector, destinationVector) if shuffleAngle < 0: shuffleAngle += 360 moves.append(Shuffle(shuffleDistance, shuffleAngle)) currentX = nextNode[0] currentY = nextNode[1] return moves
def test_angleBetween_ninetyDegrees(self): self.assertEqual(-90, Vector.angleBetween([1, 0], [0, 1]))
def test_angleBetween_minusNinetyDegreesLargeRotation(self): self.assertAlmostEqual(90, Vector.angleBetween([0, -1],[-1, 0]), delta=0.000000001)
def test_angleBetween_largeRotation(self): expectedAngle = math.degrees(-1*math.acos(2/math.sqrt(1229))) self.assertAlmostEqual(expectedAngle, Vector.angleBetween([-1, 0], [-2, -35]), delta=0.000000001)
def test_angleBetween_minusThirtyDegrees(self): self.assertAlmostEqual(30, Vector.angleBetween([-1 / 2, -math.sqrt(3) / 2], [-math.sqrt(3) / 2, -1 / 2]), delta=0.000000001)
def test_angleBetween_thirtyDegrees(self): expectedAngle = -30 self.assertAlmostEqual(expectedAngle, Vector.angleBetween([-math.sqrt(3) / 2, -1 / 2], [-1 / 2, -math.sqrt(3) / 2]), delta=0.000000001)
def test_angleBetween_minusFortyFiveDegrees(self): expectedAngle = 45 self.assertEqual(expectedAngle, Vector.angleBetween([1, 1], [1, 0]))
def test_angleBetween_minusOneEightyDegrees(self): self.assertEqual(180, Vector.angleBetween([-1, 0], [1, 0]))
def test_angleBetween_oneEightyDegrees(self): self.assertEqual(-180, Vector.angleBetween([1, 0], [-1, 0]))
def test_angleBetween_zeroDegrees(self): self.assertEqual(0, Vector.angleBetween([1, 1], [2, 2]))
def test_angleBetween_minusNinetyDegrees(self): self.assertEqual(90, Vector.angleBetween([0, 1], [1, 0]))