Chapter 1: Fundamentals of Robotics

Activity 1: Robot Positioning Using Odometry with Python

Solution

from math import pi

def wheel_distance(diameter, encoder, encoder_time, wheel, movement_time):
    time = movement_time / encoder_time
    wheel_encoder = wheel * time
    wheel_distance = (wheel_encoder * diameter * pi) / encoder
    
    return wheel_distance

from math import cos,sin

def final_position(initial_pos,wheel_axis,angle):
    final_x=initial_pos[0]+(wheel_axis*cos(angle))
    final_y=initial_pos[1]+(wheel_axis*sin(angle))
    final_angle=initial_pos[2]+angle
    
    return(final_x,final_y,final_angle)
def position(diameter,base,encoder,encoder_time,left,right,initial_pos,movement_time):
#First step: Wheels completed distance
    left_wheel=wheel_distance(diameter,encoder,encoder_time,left,movement_time)
    right_wheel=wheel_distance(diameter,encoder,encoder_time,right,movement_time)
#Second step: Wheel's central axis completed distance
    wheel_axis=(left_wheel+right_wheel)/2
#Third step: Robot's rotation angle
    angle=(right_wheel-left_wheel)/base
#Final step: Final position calculus
    final_pos=final_position(initial_pos,wheel_axis,angle)
    
    returnfinal_pos


position(10,80,76,5,600,900,(0,0,0),5)

Note

For further observations, you can change the wheels' diameter to 15 cm and check the difference in the output. Similarly, you can change other input values and check the difference in the output.