def test_plot_feedback_velocity_profile(self):
aim_short_dist = 0.1
max_vel = 1.0
max_accel = 0.5
dt = 0.1
profile = VelocityProfiler( aim_short_dist, max_vel, max_accel, dt)
loop_num = 0
distance_to_travel = 10.0
distance_left = distance_to_travel
distance_traveled = 0.0
velocity = 0.0
finished = False
loop_arr = [0]
dist_left_arr = [distance_left]
dist_traveled_arr = [0]
vel_arr = [0]
while not finished:
(velocity, finished, goal_reached) = \
profile.next_profile_step_with_feedback(distance_left,
velocity, 0, loop_num)
dist = (velocity * dt)
distance_traveled += dist
distance_left -= dist
loop_num += 1
loop_arr.append(loop_num)
vel_arr.append(velocity)
dist_left_arr.append(distance_left)
dist_traveled_arr.append(distance_traveled)
abs(distance_to_travel)
abs(distance_traveled)
max_dist = distance_to_travel + (max_accel * dt)
min_dist = distance_to_travel - (max_accel * dt)
print 'Feedback Actual Distance %g, Expected %g' %
(distance_traveled, distance_to_travel)
def test_plot_delayed_feedback_velocity_profile(self):
feedback_delay = 3 # dt steps
aim_short_dist = 0.1
max_vel = 1.0
max_accel = 0.5
dt = 0.1
profile = VelocityProfiler( aim_short_dist, max_vel, max_accel, dt)
loop_num = 0
distance_to_travel = 10.0
distance_left = distance_to_travel
distance_traveled = 0.0
velocity = 0.0
loop_arr = [0]
dist_left_arr = [distance_left]
dist_traveled_arr = [0]
vel_arr = [0]
for x in range(0, feedback_delay):
loop_arr.append(0)
dist_left_arr.append(distance_left)
dist_traveled_arr.append(0)
vel_arr.append(0)
finished = False
while not finished:
distance_left = dist_left_arr[loop_num]
# print
# print loop_num
# print velocity
# print distance_left
(velocity, finished, goal_reached) = \
profile.next_profile_step_with_feedback(distance_left,
velocity, 0.3, loop_num)
dist = (velocity * dt)
distance_traveled += dist
distance_left = dist_left_arr[loop_num + feedback_delay] - dist
loop_num += 1
# print dist
# print distance_left
# print distance_traveled
loop_arr.append(loop_num)
vel_arr.append(velocity)
dist_left_arr.append(distance_left)
dist_traveled_arr.append(distance_traveled)
abs(distance_to_travel)
abs(distance_traveled)
max_dist = distance_to_travel + (max_accel * dt)
min_dist = distance_to_travel - (max_accel * dt)
print 'Delayed Feedback (%g sec) Actual Distance %g, Expected %g' %
((feedback_delay * dt), distance_traveled, distance_to_travel)
def run_ballistic_velocity_profile(distance_to_travel, max_vel, max_accel, dt,
aim_short_dist):
''' Runs and plots a ballisitic (open-loop) velocity profile. '''
run_stats = 'Distance: %g Max Velocity: %g Max Acceleration: %g dt: %g Aim Short:%g '\
% (distance_to_travel, max_vel, max_accel, dt, aim_short_dist)
print '+++ Ballistic Profile +++'
print run_stats
profile = VelocityProfiler( aim_short_dist, max_vel, max_accel, dt)
loop_num = 0
distance_left = distance_to_travel
distance_traveled = 0.0
finished = False
loop_arr = [0]
dist_left_arr = [distance_left]
dist_traveled_arr = [0]
vel_arr = [0]
error_loops = []
error_vels = []
while not finished:
(velocity, finished, goal_reached) = \
profile.next_profile_step_ballistic(distance_left, loop_num)
dist = (velocity * dt)
distance_traveled += dist
distance_left -= dist
# whine but don't fail if we exceed accel limits
loop_num += 1
accel = abs(velocity - vel_arr[loop_num - 1]) / dt
if ( accel > max_accel + 0.000001): # addition for floating point alaising
print 'Loop %g acceleration %g is too high (max %g), velocity %g' % \
(loop_num, accel, max_accel, velocity)
error_loops.append(loop_num - 1)
error_loops.append(loop_num)
error_vels.append(vel_arr[loop_num - 1])
error_vels.append(velocity)
# Store all the datapoints to display
loop_arr.append(loop_num)
vel_arr.append(velocity)
# dist_left_arr.append(distance_left)
dist_traveled_arr.append(distance_traveled)
# clear last data point for pretty graphs ;)
loop_arr.pop()
vel_arr.pop()
dist_traveled_arr.pop()
title = 'Ballistic\n%s\nDistance to Target: %f' % (run_stats, distance_left)
generate_plots( loop_arr, dist_traveled_arr, vel_arr, loop_num,
distance_to_travel, max_vel, error_loops, error_vels,
title)
print '+++ Ballistic Actual Distance %g, Expected %g +++\n' % \
(distance_traveled, distance_to_travel)
'''
def run_delayed_feedback_velocity_profile(distance_to_travel, max_vel, max_accel,
dt, aim_short_dist, delay_steps):
''' Runs and plots a ballisitic (open-loop) velocity profile. '''
run_stats = 'Distance: %g Max Velocity: %g Max Acceleration: %g dt: %g Aim Short: %g'\
% (distance_to_travel, max_vel, max_accel, dt, aim_short_dist)
delay_secs = delay_steps * dt
print '=== Delayed Feedback Profile ==='
print run_stats, 'Feedback Delay:', delay_secs
profile = VelocityProfiler(aim_short_dist, max_vel, max_accel, dt)
loop_num = 0
distance_left = distance_to_travel
distance_traveled = 0.0
velocity = 0.0
loop_arr = [0]
dist_left_arr = [distance_left]
dist_traveled_arr = [0]
vel_arr = [0]
error_loops = []
error_vels = []
for x in range(0, delay_steps):
loop_arr.append(0)
dist_left_arr.append(distance_left)
dist_traveled_arr.append(0)
vel_arr.append(0)
finished = False
while not finished:
distance_left = dist_left_arr[loop_num]
# print
# print loop_num
# print velocity
# print distance_left
(velocity, finished, goal_reached) = \
profile.next_profile_step_with_feedback(distance_left,
velocity, delay_secs, loop_num)
dist = (velocity * dt)
distance_traveled += dist
distance_left = dist_left_arr[loop_num + delay_steps] - dist
# whine but don't fail if we exceed accel limits
loop_num += 1
accel = abs(velocity - vel_arr[loop_num + delay_steps - 1]) / dt
if ( accel > max_accel + 0.000001): # addition for floating point alaising
print 'Loop %g acceleration %g is too high (max %g), velocity %g' % \
(loop_num, accel, max_accel, velocity)
# record segments which violate
error_loops.append(loop_num - 1)
error_loops.append(loop_num)
error_vels.append(vel_arr[loop_num + delay_steps - 1])
error_vels.append(velocity)
# print dist
# print distance_left
# print distance_traveled
# Store all the datapoints to display
loop_arr.append(loop_num)
vel_arr.append(velocity)
dist_left_arr.append(distance_left)
dist_traveled_arr.append(distance_traveled)
title = 'Delayed Feedback (%g sec)\n%s\nDistance to Target: %f' % \
(delay_secs, run_stats, distance_left)
generate_plots( loop_arr, dist_traveled_arr, vel_arr, loop_num,
distance_to_travel, max_vel, error_loops, error_vels,
title )
print '=== Delayed Feedback (%g sec) Actual Distance %g, Expected %g ===\n' % \
(delay_secs, distance_traveled, distance_to_travel)
'''
