Package fr3_impedance_controller_real in nix env

Including all deps. Tested with nix develop then roslaunch franka_example_controllers ....

Issue with libfranka version, in the end needed to use fetchGithub in Nix to get proper files.

Closes #7 (closed)

assets/data/matlab/experiment_data_analysis.m

+%% load data
+clc; close all; clear;
+
+ctrl_state_file = '../rosbag/250208_contact_02_controller_state.csv';
+ctrl_state_data_table = readtable(ctrl_state_file);
+ctrl_state_data_array = table2array(ctrl_state_data_table);
+ctrl_state_data_array(:,2:4) = [];
+
+ft_sensor_file = '../rosbag/250208_contact_02_ft_sensor.csv';
+ft_sensor_data_table = readtable(ft_sensor_file);
+ft_sensor_data_array = table2array(ft_sensor_data_table);
+ft_sensor_data_array(:, 2:22) = [];
+
+%%
+vec_size = [1, 7, 7, 7, 7, 7, 7, 6, 6, 6, 6, 7, 7, 6, 6, 6, 6, 6];
+start_idx = [1, ];
+
+for i = 1:length(vec_size) - 1
+    start_idx(i+1) = start_idx(i) + vec_size(i);
+end
+
+data_cell = cell(1, length(vec_size));
+
+for i = 1:length(vec_size)
+    data_cell{i} = ctrl_state_data_array(:, start_idx(i):start_idx(i) + vec_size(i) - 1);
+end
+
+t_ctrl = data_cell{1};
+ddq_eul= data_cell{2};
+ddq_mob = data_cell{3};
+ddq_sosml = data_cell{4};
+tau_d_dob = data_cell{5};
+tau_d_mob = data_cell{6};
+tau_d_sosml = data_cell{7};
+F_d_fr3 = data_cell{8};
+F_d_dob = data_cell{9};
+F_d_mob = data_cell{10};
+F_d_sosml = data_cell{11};
+ee_pose_des = data_cell{12};
+ee_pose_mes = data_cell{13};
+ee_vel_des = data_cell{14};
+ee_vel_mes = data_cell{15};
+ee_acc_eul = data_cell{16};
+ee_acc_mob = data_cell{17};
+ee_acc_sosml = data_cell{18};
+
+%%
+
+
+offset = ft_sensor_data_array(1, :);
+t_ft = ft_sensor_data_array(:, 1);
+Fx_ft = ft_sensor_data_array(:, 2) - offset(2);
+Fy_ft = ft_sensor_data_array(:, 3) - offset(3);
+Fz_ft = ft_sensor_data_array(:, 4) - offset(4);
+Tx_ft = ft_sensor_data_array(:, 5) - offset(5);
+Ty_ft = ft_sensor_data_array(:, 6) - offset(6);
+Tz_ft = ft_sensor_data_array(:, 7) - offset(7);
+%%
+
+t_start = t_ctrl(1);
+t_end = t_ctrl(end);
+
+lw = 1.2;
+font = 'Times New Roman';
+fs_lgd = 14;
+fs_lbl = 22;
+fs_axis = 14;
+fs_title = 20;
+
+%% Joint acceleration
+% close all;
+figure();
+fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\ddot{q}_1$', '$\ddot{q}_2$', '$\ddot{q}_3$', '$\ddot{q}_4$', '$\ddot{q}_5$', ...
+    '$\ddot{q}_6$', '$\ddot{q}_7$',};
+plt_indices = 1:7;
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_ctrl, ddq_eul(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'k');
+    plot(t_ctrl, ddq_mob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'r');
+    % plot(t, ddq_sosml(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'g');
+    % plt2.Color(4) = 0.7;
+    xlim([t_start, t_end]);
+    % ylim([-5 5]);
+
+    ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
+    
+    % if i == 7
+    %     ylim([-1 1]);
+    % end
+
+    if i == 1
+        legend('Euler method', 'MOB', 'SOSML', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+    end
+end
+
+title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Task acceleration
+% close all;
+figure();
+fig_ddx = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\ddot{p}_x$', '$\ddot{p}_y$', '$\ddot{p}_z$', '$\dot\omega_{x}$', ...
+    '$\dot\omega_{y}$', '$\dot\omega_{z}$'};
+plt_indices = 1:6;
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_ctrl, ee_acc_eul(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'k');
+    plot(t_ctrl, ee_acc_mob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+    % plot(t, ee_acc_sosml(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+    xlim([t_start, t_end]);
+    ylabel(ytitles(i), 'Interpreter', 'latex', 'FontSize', fs_lbl);
+    
+    % if i == 1 || i == 2 || i == 3
+    %     ylim([-1 1]);
+    % elseif i == 4 || i == 5 || i == 6
+    %     ylim([-2 2]);
+    % end
+
+    if i == 3
+        legend('Euler method', 'MOB estimation', 'SOSML estimation', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+    end
+end
+
+title(fig_ddx, 'Task Acceleration', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_ddx, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Pose Tracking Performance
+% close all;
+figure();
+fig_pose = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$p_{ee} \ [m]$', '$\psi_{ee} \ [rad]$'};
+plt_indices = 1:7;
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_ctrl, ee_pose_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+    plot(t_ctrl, ee_pose_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
+    xlim([t_start, t_end]);
+    
+    if i == 1
+        ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+    end
+
+    if i == 1
+        legend('Desired', 'Impedance control', 'Impedance control + DOB' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+    end
+end
+
+% for i = 1:length(plt_indices)
+%     nexttile;
+%     hold on;
+%     grid;
+%     plot(t, ee_rpy_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+%     plot(t, ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
+%     plot(t_dob, ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+%     % plot(t, fr_ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+%     xlim([t_start, t_end]);
+% 
+%     if i == 1
+%         ylabel(ytitles{mod(i-1, 2) + 1 + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+%     end
+% end
+
+title(fig_pose, 'Pose Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_pose, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Twist Tracking Performance
+figure();
+fig_twist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\dot{p}_{ee} \ [m/s]$', '$\omega_{ee} \ [rad/s]$'};
+plt_indices = 1:6;
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_ctrl, ee_vel_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+    plot(t_ctrl, ee_vel_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1.5 * lw, 'Color', 'r');
+    xlim([t_start, t_end]);
+    
+    if i == 1 || i == 4
+        ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+    end
+
+    if i == 1
+        legend('Desired', 'Measured', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+    end
+end
+
+title(fig_twist, 'Twist Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_twist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Estimated disturbance
+% close all;
+figure(); 
+% ytitles = {'$\hat{F}_{ext} \ [N]$', '$\hat{T}_{ext} \ [N/m]$'};
+
+nexttile;
+hold on;
+grid;
+plot(t_ft, Fz_ft, 'LineStyle', '--', 'LineWidth', 1 * lw, 'Color', 'k');
+plot(t_ctrl, -1 * F_d_fr3(:, 3), 'LineStyle', '-', 'LineWidth', 2*lw, 'Color', '#EDB120');
+plot(t_ctrl, F_d_dob(:, 3), 'LineStyle', '-.', 'LineWidth', 2*lw, 'Color', '#0072BD');
+plot(t_ctrl, F_d_mob(:, 3), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', '#D95319');
+% plot(t, F_d_sosml(:, 3), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'g');
+xlim([t_start, t_end]);
+
+title('Task Disturbance Estimation', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel('Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Joint torque disturbance estiamtion
+% close all;
+figure(); 
+fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\tau_1$', '$\tau_2$', '$\tau_3$', '$\tau_4$', '$\tau_5$', '$\tau_6$', '$\tau_7$',};
+plt_indices = 1:7;
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_ctrl, tau_d_dob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+    plot(t_ctrl, tau_d_mob(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'b');
+    % plot(t, tau_d_sosml(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'g');
+    % plot(t, tau_c(:,plt_indices(i)) + tau_g(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+    % plot(t, tau_c(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+    xlim([t_start, t_end]);
+    % ylim([-5 5]);
+
+    ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
+    
+    if i == 1
+        legend('MOB estimation', 'DOB estimation', 'MuJoCo', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+    end
+end
+
+title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);