TruckBackerCtrl

TruckBackerCtrl.py

import math;
#import tkinter as tk
import matplotlib.pyplot as plt;
import numpy as np;
import random

from tkinter import *
import matplotlib
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
from matplotlib.figure import Figure

x_list = [];# time 
y_list = [];# time 
Rule_table = [  'S2','S3',  0 ,  0 ,  0 ,
                'S2','S3','S3','S3',  0 ,
                'B1','S1','S2','S3','S2',
                'B2','B2','CE','S2','S2',
                'B2','B3','B2','B1','S1',
                  0 ,'B3','B3','B3','B2',
                  0 ,  0 ,  0 ,'B3','B2',
                ];
phi_ulist = [0]*7;
x_ulist = [0]*5;

last_x_pos = 0;
last_y_pos = 0;
#last_phi = 30;

curr_theta = 0;
curr_phi  = 0;
b = 4;# length of the truck

#init
def Init_tlist():
    for i in range(0,181,1):
        t_list = i;


def math_cos(angle):

    if(angle != 180): 
        angle = angle%180;
    
    angle = angle*math.pi/180;
    if(abs(angle - math.pi/2) < 0.0001 ):
        return 0;
    else:
        return math.cos(angle);

def math_sin(angle):
    if(angle >= 360): angle = angle -360;
    angle = angle*math.pi/180;
    if(abs(angle - math.pi) < 0.0001 ):
        return 0;
    else:
        return math.sin(angle);

#formula
def x_trajectory(phi,theta):
    global last_x_pos;
    
    result = last_x_pos + math_cos(phi+theta) + math_sin(theta)*math_sin(phi);
    last_x_pos = result;
    return result;
    

def y_trajectory(phi,theta):
    global last_y_pos;
    theta   = theta*math.pi/180;
    phi     = phi*math.pi/180;
    result = last_y_pos + math_sin(phi+theta) + math_sin(theta)*math_cos(phi);
    last_y_pos = result;
    return result;

def new_phi_output(cur_phi,theta):
    #if(cur_phi%180 == 0): cur_phi = 0;
    if(theta%180 == 0): theta = 0;
    cur_phi = cur_phi*math.pi/180;
    theta = theta*math.pi/180;
    
    result = cur_phi - math.asin(2*math.sin(theta)/b);
    result = result*180/math.pi;
    if(result < -90 ):
        return 90;
    elif(result > 270):
        return 270;
    else:
        return result;


#fuzzy rule
def Triangular_rule(x,a,b):
    m = (a+b)/2;
    a = m-a;
    b = b-m;
    if( x >= m - a and x <= m ):
        return 1 + (x-m)/a;
    elif(x > m and x <= m + b):
        return 1 + (m-x)/b;
    else:
        return 0;
def Trapezood_rule(x,a,m,mode):
    
    if(mode == 'Left'):
        slope = 1/(m-a);
        if( x > m ):
            return 1;
        elif(x > a and x < m ):
            return  slope*(m-x);
        else:
            return 0;
    elif(mode == 'Right'):
        slope = -1/(m-a);
        if( x > m  ):
            return 0;
        elif(x > a and x < m ):
            return slope*(m-x);
        else:
            return 1;
    else:
        print('mode is nono');
        return 0;
        
def BackTriangular_rule(y,a,b):
    return (a+b)/2;
def BackHalfTriangular_rule(y,a,b,mode):
    if(mode == 'Left'):
        x = b - (b-a)*y;                
    elif(mode == 'Right'):
        x = a + (b-a)*y;
    else:
        return 0;
    return (a+x)/2;

def phi_rule(type,x):
    return {
        'S3' : lambda x: Triangular_rule(x,-115,-15),
        'S2' : lambda x: Triangular_rule(x,-45,45),
        'S1' : lambda x: Triangular_rule(x,15,90),
        'CE' : lambda x: Triangular_rule(x,80,100),
        'B1' : lambda x: Triangular_rule(x,90,165),
        'B2' : lambda x: Triangular_rule(x,135,225),
        'B3' : lambda x: Triangular_rule(x,195,295),
    }.get(type)(x)

def x_rule(type,x):
    return {
        'S2' : lambda x: Trapezood_rule(x,1.5,7,'Right'),
        'S1' : lambda x: Triangular_rule(x,4,10),
        'CE' : lambda x: Triangular_rule(x,9,11),
        'B1' : lambda x: Triangular_rule(x,10,16),
        'B2' : lambda x: Trapezood_rule(x,13,18.5,'Left'),
    }.get(type)(x)

def Out_rule(type,y):
    return {
        'S3' : lambda y: BackHalfTriangular_rule(y,-40,-20,'Left'),#NL
        'S2' : lambda y: BackTriangular_rule(y,-33,-7),#S2
        'S1' : lambda y: BackTriangular_rule(y,-14, 0),#S1
        'CE' : lambda y: BackTriangular_rule(y, -4, 4),#CE
        'B1' : lambda y: BackTriangular_rule(y,  0,14),#B1
        'B2' : lambda y: BackTriangular_rule(y,  7,33),#B2
        'B3' : lambda y: BackHalfTriangular_rule(y,20,40,'Right'),#B3
        0 : lambda y:  0,  

    }.get(type)(y)

    
    

