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_buildUnitaryVectorFromAngle_210(self): vector = Vector.buildUnitaryVectorFromAngle(-210) self.assertAlmostEqual(-math.sqrt(3)/2, vector[0], delta=0.000001) self.assertAlmostEqual(-0.5, vector[1], delta=0.0000001)
def test_buildUnitaryVectorFromAngle_ninety(self): vector = Vector.buildUnitaryVectorFromAngle(90) self.assertAlmostEqual(0, vector[0], delta=0.000001) self.assertEqual(-1, vector[1])
def test_buildUnitaryVectorFromAngle_zero(self): vector = Vector.buildUnitaryVectorFromAngle(0) self.assertEqual([1, 0], vector)