binned throttle for donkeycar

binned_throttle.py

import numpy as np
np.set_printoptions(precision=4,suppress=True,linewidth=180)

def clamp(n, min, max):
    if n < min:
        return min
    if n > max:
        return max
    return n

def linear_bin(a, N=15, offset=1, R=2.0):
    '''
    create a bin of length N
    map val A to range R
    offset one hot bin by offset, commonly R/2
    '''
    a = a + offset
    b = round(a / (R/(N-offset)))
    arr = np.zeros(N)
    b = clamp(b, 0, N - 1)
    arr[int(b)] = 1
    return arr


def linear_unbin(arr, N=15, offset=-1, R=2.0):
    '''
    preform inverse linear_bin, taking
    one hot encoded arr, and get max value
    rescale given R range and offset
    '''
    b = np.argmax(arr)
    a = b *(R/(N + offset)) + offset
    return a

throttle_commands = np.arange(0,1,0.05)

'''
Using R = 0.5 will create the following buckets and push all 
values greater that 0.50 into the last bucket
'''
for tc in throttle_commands:
    print ("throttle:",tc,"\t", linear_bin(tc,N=20,offset=0,R=.5))

'''
Adjusting offset to 1.0 seems to work better.
'''
print ('\nR adjusted to better span 0.0 to 1.0')
for tc in throttle_commands:
    print ("throttle:",tc,"\t", linear_bin(tc,N=20,offset=0,R=1.0))