def convert_tf_to_csv(yalm_file_dir, filename, s_grasp):
"""
Read the tf yaml file, calculate transforms for links, create a header for the file and save it to a csv
:param yalm_file_dir: directory of the yaml file
:param filename: name of the yaml file
:param s_grasp: successful grasp value
:return: None - create a csv file and saves the information to it
"""
# header
header = get_header(filename)
# read file
yalm_object = open(yalm_file_dir)
print 'reading {0}'.format(filename)
# variable to keep all the data points
dataset = np.empty(shape=(0, 0), dtype='object')
# binary flag used to know when to start collecting data
val_flag = False
# variable used to keep all the information for the row of the file
data_point = np.zeros(shape=(1, 9), dtype='object')
# keep track of initial time
time_step = list()
for row_index, line in enumerate(yalm_object):
# if reading on line for link2, turn on flag in order to start collecting data
if 'child_frame_id: staubli_rx60l_link2' in line:
transform_data = Transform()
val_flag = True
elif 'secs' in line and val_flag:
val = re.search('\s*secs:\s(.*)\\n', line).group(1)
time_step.append(val)
if len(time_step) == 2:
# switch the nsecs with the secs values in order to get the right datetime
time_step[0], time_step[1] = time_step[1], time_step[0]
timestep = ''.join(time_step)
# seconds have to go before all the other fingers and palm values
data_point[0, 0] = Decimal(timestep)
elif ('w:' in line or 'x:' in line or 'y:' in line
or 'z:' in line) and val_flag:
# extract and collect the sensor value
val = Decimal(re.search('\s *.:\s(.*)\\n', line).group(1))
transform_data.add(val)
# check if all the necessary values have been added
if transform_data.all_data_included():
tmp_data_point = list(
transform_data.get_rotation_translation())
data_point[0, 1:-1] = np.array(tmp_data_point, dtype='object')
data_point[0, -1] = s_grasp
dataset = append_to_dataset(data_matrix=dataset,
r_data=data_point,
filename=filename,
n_row=row_index,
n_cols=9)
# reset variables
val_flag = False
time_step = list()
data_point = np.zeros(shape=(1, 9), dtype='object')
transform_data = Transform()
return header, dataset
