append_primitives.py

# COMBINE MULTIPLE MOTOR PRIMITIVES TOGETHER --------------------------
import numpy as np
import agent
import simulation
import motorPrimitive
import matplotlib.pyplot as plt

# KEY OF ALL SIMULATION PARAMETERS (REFERENCE MAIN_NOTE.TXT FOR EFFECTIVE COMBINATIONS)
## CONTROL VARIABLES ---------------------------------------------------
### INITIAL PHI, (X,Y) LOC FOR FIRST MOTOR PRIMITIVE
start_x = 0
start_y = 0
start_phi = 0
## ALL MOTOR PRIMITIVE PARAMETERS --------------------------------------
### TOTAL TIME, TIME-STEP
timeLength = [100,100]
timeStep = [0.1,0.1]
velocity = [1,1]
gain = [1,1]
### SHAPE, AMPLITUDE, TOTAL_LIST
function = ['gaussian','gaussian']
amplitude = [1,1]
total_num = 2

# FUNCTION THAT ASSIGNS PARAMETERS FOR TIMELENGTH, TIMESTEP, FUNCTION, AMPLITUDE, VELOCITY, GAIN
def init_motorPrim(total_num,timeLength,timeStep,function,amplitude,velocity,gain):
    MP_list = []
    for i in range(0,total_num):
        new_MP = motorPrimitive.MotorPrimitive(timeLength[i],timeStep[i],function[i],amplitude[i],velocity[i], gain[i])
        MP_list.append(new_MP)
    return(MP_list)

MP_list = init_motorPrim(total_num,timeLength,timeStep,function,amplitude,velocity,gain)

# FUNCTION THAT GENERATES X,Y, AND PHI ARRAYS FOR ALL MOTOR PRIMITIVES---------------------------------
def generate_XYPHI(total_num,MP_list):
    for i in range(0,total_num):
        current_MP = MP_list[i]
        t = np.arange(0,current_MP.timeLength+current_MP.timeStep, current_MP.timeStep)
        
        current_MP.x = np.zeros(len(t))   
        current_MP.y = np.zeros(len(t))
        current_MP.phi = np.zeros(len(t))
        print(current_MP.x,current_MP.y,current_MP.phi)
    return(MP_list)

MP_list1 = generate_XYPHI(total_num,MP_list)

# FUNCTION THAT SETS START_X, START_Y, AND START_PHI FOR FIRST MOTOR PRIMITIVE ONLY 
def firstMP_init_xyphi(MP_list, start_x, start_y, start_phi):
    MP1 = MP_list[0]

    MP1.start_x = start_x
    MP1.start_y = start_y
    MP1.start_phi = start_phi

    MP1.x[0] = start_x
    MP1.y[0] = start_y
    MP1.phi[0] = start_phi
    return(MP1)

MP1 = firstMP_init_xyphi(MP_list1, start_x, start_y, start_phi)
MP2 = MP_list1[1]
motorPrimitive.MotorPrimitive.MP_parameters(MP2)

# APPLY INIT_FUNC TO ALL MOTOR PRIMITIVES IN THE LIST---------------------------------------------------
MP1_f,MP1_t,MP1_alist,MP1_func = motorPrimitive.MotorPrimitive.init_func(MP1,mu=30,sigma=10) #FOR FIRST MOTOR PRIMITIVE
MP2_f,MP2_t,MP2_alist,MP2_func = motorPrimitive.MotorPrimitive.init_func(MP2,mu=30,sigma=10)

# POPULATE X,Y,PHI FOR FIRST MOTOR PRIMITIVE ONLY (REST ARE CALCULATED LATER)---------------------------
MP1_x,MP1_y,MP1_phi = simulation.populate_xyphi(MP1,MP1_f,MP1_t)

# SET INITIAL X,Y,PHI FOR THE REST OF THE MOTOR PRIMITIVES ----------------------------------------------------
def allMPs_init_xyphi(MP_list,MP_num, prevMP_x,prevMP_y,prevMP_phi):
    current_MP = MP_list[MP_num - 1]
    current_MP.start_x = prevMP_x[(len(prevMP_x))-1]
    current_MP.start_y = prevMP_y[(len(prevMP_y))-1]
    current_MP.start_phi = prevMP_phi[(len(prevMP_phi))-1]

    current_MP.x[0] = current_MP.start_x
    current_MP.y[0] = current_MP.start_y
    current_MP.phi[0] = current_MP.start_phi
    return(current_MP.x[0],current_MP.y[0],current_MP.phi[0])


MP2.start_x, MP2.start_y, MP2.start_phi = allMPs_init_xyphi(MP_list1,2,MP1_x,MP1_y,MP1_phi)
print(MP2.start_y)

# POPULATE THE REST OF THE MOTOR PRIMITIVE XYPHI
MP2_x,MP2_y,MP2_phi = simulation.populate_xyphi(MP2,MP2_f,MP2_t)

# COMBINE ALL MOTOR PRIMITIVES
def combine_MPs(MP_list):
    final_x = []
    final_y = []

    MP1 = MP_list[0]
    MP2 = MP_list[1]

    final_x = np.concatenate((MP1.x, MP2.x))
    final_y = np.concatenate((MP1.y, MP2.y))

    return(final_x,final_y)

final_x,final_y = combine_MPs(MP_list1)
# plt.plot(final_x,final_y)
# plt.show()


# TEST CODE ----------------------------------------------------------------------