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.