def inference(phi,x):    

    phi_ulist[0] = phi_rule('S3',phi);
    phi_ulist[1] = phi_rule('S2',phi);
    phi_ulist[2] = phi_rule('S1',phi);
    phi_ulist[3] = phi_rule('CE',phi);
    phi_ulist[4] = phi_rule('B1',phi);
    phi_ulist[5] = phi_rule('B2',phi);
    phi_ulist[6] = phi_rule('B3',phi);
    

    '''
    for i in range(0,7,1):
        if(phi_ulist[i] > 0 ):
            print('inf phi',i,phi_ulist[i]);
    '''
    x_ulist[4] = x_rule('B2',x);
    x_ulist[3] = x_rule('B1',x);
    x_ulist[2] = x_rule('CE',x);
    x_ulist[1] = x_rule('S1',x);
    x_ulist[0] = x_rule('S2',x);

def RuleTable(k,y):    
    weight = Out_rule(Rule_table[k],y);
    return weight;    

def LastOutput():
    last_ulist = [0]*35;
    w_sum = 0;
    s_sum = 0
    k = 0;
    for i in range(0,7,1):
        for j in range(0,5,1):
            last_ulist[k] = min(phi_ulist[i],x_ulist[j]);
            #if(last_ulist[k] > 0.01):
            #    print('k',k,last_ulist[k]);
            k = k + 1;

    for k in range(0,35,1):
        w_sum += RuleTable(k,last_ulist[k])*last_ulist[k];

    for k in range(0,35,1):
        s_sum += last_ulist[k];
    if(s_sum == 0):
        print('s_sum == 0');
        return 0;
    if(w_sum == 0):
        print('w_sum == 0');
        return 0;
    output_value = w_sum /s_sum ;
    if(output_value < -40):
        return -40;
    elif(output_value > 40):
        return 40;
    else:
        #print('w_sum: ',w_sum,'sum: ',s_sum,'output_value',output_value);
        return output_value;




def main_iteration(input_x,input_phi):
    #main init 
    global curr_theta;
    global x_list;
    global y_list;

    global last_x_pos ;
    global last_y_pos ;
    global curr_phi  ;

    last_x_pos = 0;
    last_y_pos = 0;
    curr_theta = 0;
    curr_phi  = 0;

    last_x_pos = input_x;
    
    curr_phi  = input_phi;
    del x_list[:] ;# time 
    del y_list[:];# time 
    x_list = [0]*180;# time 
    y_list = [0]*180;# time 

    #print('input2',last_x_pos,curr_phi,curr_theta);
    if(last_x_pos < 0):
        last_x_pos = 0;
    elif(last_x_pos > 20):
        last_x_pos = 20;

    if(curr_phi < -90):
        curr_phi = -90;
    elif(curr_phi > 270):
        curr_phi = 270;

    #main iteration
    for t in range(0,180,1):
        x_list[t]   = x_trajectory(curr_phi,curr_theta);
        curr_phi    = new_phi_output(curr_phi,curr_theta);
        y_list[t]   = y_trajectory(curr_phi,curr_theta);
        

        if(abs(x_list[t] - 10)< 0.05 and abs(curr_phi - 90) < 0.1 ):
            print('break!');
            x_list = x_list[:t];
            y_list = y_list[:t];
            break;
        else:
            inference(curr_phi,x_list[t]);
            curr_theta = LastOutput();            
            print('t:',t,'x: ',x_list[t],'y: ',y_list[t],'phi: ',curr_phi,'theta: ',curr_theta);
            pass;



def drawPic():
    """
    获取GUI界面设置的参数，利用该参数绘制图片
    """
       
    #获取GUI界面上的参数      
    try:
        X_POS = float(inputEntry_x.get());
        PHI_ANGLE = float(inputEntry_phi.get());
    except:
        #X_POS = 10;
        #PHI_ANGLE = 180;
        X_POS = np.random.uniform(0,20);
        PHI_ANGLE = np.random.uniform(-90,270);
        print('please input number');
       
       #清空图像，以使得前后两次绘制的图像不会重叠
    drawPic.f.clf()
    drawPic.a=drawPic.f.add_subplot(111)
       

    print('input1',X_POS,PHI_ANGLE);
    main_iteration(X_POS,PHI_ANGLE);
    
    color=['b','r','y','g']
       
    drawPic.a.plot(x_list,y_list,'-.',color=color[np.random.randint(len(color))]);
    drawPic.a.set_title('Demo: Draw Truck Backer Control')
    drawPic.canvas.draw()
       


if __name__ == '__main__':
       
       matplotlib.use('TkAgg')
       root=Tk()
       root.title("TruckBackCtrl Simulate")
       #在Tk的GUI上放置一个画布，并用.grid()来调整布局
       drawPic.f = Figure(figsize=(5,4), dpi=100) 
       drawPic.canvas = FigureCanvasTkAgg(drawPic.f, master=root)
       drawPic.canvas.draw();
       drawPic.canvas.get_tk_widget().grid(row=0, columnspan=3)    

       drawPic();
       #放置标签、文本框和按钮等部件，并设置文本框的默认值和按钮的事件函数
       Label(root,text='input x ').grid(row=1,column=0)
       Label(root,text='input phi ').grid(row=1,column=1)

       inputEntry_x   = Entry(root);
       inputEntry_x.grid(row=2,column=0);
       inputEntry_phi = Entry(root);
       inputEntry_phi.grid(row=2,column=1);

       Button(root,text='simulate',command=drawPic).grid(row=2,column=2,columnspan=3)
       
       #启动事件循环
       root.mainloop()