def integrateMovement(self, motion):
tmpRobot = Robot.Robot()
v = motion[0]
omega = motion[1]
# translational and rotational speed is limited:
if omega > tmpRobot._maxOmega:
omega = tmpRobot._maxOmega
if omega < -tmpRobot._maxOmega:
omega = -tmpRobot._maxOmega
if v > tmpRobot._maxSpeed:
v = tmpRobot._maxSpeed
if v < -tmpRobot._maxSpeed:
v = -tmpRobot._maxSpeed
newPoses = []
for pose in self.Particles:
# Add noise to v:
sigma_v_2 = (tmpRobot._k_d / tmpRobot._T) * abs(v)
v_noisy = v + random.gauss(0.0, np.sqrt(sigma_v_2))
# Add noise to omega:
sigma_omega_tr_2 = (tmpRobot._k_theta / tmpRobot._T) * abs(
omega) # turning rate noise
sigma_omega_drift_2 = (tmpRobot._k_drift / tmpRobot._T) * abs(
v) # drift noise
omega_noisy = omega + random.gauss(0.0, np.sqrt(sigma_omega_tr_2))
omega_noisy += random.gauss(0.0, np.sqrt(sigma_omega_drift_2))
# Set SigmaMotion:
tmpRobot._SigmaMotion[0, 0] = sigma_v_2
tmpRobot._SigmaMotion[1,
1] = sigma_omega_tr_2 + sigma_omega_drift_2
# Move robot in the world (with noise):
d_noisy = v_noisy * tmpRobot._T
dTheta_noisy = omega_noisy * tmpRobot._T
x = pose[0] + d_noisy * np.cos(pose[2] + 0.5 * dTheta_noisy)
y = pose[1] + d_noisy * np.sin(pose[2] + 0.5 * dTheta_noisy)
theta = pose[2] + dTheta_noisy
newPoses.append([x, y, theta])
self.Particles = newPoses
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()
