def update_encoders(left_prev, right_prev, mu_prev):
"""Odometry Sensor Integration
Integration of wheel encoder information to provide a continuous update of
internal position. This state is never intended to be updated with other
sensor information. Per 5.4 Odometry Motion Model - this is the function
that will be reporting relative motion information such as advancement.
:param left_prev: prior left encoder
:param right_prev: prior right encoder
:param mu_prev: prior internal pose - x_bar_t-1
TODO: Add relative advancement in x and y to the return
y_bar_prime - y_bar
x_bar_prime - x_bar
theta_bar_prime - theta_bar
:return: current internal pose - x_bar_t
"""
# Initialize data needed
new_reading = get_reading(read_mag=False)
# Update Encoders
scale = 0.0577
left_delta = new_reading.get('left_enc') - left_prev
right_delta = new_reading.get('right_enc') - right_prev
r = scale * (left_delta + right_delta) / 2
rotation_scale = 5.5
theta_delta = (left_delta - right_delta) / rotation_scale
x_delta = math.sin((mu_prev[2] + theta_delta) * np.pi / 180) * r
y_delta = math.cos((mu_prev[2] + theta_delta) * np.pi / 180) * r
mu = mu_prev + np.array([x_delta, y_delta, theta_delta]).T
return new_reading.get('left_enc'), new_reading.get('right_enc'), mu
def update_position(left_prev, right_prev, theta_prev, time_prev):
new_reading = get_reading()
# Update Encoders
left_delta = new_reading.get('left_enc') - left_prev
right_delta = new_reading.get('right_enc') - right_prev
# Scale factor to go from encoder to centimeter -
scale = 0.0577
# Distance traveled in scaled units
lr_avg = (left_delta + right_delta) / 2 * scale
# translation in a straight line (distance in cm - r) is lr_avg
# translational_vel = lr_avg / delta_time * delta_theta
# Update theta based upon gyroscope
delta_time = (new_reading.get('time') - time_prev).total_seconds()
theta = theta_prev - new_reading.get('gyro_y') * delta_time
delta_x = math.sin(theta * 0.0174533) * lr_avg
delta_y = math.cos(theta * 0.0174533) * lr_avg
return new_reading.get('left_enc'), new_reading.get(
'right_enc'), delta_x, delta_y, theta, new_reading.get('time')
from drive.utils import get_reading
gpg = EasyGoPiGo3()
gpg.reset_encoders()
atexit.register(gpg.stop)
def drive_instructions():
process_name = multiprocessing.current_process().name
print("Starting Drive Process {}".format(process_name))
gpg.drive_cm(10)
gpg.turn_degrees(90)
gpg.drive_cm(20)
gpg.turn_degrees(-90)
gpg.drive_cm(10)
gpg.stop()
drive_process = multiprocessing.Process(name='drive',
target=drive_instructions)
drive_process.start()
num = 30
while num > 0:
time.sleep(.10)
measurements = get_reading()
# pprint(measurements)
print("Bearing: {}".format(measurements.get('euler_x')))
num -= 1
drive_process.join()
def update_position(left_prev, right_prev, mu_prev, cov, time_prev):
theta_prev = mu_prev[2, 0]
# Initialize data needed
new_reading = get_reading(read_mag=False)
delta_time = (new_reading.get('time') - time_prev).total_seconds()
# Get 'r' from encoders and scale to centimeters
scale = 0.0577
left_delta = new_reading.get('left_enc') - left_prev
right_delta = new_reading.get('right_enc') - right_prev
# scale * average encoder advancement / time_elapsed
nu = scale * (left_delta + right_delta) / 2 / delta_time
# Update theta based upon gyroscope
omega = new_reading.get('gyro_y') * np.pi / 180
vt_omega = omega if np.abs(omega) > 10e-8 else 10e-8
theta_new = theta_prev - omega * delta_time
r = nu / omega if np.abs(omega) > 10e-6 else nu / 10e-6
G_t = np.array([[1, 0, r * (np.cos(theta_new) - np.cos(theta_prev))],
[0, 1, r * (np.sin(theta_new) - np.sin(theta_prev))],
[0, 0, 1]])
V_t = np.array(
[[(-np.sin(theta_prev) + np.sin(theta_new)) / vt_omega,
nu * (np.sin(theta_prev) - np.sin(theta_new)) / vt_omega**2 +
(r * np.cos(theta_new) * delta_time)],
[(np.cos(theta_prev) - np.cos(theta_new)) / vt_omega,
-nu * (np.cos(theta_prev) - np.cos(theta_new)) / vt_omega**2 +
(r * np.sin(theta_new) * delta_time)], [0, delta_time]])
# What to plugin to alpha 1-4?
alpha_1, alpha_2, alpha_3, alpha_4 = 10e-7, 15e-7, 5e-7, 20e-7
M_t_1_1 = alpha_1 * nu**2 + alpha_2 * omega**2
M_t_2_2 = alpha_3 * nu**2 + alpha_4 * omega**2
M_t = np.array([[M_t_1_1, 0], [0, M_t_2_2]])
try:
Sigma = G_t.dot(cov).dot(G_t.T) + V_t.dot(M_t).dot(V_t.T)
except:
Sigma = G_t.dot(cov).dot(G_t.T)
print("Unexpected error:", sys.exc_info()[0])
print(vt_omega)
print(theta_prev)
print(theta_new)
mu_new = mu_prev + np.array([[
r * (np.sin(theta_new) - np.sin(theta_prev))
], [r * (np.cos(theta_prev) - np.cos(theta_new))], [omega * delta_time]])
if i % 10 == 0:
print(mu_prev)
print(mu_new)
print(nu)
print(omega)
pprint(M_t)
print(G_t)
pprint(V_t)
pprint(Sigma)
return new_reading.get('left_enc'), new_reading.get(
'right_enc'), mu_new, Sigma, new_reading.get('time')
print(nu)
print(omega)
pprint(M_t)
print(G_t)
pprint(V_t)
pprint(Sigma)
return new_reading.get('left_enc'), new_reading.get(
'right_enc'), mu_new, Sigma, new_reading.get('time')
# Initialize Measurements for drive
i = 0
data = []
pose = np.array([[init_x], [init_y],
[get_reading().get('euler_x') * np.pi / 180]])
cov = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
left_prev = 0
right_prev = 0
curr_time = datetime.now()
# Start Driving
drive_process.start()
# Measure while driving loop
while i < 100:
# mu is 3 x 1 pose
left_prev, right_prev, pose, cov, curr_time = update_position(
left_prev, right_prev, pose, cov, curr_time)
# print("x= %2.2f, y=%2.2f, theta==%2.2f " % (pose[0], pose[1], pose[2]))
# print(pose)
theta = theta_prev - new_reading.get('gyro_y') * delta_time
delta_x = math.sin(theta * 0.0174533) * lr_avg
delta_y = math.cos(theta * 0.0174533) * lr_avg
return new_reading.get('left_enc'), new_reading.get(
'right_enc'), delta_x, delta_y, theta, new_reading.get('time')
# Initialize Measurements for drive
i = 0
data = []
left_prev = 0
right_prev = 0
x_total = init_x
y_total = init_y
theta = get_reading().get('euler_x')
curr_time = datetime.now()
# Start Driving
drive_process.start()
# Measure while driving loop
while i < 100:
left_prev, right_prev, delta_x, delta_y, theta, curr_time = update_position(
left_prev, right_prev, theta, curr_time)
x_total = x_total + delta_x
y_total = y_total + delta_y
print("x= %2.2f, y=%2.2f, theta==%2.2f " %
(x_total / 44, y_total / 44, theta))
data.append({"t": str(curr_time), "x": x_total, "y": y_total})
# Setup Standard Drive
q = multiprocessing.Queue()
drive_process = multiprocessing.Process(name='drive',
target=test_drive_instr,
args=(q, ))
# drive_process.start()
i = 0
data = []
while i < 200:
# if i == 100:
# drive_process.start()
reading = get_reading(read_mag=False)
reading['drive name'] = drive_name
data.append(reading)
i += 1
while not q.empty():
print(q.get())
time.sleep(.0625)
# Wrap up processes, print and save
# drive_process.join()
pprint(data)
print('done')
if saving_data:
output_file_name = os.path.join(os.getcwd(), 'offline', 'data', file_out)
# keys = data[0].keys()
def update_position(left_prev, right_prev, vel_prev, mu_control_prev,
mu_sensor_prev, cov, time_prev):
"""Update position using two sources of information; control and sensors.
The sensor data comes from the imu/gyroscope and the control comes from the
odometers.
nu is translational velocity in cm per second
omega is Rotational Velocity in radians per second
scales is a tuned parameter that turns the incoming odometer units to
centimeters.
:param left_prev:
:param right_prev:
:param vel_prev:
:param mu_control_prev:
:param mu_sensor_prev:
:param cov:
:param time_prev:
:return:
"""
# Get new measurements
new_reading = get_reading(read_mag=False)
# pprint(new_reading)
delta_time = (new_reading.get('time') - time_prev).total_seconds()
# Initialize
# Get 'r' from encoders and scale to centimeters
theta_control_prev = mu_control_prev[2, 0]
theta_sensor_prev = mu_sensor_prev[2, 0]
scale = 0.0577 * 2 * 10e-3 # Odometer units to centimeters, 20e-3 is magic number
left_delta = new_reading.get('left_enc') - left_prev
right_delta = new_reading.get('right_enc') - right_prev
# scale * average encoder advancement / time_elapsed
nu_control = scale * (left_delta + right_delta) / (2 * delta_time
) # should be cm/s
# TODO: Test the below decompositions, then refactor
# Calibration from 7/19 [0.554, -0.235, 1.755]
accel_forward = (new_reading.get('accel_z') - 1.755)
accel_side = (new_reading.get('accel_x') - 0.554)
vel_now = vel_prev + delta_time * np.array([
accel_forward * np.sin(theta_sensor_prev) + accel_side *
np.cos(theta_sensor_prev), accel_forward * np.cos(theta_sensor_prev) +
accel_side * np.sin(theta_sensor_prev)
])
# TODO: Decompose back to velocity in forward direction
# Total velocity is L2 Norm
# nu_sensor?
# nu_sensor = np.sqrt(vel_now[0]**2 + vel_now[1]**2)
nu_sensor = np.abs(np.sin(theta_sensor_prev) * vel_now[0]) + np.abs(
np.cos(theta_sensor_prev) * vel_now[1])
# Theta from Odometer
rotation_scale = 5.5
theta_delta_control = (left_delta - right_delta) / rotation_scale
theta_control_new = theta_control_prev + theta_delta_control
omega_control = theta_delta_control * np.pi / (180 * delta_time)
# Theta from Gyroscope
omega_sensor = -1 * new_reading.get('gyro_y') * np.pi / 180
theta_delta_sensor = omega_sensor * delta_time
theta_sensor_new = theta_sensor_prev + theta_delta_sensor
if np.abs(omega_control) > 10e-4:
omega_control_star = omega_control
theta_control_new_star = theta_control_prev + theta_delta_control
else:
omega_control_star = 10e-4
theta_control_new_star = theta_control_prev + 10e-4 * delta_time
r_control = nu_control / omega_control_star
if np.abs(omega_sensor) > 10e-4:
omega_sensor_star = omega_sensor
theta_sensor_new_star = theta_sensor_prev + theta_delta_sensor
else:
omega_sensor_star = 10e-4
theta_sensor_new_star = theta_sensor_prev + 10e-4 * delta_time
r_sensor = nu_sensor / omega_sensor_star
# r_control = nu_control / omega_control_star if np.abs(omega_control) > 10e-6 else nu_control / 10e-6
# r_sensor = nu_sensor / omega_sensor if np.abs(omega_sensor) > 10e-6 else nu_sensor / 10e-6
# r_control = nu_control * delta_time
# r_sensor = nu_sensor * delta_time
mu_control_new = mu_control_prev + np.array(
[[
r_control *
(np.sin(theta_control_new_star) - np.sin(theta_control_prev))
],
[
r_control *
(np.cos(theta_control_prev) - np.cos(theta_control_new_star))
], [omega_control * delta_time]])
mu_sensor_new = mu_sensor_prev + np.array([[
r_sensor * (np.sin(theta_sensor_new_star) - np.sin(theta_sensor_prev))
], [
r_sensor * (np.cos(theta_sensor_prev) - np.cos(theta_sensor_new_star))
], [omega_sensor * delta_time]])
if i % 10 == 0:
print("Control", nu_control, omega_control)
print(mu_control_prev, mu_control_new)
print("Sensor", nu_sensor, omega_sensor)
print(mu_sensor_prev, mu_sensor_new)
velocities = {
'nu_control': nu_control,
'omega_control': omega_control,
'nu_sensor': nu_sensor,
'omega_sensor': omega_sensor,
}
return (
new_reading.get('left_enc'),
new_reading.get('right_enc'),
vel_now,
mu_control_new,
mu_sensor_new,
cov, # Sigma
new_reading.get('time'),
velocities)
rotation_scale = 5.5
theta_delta = (left_delta - right_delta) / rotation_scale
x_delta = math.sin((mu_prev[2] + theta_delta) * np.pi / 180) * r
y_delta = math.cos((mu_prev[2] + theta_delta) * np.pi / 180) * r
mu = mu_prev + np.array([x_delta, y_delta, theta_delta]).T
return new_reading.get('left_enc'), new_reading.get('right_enc'), mu
# Initialize Measurements for drive
i = 0
t1 = datetime.now()
data = []
int_pose = np.array([init_x, init_y, 0]).T
left_enc = get_reading().get('left_enc')
right_enc = get_reading().get('right_enc')
# Start Driving
drive_process.start()
# Measure while driving loop
while i < 100:
t2 = datetime.now()
left_enc, right_enc, int_pose = update_encoders(left_enc, right_enc,
int_pose)
print(int_pose)
data.append({
"t": str(t2),
"x": int_pose[0],
