combine_motorPrimitives.py

# COMBINE MULTIPLE MOTOR PRIMITIVES TOGETHER --------------------------
import numpy as np
import agent
import simulation
import motorPrimitive
import templates
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(ith,timeLength,timeStep,function,amplitude,velocity,gain):
    MP = motorPrimitive.MotorPrimitive(timeLength[ith],timeStep[ith],function[ith],amplitude[ith],velocity[ith], gain[ith])
    return(MP)

# FUNCTION THAT GENERATES X,Y, AND PHI ARRAYS -----------------------------------------------------------------
def generate_XYPHI_arrays(MP):
    t = np.arange(0,MP.timeLength+MP.timeStep, MP.timeStep)
    MP.x = np.zeros(len(t))   
    MP.y = np.zeros(len(t))
    MP.phi = np.zeros(len(t))
    print(MP.x,MP.y,MP.phi)
    return(MP)


# FUNCTION THAT SETS START_X, START_Y, AND START_PHI FOR FIRST MOTOR PRIMITIVE ONLY --------------------------
def firstMP_init_xyphi_values(MP1, start_x, start_y, start_phi):
    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)

# COMBINE MOTOR PRIMITIVES -------------------------------------------------------------------------------------
def sum_MPs(MP1_f,MP1_alist, MP2_f,MP2_alist):
    mp1 = MP1_f(MP1_alist,MP1_alist)
    mp2 = MP2_f(MP2_alist,MP2_alist)
    final_mp = mp1+ mp2
    plt.plot(final_mp)
    plt.show()
    return(final_mp)


# SET INITIAL X,Y,PHI FOR THE REST OF THE MOTOR PRIMITIVES ----------------------------------------------------
def allMPs_init_xyphi(MP,prevMP_x,prevMP_y,prevMP_phi):
    current_MP = MP
    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])


# TEST ---------------------------------------------------------------------------------------

## Generate MP1
# initialize MP
MP1 = init_motorPrim(0,timeLength,timeStep,function,amplitude,velocity,gain)

# define X,Y, and Phi arrays
generate_XYPHI_arrays(MP1)

# define initial X,Y, and Phi values
firstMP_init_xyphi_values(MP1, start_x, start_y, start_phi)

# define MP shape and parameter values
MP1_f,MP1_t,MP1_alist,MP1_func = motorPrimitive.MotorPrimitive.init_func(MP1,mu=30,sigma=10) #FOR FIRST MOTOR PRIMITIVE

# fill in X,Y,Phi arrays
MP1_x,MP1_y,MP1_phi = simulation.populate_xyphi(MP1,MP1_f,MP1_t)

# -----------------------------------------------------------------------------------------

## Generate MP2
# initialize MP
MP2 = init_motorPrim(1,timeLength,timeStep,function,amplitude,velocity,gain)

# define X,Y, and Phi arrays
generate_XYPHI_arrays(MP2)

# define MP2 shape and parameter values
MP2_f,MP2_t,MP2_alist,MP2_func = motorPrimitive.MotorPrimitive.init_func(MP2,mu=80,sigma=10)

# sum MP1 and MP2 functions
final_MP = sum_MPs(MP1_f,MP1_alist, MP2_f,MP2_alist)

# # define initial X,Y, and Phi values
# allMPs_init_xyphi(MP2,MP1_x,MP1_y,MP1_phi)
# # fill in X,Y,Phi arrays
#MP2_x,MP2_y,MP2_phi = simulation.populate_xyphi(MP1,final_MP,MP2_t)

# a = MP2_f(MP2_alist,MP2_alist)

# plt.plot(a)
# plt.show()