Exp_04.py

# -*- coding: utf-8 -*-
"""
Created on Wed Jan 20 17:44:30 2021

@author: GSS-fearless
"""

import numpy as np
import time
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Circle
from matplotlib.lines import Line2D

pos = []  # position list
vel = []  # velocity list
abs_v1 = []
abs_v2 = []
abs_v3 = []
abs_v4 = []
abs_v5 = []
total_time = []
T_distance = []
rx, ry = [], []

v_max = 10  # maximum speed

# For attractive parameters
k1 = 0.5
k2 = 30
k3 = 60


ln = 1  # ob_radius
m = [2, 2, 2, 2, 2]  # mass
# parameters
i0 = 10  # obstacle_stimuli
a = 15
b = 5
y = 10
# boundary_stimuli
i0b = 100
lnb = 1
ab = 150
yb = 1000000
# boundary location
line1 = [(70, 20), (70, -40)]
line2 = [(70, -40), (0, -40)]
line3 = [(0, -40), (0, -50)]
line4 = [(0, -50), (-50, -50)]
line5 = [(-50, -50), (-50, 20)]
line6 = [(-50, 20), (-70, 20)]
line7 = [(-70, 20), (-70, 50)]
line8 = [(-70, 50), (60, 50)]
line9 = [(60, 50), (60, 20)]
line10 = [(60, 20), (70, 20)]

d = 10
l = 20
line11 = [(70 - d, 10), (70 - d, 10 - l)]
line12 = [(70-d,10-l),(-50+d,10-l)]
line13 = [(-50+d,10-l),(-50+d,10)]
line14 = [(-50+d,10),(70-d,10)]

line = [line1, line2, line3, line4, line5, line6, line7, line8, line9, line10, line11, line12, line13, line14]  # force given in clockwise
# line_direction = [(-1,0),(0,-1),(-1,0),(0,-1),(1,0),(0,1),(1,0),(0,-1),(-1,0),(0,-1),(1,0),(0,1),(-1,0),(0,1)]
line_direction = [(1, 0), (0, -1), (1, 0), (0, -1), (-1, 0), (0, -1), (-1, 0), (0, 1), (1, 0), (0, 1), (-1, 0), (0, 1),
                  (1, 0), (0, -1)]
pos.append([np.array([-40, -40])])  # initial position
vel.append([np.array([1, 0])])  # init speed = current direction, body
body = Circle(xy=(-40, -40), radius=1, fc='r', ec='r', alpha=1)
# object number M objects + point of interactions
ob = [body]  # object list
# print('ob',ob)
obr = 1.5  # relates to the target destination
tar = []
target_n = 1  # target position
v = 4 * np.random.random_sample((2,))
vel.append([v])
gx = 40
gy = 40
sx = -40
sy = -40
t1 = [gx, gy]
target = Circle(xy=t1, radius=obr, fc='g', ec='g', alpha=0.15)
tar.append(target)

t = [time.time()]  # time.time() will return the timestamp
tc = [0]  # sampling period


def count_time():
    t.append(time.time())
    tb = (t[-1] - t[-2])
    tc.append(tb)
    '''print('here is t:',t)
    print('here is tb:',tb)
    print('here is tc',tc)'''


def delta_v(j, i):  # trend of collision
    dr = get_direction(j, i)
    dd = 1
    # print('vel[j]',vel[j])
    vj = (vel[j][-1]) ** 2
    vj = (vj[0] + vj[1]) ** 0.5
    vjr = vel[j][-1]
    vi = (vel[i][-1]) ** 2
    vi = (vi[0] + vi[1]) ** 0.5
    vir = vel[i][-1]
    # print('vel[j][-1]',vel[j][-1])
    '''print('dr:',dr)
    print('vj:',vj)
    print('vjr:',vjr)
    print('vi:',vi)
    print('vir:',vir)'''
    cos_j = np.dot(dr, vjr) / (vj * dd)
    cos_i = np.dot(dr, vir) / (vi * dd)
    # print('this is cos_j',cos_j)
    if cos_j < 0:
        vjr = -vjr
    if cos_i > 0:
        vir = -vir
    va = vjr + vir
    return va


def delta_vb(j, i):
    dr = np.array(line_direction[i])
    dd = 1
    vj = (vel[j][-1]) ** 2
    vj = (vj[0] + vj[1]) ** 0.5
    vjr = vel[j][-1]
    cos_j = np.dot(dr, vjr) / (vj * dd)
    if cos_j < 0:
        vjr = -vjr
    va = vjr
    return va


def t_distance(j):
    # distance to the target
    distance = np.array(tar[j].get_center()) - np.array(ob[j].get_center())
    distance = distance ** 2
    distance = (distance[0] + distance[1]) ** 0.5
    # print('distance',distance)
    return distance  # always positive


