def Eul2DCM(ang, axis, deg):
if deg:
angs = r(ang)
else:
angs = ang
MatRes = np.eye(3)
for n, th in enumerate(angs):
if axis[n] == 1:
MatInt = np.array([[1, 0, 0], [0, c(th), s(th)],
[0, -s(th), c(th)]])
elif axis[n] == 2:
MatInt = np.array([[c(th), 0, -s(th)], [0, 1, 0],
[s(th), 0, c(th)]])
else:
MatInt = np.array([[c(th), s(th), 0], [-s(th), c(th), 0],
[0, 0, 1]])
MatRes = np.dot(MatInt, MatRes)
return MatRes
def z_end(theta):
'''Function to return z_value of position vector
------------------------------------------------
theta: List[3]= r(theta[2]), r(theta[3]), theta4 List of theta angles
alpha: List[4]= alpha1, alpha2, alpha3, alpha4 List of alpha angles
length: List[4] = length1, length2, length3, length4 List of joint length'''
z = length[0] + length[1] * cos(r(theta[1])) - (
cos(r(theta[1])) * sin(r(theta[2])) +
cos(r(theta[2])) * sin(r(theta[1]))
) * (length[3] * cos(r(theta[3])) - length[4] * sin(r(theta[3]))) - (
cos(r(theta[1])) * cos(r(theta[2])) -
sin(r(theta[1])) * sin(r(theta[2]))) * (
length[4] * cos(r(theta[3])) +
length[3] * sin(r(theta[3]))) - length[2] * cos(r(theta[1])) * sin(
r(theta[2])) - length[2] * cos(r(theta[2])) * sin(r(theta[1]))
return z
def y_end(theta):
''' Function to return x-value of position vector
-------------------------------------------------
theta: List[4]= theta[0], theta[1], theta[2], theta[3] List of theta angles
length: List[4] = length1, length2, length3, length4 List of joint length'''
y = length[1] * sin(r(theta[0])) * sin(r(theta[1])) - (
sin(r(theta[0])) * sin(r(theta[1])) * sin(r(theta[2])) -
cos(r(theta[1])) * cos(r(theta[2])) * sin(r(theta[0]))
) * (length[3] * cos(r(theta[3])) - length[4] * sin(r(theta[3]))) - (
cos(r(theta[1])) * sin(r(theta[0])) * sin(r(theta[2])) +
cos(r(theta[2])) * sin(r(theta[0])) * sin(r(theta[1]))) * (
length[4] * cos(r(theta[3])) +
length[3] * sin(r(theta[3]))) + length[2] * cos(r(theta[1])) * cos(
r(theta[2])) * sin(r(theta[0])) - length[2] * sin(r(
theta[0])) * sin(r(theta[1])) * sin(r(theta[2]))
return y
def extract_data_and_save_to_file(labels_array='',
ignored_indices='',
dataset='',
motion_class='',
dataset_path='',
current_file_name=''):
# variable to store all the segments and vectors values
data = np.empty((1, 1))
for vector in motion_class.vectorsUsed:
v_data = dataset[vector]
if 'joint' == vector:
for joints in motion_class.jointUsed:
sensor_data = v_data[0][0][joints][0][0][2:]
number_row, number_column = sensor_data.shape
_, ds_column = data.shape
# temporary array
temp_array = sensor_data
# if ds_column is 1 it is the first iteration and special measures have
# to be taken into consideration when specifying the size of the array if not
# check this condition, then the code would break trying to add the data
if ds_column != 1:
# create new array with extra index for new data
temp_array = np.zeros(
(number_row, number_column + ds_column))
# merge data
temp_array[:, 0:ds_column] = data
temp_array[:, ds_column:] = sensor_data
# add values to the final variable
data = np.vstack(temp_array)
else:
for segments in motion_class.segmentUsed:
# obtains the values based on the segments and vectors used
sensor_data = v_data[0][0][segments][0][0][2:]
number_row, number_column = sensor_data.shape
_, ds_column = data.shape
# temporary array
temp_array = sensor_data
# if ds_column is 1 it is the first iteration and special measures have
# to be taken into consideration when specifying the size of the array if not
# check this condition, then the code would break trying to add the data
if ds_column != 1:
# create new array with extra index for new data
temp_array = np.zeros(
(number_row, number_column + ds_column))
# merge data
temp_array[:, 0:ds_column] = data
temp_array[:, ds_column:] = sensor_data
# add values to the final variable
data = np.vstack(temp_array)
import IPython
IPython.embed()
# merge data with their respective labels
tmp_arr = ''
try:
printout(message='\tMerging data and labels arrays', verbose=True)
tmp_arr = np.c_[data, labels_array]
except ValueError:
msg = '\tsize of data: {0}'.format(np.shape(data))
printout(message=msg, verbose=True)
msg = '\tsize of labels: {0}'.format(np.shape(labels_array))
printout(message=msg, verbose=True, extraspaces=2)
exit(1)
if len(ignored_indices) != 0:
printout(message='\tRemoving \'Ignored\' labels', verbose=True)
data_labels = remove_ignores(tmp_arr, ignored_indices)
else:
data_labels = tmp_arr
# this information will be used to train the hmm since its important to know the start and end of
# an activity in order for the EM algo to not learn from non-concurrent activities
n_datapoints = np.shape(data_labels)[0]
array_length = np.zeros([n_datapoints, 1])
array_length[0, 0] = n_datapoints
dataset = np.c_[data_labels, array_length]
# current user and activity based on the file name
user, activity, leftright = file_information(current_file_name)
# list of files already processed
files_processed = [
pfile for pfile in listdir(dataset_path)
if isfile(join(dataset_path, pfile))
]
# list of user already processed
users_processed = [
pfile for pfile in files_processed
if (user in pfile and activity in pfile)
]
# concatenate users performing the same activities
for uprocessed in users_processed:
old_data = np.load(uprocessed)
tmp_arr = np.r(old_data, dataset)
dataset = tmp_arr
new_file_name = user + '_' + leftright + '_' + activity
current_out_path = os.path.join(dataset_path, new_file_name)
msg = '\tOutput file directory: {0}'.format(current_out_path)
printout(message=msg, verbose=True)
np.save(current_out_path, dataset)
import numpy as np
from numpy import deg2rad as r
from numpy import rad2deg as d
from numpy import cos as c
from numpy import sin as s
def matTrans(thetArr):
t1 = thetArr[0]
t2 = thetArr[1]
t3 = thetArr[2]
matRes = np.array([[0, s(t3), c(t3)], [0, c(t3) * c(t2), -s(t3) * c(t2)],
[c(t2), s(t2) * s(t3),
c(t3) * s(t2)]])
return matRes
thet = np.array([[r(40), r(30), r(80)]])
for i in range(0, 600):
tempo = i / 10
omega = np.array(
[r(20 * s(0.1 * tempo)),
r(20 * 0.01),
r(20 * c(0.1 * tempo))])
matt = matTrans(thet[-1])
new_thet_p = np.dot(matt, omega)
new_thet = thet[-1] + new_thet_p * 0.1
thet = np.append(thet, [new_thet], axis=0)
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 19 21:22:19 2018
@author: lele
"""
import numpy as np
from numpy import deg2rad as r
from numpy import rad2deg as d
from numpy import cos as c
from numpy import sin as s
from DCMtoEP import EPrates
bet = np.array([[0.408248, 0, 0.408248, 0.816497]])
for i in range(0, 6000):
tempo = i / 100
omega = np.array(
[r(20 * s(0.1 * tempo)),
r(20 * 0.01),
r(20 * c(0.1 * tempo))])
new_bet_p = EPrates(bet[-1], omega)
new_bet = bet[-1] + new_bet_p * 0.01
bet = np.append(bet, [new_bet], axis=0)
