def a():
myWorld = simpleWorld.buildWorld()
myRobot = RobotMovement.RobotMovement()
pfpe = ParticleFilterPoseEstimator(myRobot)
pfpe.initialize((2, 2, pi), (6, 6, 0), n=10)
drawParticles(myWorld, pfpe.particles)
myWorld.setRobot(myRobot, [4, 4, pi / 2])
myRobot.move([1, 0.3])
pfpe.integrateMovement([1, 0.3])
drawParticles(myWorld, pfpe.particles)
myRobot.move([1, 0.3])
pfpe.integrateMovement([1, 0.3])
drawParticles(myWorld, pfpe.particles)
myRobot.move([1, 0.3])
pfpe.integrateMovement([1, 0.3])
drawParticles(myWorld, pfpe.particles)
myRobot.move([1, 0.3])
pfpe.integrateMovement([1, 0.3])
drawParticles(myWorld, pfpe.particles)
# myRobot.curveDrive(0.2, 4.5, -180)
myWorld.close()
def b():
myWorld = simpleWorld.buildWorld()
myRobot = RobotMovement.RobotMovement()
myWorld.setRobot(myRobot, [4, 4, pi / 2])
myGrid = myWorld.getDistanceGrid()
pfpe = ParticleFilterPoseEstimator(myRobot)
pfpe.initialize((3, 3, pi), (7, 7, 0), n=200)
drawParticles(myWorld, pfpe.particles)
pose = pfpe.getPose()
point = graphics.Point(pose[0], pose[1])
c = graphics.Circle(point, 0.1)
c.setFill(("yellow"))
c.draw(myWorld.win)
pfpe.integrateMeasurement(myRobot.sense(), myRobot.getSensorDirections(),
myGrid)
drawParticles(myWorld, pfpe.particles)
pose = pfpe.getPose()
point = graphics.Point(pose[0], pose[1])
c = graphics.Circle(point, 0.1)
c.setFill(("blue"))
c.draw(myWorld.win)
myWorld.close()
def c():
myWorld = simpleWorld.buildWorld()
myRobot = RobotMovement.RobotMovement()
myWorld.setRobot(myRobot, [4, 4, pi / 2])
myWorld._showPathHistory = True
myGrid = myWorld.getDistanceGrid()
pfpe = ParticleFilterPoseEstimator(myRobot)
pfpe.initialize((3, 3, pi), (15, 10, 0), n=500)
drawParticles(myWorld, pfpe.particles)
pose = pfpe.getPose()
point = graphics.Point(pose[0], pose[1])
c = graphics.Circle(point, 0.1)
c.setFill(("yellow"))
c.draw(myWorld.win)
motionCircle = curveDrive(0.8, 4, -180)
idx = 0
truePose = []
trueRobotPose = myWorld.getTrueRobotPose()
truePose.append((trueRobotPose[0], trueRobotPose[1]))
for motion in motionCircle:
myRobot.move(motion)
trueRobotPose = myWorld.getTrueRobotPose()
truePose.append((trueRobotPose[0], trueRobotPose[1]))
pfpe.integrateMovement(motion)
pfpe.integrateMeasurement(myRobot.sense(),
myRobot.getSensorDirections(), myGrid)
drawParticles(myWorld, pfpe.particles)
pose = pfpe.getPose()
if idx % 10 == 0:
cov_x_y, sigma_theta = pfpe.getCovariance()
plotPositionCovariance((pose[0], pose[1]), cov_x_y)
idx += 1
point = graphics.Point(pose[0], pose[1])
c.undraw()
c = graphics.Circle(point, 0.1)
c.setFill(("blue"))
c.draw(myWorld.win)
for _ in range(0, len(truePose)):
x = [i[0] for i in truePose]
y = [i[1] for i in truePose]
plt.plot(x, y)
plt.show()
myWorld.close()
def d():
myWorld = simpleWorld.buildWorld()
myRobot = RobotMovement.RobotMovement()
myWorld.setRobot(myRobot, [4, 4, pi / 2])
myWorld._showPathHistory = True
myWorld.addDynObstacleLine(7, 4, 7, 7)
myGrid = myWorld.getDistanceGrid()
myGrid.drawGrid()
pfpe = ParticleFilterPoseEstimator(myRobot)
pfpe.initialize((3, 3, pi), (7, 7, 0), n=200)
drawParticles(myWorld, pfpe.particles)
pose = pfpe.getPose()
point = graphics.Point(pose[0], pose[1])
c = graphics.Circle(point, 0.1)
c.setFill(("yellow"))
c.draw(myWorld.win)
motionCircle = curveDrive(0.2, 4, -180)
circle = c
for motion in motionCircle:
myRobot.move(motion)
pfpe.integrateMovement(motion)
pfpe.integrateMeasurement(myRobot.sense(),
myRobot.getSensorDirections(), myGrid)
drawParticles(myWorld, pfpe.particles)
pose = pfpe.getPose()
cov_x_y, sigma_theta = pfpe.getCovariance()
plotPositionCovariance((pose[0], pose[1]), cov_x_y)
point = graphics.Point(pose[0], pose[1])
c.undraw()
c = graphics.Circle(point, 0.1)
c.setFill(("blue"))
c.draw(myWorld.win)
myWorld.close()
import numpy as np
from Robot_Simulator_V3 import simpleWorld
from Robot_Simulator_V3 import Robot
def curveDrive(robot, world, v=1, r=4, deltaTheta=-np.pi, n=100):
omega = v / r * np.sign(deltaTheta)
robot.setTimeStep(abs(deltaTheta / omega))
poses = [world.getTrueRobotPose()]
for _ in range(n):
robot.move([v / n, omega / n])
poses.append(world.getTrueRobotPose())
return poses
if __name__ == "__main__":
myWorld = simpleWorld.buildWorld()
myRobot = Robot.Robot()
myWorld.setRobot(myRobot, [4, 4, np.pi / 2])
print(curveDrive(myRobot, myWorld))
myWorld.close()