def display_bicycle_wheels(rear_wheel, front_wheel, theta):
# Initialize figure
fig = plt.figure(figsize=(5, 5))
ax = plt.gca()
ax.set_xlim([0,4])
ax.set_ylim([0,4])
ax.tick_params(axis='both', which='major', labelsize=7)
plt.title('Overhead View')
plt.xlabel('X (m)',weight='bold')
plt.ylabel('Y (m)',weight='bold')
ax.plot(0,0)
rear_wheel_x = FancyArrowPatch((0,0), (0.4,0),
mutation_scale=8,color='red')
rear_wheel_y = FancyArrowPatch((0,0), (0,0.4),
mutation_scale=8,color='red')
front_wheel_x = FancyArrowPatch((0,0), (0.4,0),
mutation_scale=8,color='blue')
front_wheel_y = FancyArrowPatch((0,0), (0,0.4),
mutation_scale=8,color='blue')
custom_lines = [Line2D([0], [0], color='red', lw=4),
Line2D([0], [0], color='blue', lw=4)]
# Apply translation and rotation as specified by current robot state
cos_theta, sin_theta = np.cos(theta), np.sin(theta)
Tw_rear = np.eye(3)
Tw_rear[0:2,2] = rear_wheel
Tw_rear[0:2,0:2] = [[cos_theta,-sin_theta],[sin_theta,cos_theta]]
Tw_rear_obj = transforms.Affine2D(Tw_rear)
Tw_front = np.eye(3)
Tw_front[0:2,2] = front_wheel
Tw_front[0:2,0:2] = [[cos_theta,-sin_theta],[sin_theta,cos_theta]]
Tw_front_obj = transforms.Affine2D(Tw_front)
ax_trans = ax.transData
rear_wheel_x.set_transform(Tw_rear_obj+ax_trans)
rear_wheel_y.set_transform(rear_wheel_x.get_transform())
ax.add_patch(rear_wheel_x)
ax.add_patch(rear_wheel_y)
front_wheel_x.set_transform(Tw_front_obj+ax_trans)
front_wheel_y.set_transform(front_wheel_x.get_transform())
ax.add_patch(front_wheel_x)
ax.add_patch(front_wheel_y)
ax.legend(custom_lines, ['Rear Wheel', 'Front Wheel'])