def t_direction(j):
    direction = np.array(tar[j].get_center()) - np.array(ob[j].get_center())
    distance = direction ** 2
    distance = (distance[0] + distance[1]) ** 0.5
    direction = direction / (distance + 1)
    # print('direction', direction)
    return direction


def foot_point(j, i):
    """
    # Perpendicular foot from point_j to line_i (boundary)
    x0 = pos[j][-1][0]
    y0 = pos[j][-1][1]
    x1 = line[i][0][0]
    y1 = line[i][0][1]
    x2 = line[i][1][0]
    y2 = line[i][1][1]
    k = -((x1 - x0) * (x2 - x1) + (y1 - y0) * (y2 - y1)) / ((x2 - x1) ** 2 + (y2 - y1) ** 2)
    xn = k * (x2 - x1) + x1
    yn = k * (y2 - y1) + y1
    point = np.array([xn,yn])
    return point
    """

    x = 0
    y = 0
    x0 = pos[j][-1][0]
    y0 = pos[j][-1][1]
    x1 = line[i][0][0]
    y1 = line[i][0][1]
    x2 = line[i][1][0]
    y2 = line[i][1][1]
    if y1 == y2:
        if x2 > x1:
            if (x0 > x1) and (x0 < x2):
                x = x0
                y = y1
            elif x0 > x2:
                x = x2
                y = y1
            elif x0 < x1:
                x = x1
                y = y1
        if x1 > x2:
            if (x0 > x2) and (x0 < x1):
                x = x0
                y = y1
            elif x0 > x1:
                x = x1
                y = y1
            elif x0 < x2:
                x = x2
                y = y1
        point = np.array([x, y])  # output [x0,y1] when y1==y2
    else:
        if x1 == x2:
            if y2 > y1:
                if (y0 > y1) and (y0 < y2):
                    x = x1
                    y = y0
                elif y0 > y2:
                    x = x1
                    y = y2
                elif y0 < y1:
                    x = x1
                    y = y1
            if y1 > y2:
                if (y0 > y2) and (y0 < y1):
                    x = x1
                    y = y0
                elif y0 > y1:
                    x = x1
                    y = y1
                elif y0 < y2:
                    x = x1
                    y = y2
            point = np.array([x, y])  # output [x1,y0] when x1==x2
        else:
            point = np.array([0, 0])
    return point


def siep(j, i):
    tp = tc[-1]
    si = i0 + (a * ln + b * vel[i][-1] * tp) / (1 + np.exp(delta_v(j, i)) / y)
    delta_p = get_distance(j, i)
    f = si / delta_p
    return f
    # return force(scalar)


'''
def f_boundary(j,i):
    distance = (foot_point(j,i) - pos[j][-1]) ** 2
    f = i0b[i] + ab[i]/(distance + 1)
    print('distance',distance)
    if distance.any() < 200:
        f = f * 2
    else:
        if distance.all() > 1000:
            f = 0
    print('f_boundary',f)
    return f
'''


def siep_boundary(j, i):
    si = i0b + (ab * lnb) / (1 + np.exp(delta_vb(j, i)) / yb)
    distance = (foot_point(j, i) - pos[j][-1]) ** 2
    delta_p = (distance[0] + distance[1]) ** 0.5
    f = si / (delta_p + 1)

    '''
    if (distance[0] > 800) and (distance[1] > 800):
        f = 0
    else:
        if (distance[0] < 200) or (distance[1] < 200):
            f = 1.5 * f
    '''
    return f  # scalar


def controller(j):
    # stop by the target
    begin_time = time.time()
    td = t_distance(j)
    if td < 1:
        tp = np.array(tar[j].get_center())
        return tp
        # stimuli-induced force from ob
    f = np.array([0, 0])
    # stimuli-induced force from boundary
    for i in range(len(line)):
        f = f + np.array(line_direction[i]) * siep_boundary(j, i)
    # target attraction
    T_distance.append(t_distance(j))
    f_target = t_direction(j) * (k1 * t_distance(j) + k2 + k3/t_distance(j))
    f = f_target - f
    # acceleration
    acc = f / m[j]
    # print('acc',acc)
    tp = tc[-1]
    vr = vel[j][-1] + acc * tp
    vm = vr ** 2
    vm = (vm[0] + vm[1]) ** 0.5
    # speed limitation
    if vm <= v_max:
        vn = vr
    else:
        vn = vr / vm * v_max
    # print('vn:',vn)
    vel[j].append(vn)
    p = pos[j][-1] + vn * tp
    # print('this is pos[j][-1]',pos[j][-1])
    # print('this is pos[j][-1][0]',pos[j][-1][0])
    rx.append(p[0])
    ry.append(p[1])
    pos[j].append(p)
    abs_1 = (vel[0][-1][0] ** 2 + vel[0][-1][1] ** 2) ** 0.5
    abs_v1.append(abs_1)

    end_time = time.time()
    # run_time = end_time - begin_time
    # total_time.append(run_time)
    print('Run_time:', end_time - begin_time)
    return p  # return current position


