clear; clc;
% add genomix-matlab path
setenv('ROBOTPKG_BASE','/home/ayham/ut-tools/openrobots')
addpath(strcat(getenv('ROBOTPKG_BASE'),'/lib/matlab'));
addpath(strcat(getenv('ROBOTPKG_BASE'),'/lib/matlab/simulink'));
addpath(strcat(getenv('ROBOTPKG_BASE'),'/lib/matlab/simulink/genomix'));
ft_sens_plugin_path = '/lib/genom/pocolibs/plugins/';

host = 'localhost';

% Initialize GenoM stuff and connect the ports %
% --------------------------------------------------


disp('Initialize genom components for simulation');
client = genomix.client(host);

disp('* Initialize ft-sensor');
ftsens = client.load(strcat(ft_sens_plugin_path,'ftsens'));
pause(1);
result_ft = ftsens.connect('/tmp/can', 1000);
stringmsg = ['Connecting to ftsens: ',result_ft.status];
disp(stringmsg);
result_ft = ftsens.set_zero(5000);
stringmsg = ['Zeroing the FT-sensor: ',result_ft.status];
disp(stringmsg);
result_ft = ftsens.wrench();
stringmsg = ['FT-sensor reading: ',num2str(result_ft.wrench.force.x),...
    ', ', num2str(result_ft.wrench.force.y), ', ', num2str(result_ft.wrench.force.z)];
disp(stringmsg);