Previous lab. Return home. Next lab.
This function is used to compute, from the current location and the previous location, the first angle of rotation, the movement distance, and the second angle of rotation. The following code shows my implementation for this.
def compute_control(cur_pose, prev_pose):
delta_pose = cur_pose[0] - prev_pose[0]
translation_angle = math.atan2( delta_pose[1], delta_pose[0] )*180/math.pi
delta_rot_1 = translation_angle - prev_pose[0][2]
delta_trans = math.sqrt( delta_pose[0]**2 + delta_pose[1]**2 )
delta_rot_2 = cur_pose[0][2] - translation_angle
return delta_rot_1, delta_trans, delta_rot_2
It essentially takes the current position's x and y coordinates, and takes the difference so that it knows the magnitude of the translation in each dimension. It then uses this to calculate the angle of this translation in degrees. This will then be used in conjunction with the starting angle to know what the first rotation is, which is rotation from the starting angle to the translation angle. Then, going from the translation angle to the final angle is the second rotation.
Alas, it is at this point that I must stop, because I have been informed that there is a link to the lab 10 solution in lab 11 handout. It will be better for me to try to continue on then work on a lab for which solutions have already been released.