def animate():
    fig, ax1 = plt.subplots(1, 1, figsize=(12, 9))

    def init():
        ax1.set_xlim([-80, 80])
        ax1.set_ylim([-60, 60])
        ax1.set_title("SIEP control scenario", size=20)
        # add lines
        (line1_xs, line1_ys) = zip(*line1)  # zip(*) means unzip
        (line2_xs, line2_ys) = zip(*line2)
        (line3_xs, line3_ys) = zip(*line3)
        (line4_xs, line4_ys) = zip(*line4)
        (line5_xs, line5_ys) = zip(*line5)  # zip(*) means unzip
        (line6_xs, line6_ys) = zip(*line6)
        (line7_xs, line7_ys) = zip(*line7)
        (line8_xs, line8_ys) = zip(*line8)
        (line9_xs, line9_ys) = zip(*line9)
        (line10_xs, line10_ys) = zip(*line10)
        (line11_xs, line11_ys) = zip(*line11)
        (line12_xs, line12_ys) = zip(*line12)
        (line13_xs, line13_ys) = zip(*line13)
        (line14_xs, line14_ys) = zip(*line14)


        '''print('line1_xs',line1_xs)
        print('line1_ys',line1_ys)'''
        lw = 2
        ax1.add_line(Line2D(line1_xs, line1_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line2_xs, line2_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line3_xs, line3_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line4_xs, line4_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line5_xs, line5_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line6_xs, line6_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line7_xs, line7_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line8_xs, line8_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line9_xs, line9_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line10_xs, line10_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line11_xs, line11_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line12_xs, line12_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line13_xs, line13_ys, linewidth=lw, color='g'))
        ax1.add_line(Line2D(line14_xs, line14_ys, linewidth=lw, color='g'))

        for i in range(len(ob)):
            ax1.add_artist(ob[i])
        for j in range(len(tar)):
            ax1.add_artist(tar[j])

    def draw(n):
        # sampling
        count_time()
        for j in range(len(ob)):
            npo = controller(j)
            ob[j].set_center(npo)
            # print('This is ob[j]',ob[j])

    ani = FuncAnimation(fig, draw, init_func=init, frames=1000, interval=15, blit=False)
    # ani.save('/home/ryan/SIEP_control.gif',writer='pillow',fps=30)
    plt.show()
    T1 = np.linspace(1, len(abs_v1) * 100, len(abs_v1))
    plt.scatter(T1, abs_v1, s=5)
    plt.title('Robot velocity')
    plt.xlabel('Time')
    plt.ylabel('Velocity')
    plt.show()
    T2 = np.linspace(1, len(T_distance) * 100, len(T_distance))
    plt.scatter(T2, T_distance, color='orange', s=5)
    plt.title('Robot-target distance')
    plt.xlabel('Time')
    plt.ylabel('Distance')
    plt.show()

def show():
    fig, ax1 = plt.subplots(1, 1, figsize=(12, 9))
    ax1.set_xlim([-80, 80])
    ax1.set_ylim([-60, 60])
    ax1.set_title("SIEP control scenario", size=20)
    # add lines
    (line1_xs, line1_ys) = zip(*line1)  # zip(*) means unzip
    (line2_xs, line2_ys) = zip(*line2)
    (line3_xs, line3_ys) = zip(*line3)
    (line4_xs, line4_ys) = zip(*line4)
    (line5_xs, line5_ys) = zip(*line5)  # zip(*) means unzip
    (line6_xs, line6_ys) = zip(*line6)
    (line7_xs, line7_ys) = zip(*line7)
    (line8_xs, line8_ys) = zip(*line8)
    (line9_xs, line9_ys) = zip(*line9)
    (line10_xs, line10_ys) = zip(*line10)
    (line11_xs, line11_ys) = zip(*line11)
    (line12_xs, line12_ys) = zip(*line12)
    (line13_xs, line13_ys) = zip(*line13)
    (line14_xs, line14_ys) = zip(*line14)

    lw = 2
    ax1.add_line(Line2D(line1_xs, line1_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line2_xs, line2_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line3_xs, line3_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line4_xs, line4_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line5_xs, line5_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line6_xs, line6_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line7_xs, line7_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line8_xs, line8_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line9_xs, line9_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line10_xs, line10_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line11_xs, line11_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line12_xs, line12_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line13_xs, line13_ys, linewidth=lw, color='g'))
    ax1.add_line(Line2D(line14_xs, line14_ys, linewidth=lw, color='g'))

    plt.plot(gx,gy,'xb')
    plt.plot(sx,sy,'og')
    plt.plot(rx,ry,'-r')

    plt.show()

animate()
show()