diff --git a/doc/2021-06-23-Nano hexapode.xlsx b/doc/2021-06-23-Nano hexapode.xlsx new file mode 100644 index 0000000..54bb55c Binary files /dev/null and b/doc/2021-06-23-Nano hexapode.xlsx differ diff --git a/figs/IMG_20210625_083801.jpg b/figs/IMG_20210625_083801.jpg new file mode 100644 index 0000000..cce5c24 Binary files /dev/null and b/figs/IMG_20210625_083801.jpg differ diff --git a/figs/acc_top_plat_pos_x.jpg b/figs/acc_top_plat_pos_x.jpg new file mode 100644 index 0000000..de33cef Binary files /dev/null and b/figs/acc_top_plat_pos_x.jpg differ diff --git a/figs/acc_top_plat_pos_x.svg b/figs/acc_top_plat_pos_x.svg new file mode 100644 index 0000000..9533c3a Binary files /dev/null and b/figs/acc_top_plat_pos_x.svg differ diff --git a/figs/acc_top_plat_pos_y.jpg b/figs/acc_top_plat_pos_y.jpg new file mode 100644 index 0000000..67e94db Binary files /dev/null and b/figs/acc_top_plat_pos_y.jpg differ diff --git a/figs/acc_top_plat_pos_y.svg b/figs/acc_top_plat_pos_y.svg new file mode 100644 index 0000000..e9db7e9 Binary files /dev/null and b/figs/acc_top_plat_pos_y.svg differ diff --git a/figs/acc_top_plat_top_view.jpg b/figs/acc_top_plat_top_view.jpg new file mode 100644 index 0000000..6953bc1 Binary files /dev/null and b/figs/acc_top_plat_top_view.jpg differ diff --git a/figs/acc_top_plat_top_view.png b/figs/acc_top_plat_top_view.png new file mode 100644 index 0000000..33ae188 Binary files /dev/null and b/figs/acc_top_plat_top_view.png differ diff --git a/figs/acc_top_plat_zoom.jpg b/figs/acc_top_plat_zoom.jpg new file mode 100644 index 0000000..17358d1 Binary files /dev/null and b/figs/acc_top_plat_zoom.jpg differ diff --git a/figs/bode_Va_dL_effect_k_coef.pdf b/figs/bode_Va_dL_effect_k_coef.pdf new file mode 100644 index 0000000..f721e85 Binary files /dev/null and b/figs/bode_Va_dL_effect_k_coef.pdf differ diff --git a/figs/bode_Va_dL_effect_k_coef.png b/figs/bode_Va_dL_effect_k_coef.png new file mode 100644 index 0000000..a322269 Binary files /dev/null and b/figs/bode_Va_dL_effect_k_coef.png differ diff --git a/figs/bode_Va_dL_effect_m_coef.pdf b/figs/bode_Va_dL_effect_m_coef.pdf new file mode 100644 index 0000000..36a093e Binary files /dev/null and b/figs/bode_Va_dL_effect_m_coef.pdf differ diff --git a/figs/bode_Va_dL_effect_m_coef.png b/figs/bode_Va_dL_effect_m_coef.png new file mode 100644 index 0000000..c291f8c Binary files /dev/null and b/figs/bode_Va_dL_effect_m_coef.png differ diff --git a/figs/bode_Va_dL_effect_xi_damp.pdf b/figs/bode_Va_dL_effect_xi_damp.pdf new file mode 100644 index 0000000..ccf8634 Binary files /dev/null and b/figs/bode_Va_dL_effect_xi_damp.pdf differ diff --git a/figs/bode_Va_dL_effect_xi_damp.png b/figs/bode_Va_dL_effect_xi_damp.png new file mode 100644 index 0000000..9cbbffa Binary files /dev/null and b/figs/bode_Va_dL_effect_xi_damp.png differ diff --git a/figs/control_architecture_hac_iff_L.pdf b/figs/control_architecture_hac_iff_L.pdf new file mode 100644 index 0000000..abada49 Binary files /dev/null and b/figs/control_architecture_hac_iff_L.pdf differ diff --git a/figs/control_architecture_hac_iff_L.png b/figs/control_architecture_hac_iff_L.png new file mode 100644 index 0000000..b1d3999 Binary files /dev/null and b/figs/control_architecture_hac_iff_L.png differ diff --git a/figs/control_architecture_hac_iff_L.svg b/figs/control_architecture_hac_iff_L.svg new file mode 100644 index 0000000..35d1961 Binary files /dev/null and b/figs/control_architecture_hac_iff_L.svg differ diff --git a/figs/control_architecture_hac_iff_L_feedforward.pdf b/figs/control_architecture_hac_iff_L_feedforward.pdf new file mode 100644 index 0000000..5e5e024 Binary files /dev/null and b/figs/control_architecture_hac_iff_L_feedforward.pdf differ diff --git a/figs/control_architecture_hac_iff_L_feedforward.png b/figs/control_architecture_hac_iff_L_feedforward.png new file mode 100644 index 0000000..9577b90 Binary files /dev/null and b/figs/control_architecture_hac_iff_L_feedforward.png differ diff --git a/figs/control_architecture_hac_iff_L_feedforward.svg b/figs/control_architecture_hac_iff_L_feedforward.svg new file mode 100644 index 0000000..010a06d Binary files /dev/null and b/figs/control_architecture_hac_iff_L_feedforward.svg differ diff --git a/figs/control_architecture_iff.pdf b/figs/control_architecture_iff.pdf new file mode 100644 index 0000000..67e4fe8 Binary files /dev/null and b/figs/control_architecture_iff.pdf differ diff --git a/figs/control_architecture_iff.png b/figs/control_architecture_iff.png new file mode 100644 index 0000000..74f253c Binary files /dev/null and b/figs/control_architecture_iff.png differ diff --git a/figs/control_architecture_iff.svg b/figs/control_architecture_iff.svg new file mode 100644 index 0000000..d76ea79 Binary files /dev/null and b/figs/control_architecture_iff.svg differ diff --git a/figs/control_architecture_iff_feedforward.pdf b/figs/control_architecture_iff_feedforward.pdf new file mode 100644 index 0000000..0b2bf84 Binary files /dev/null and b/figs/control_architecture_iff_feedforward.pdf differ diff --git a/figs/control_architecture_iff_feedforward.png b/figs/control_architecture_iff_feedforward.png new file mode 100644 index 0000000..738df26 Binary files /dev/null and b/figs/control_architecture_iff_feedforward.png differ diff --git a/figs/control_architecture_iff_feedforward.svg b/figs/control_architecture_iff_feedforward.svg new file mode 100644 index 0000000..df78d1a Binary files /dev/null and b/figs/control_architecture_iff_feedforward.svg differ diff --git a/figs/damped_iff_plates_plant_comp_diagonal.pdf b/figs/damped_iff_plates_plant_comp_diagonal.pdf new file mode 100644 index 0000000..ca0d9b8 Binary files /dev/null and b/figs/damped_iff_plates_plant_comp_diagonal.pdf differ diff --git a/figs/damped_iff_plates_plant_comp_diagonal.png b/figs/damped_iff_plates_plant_comp_diagonal.png new file mode 100644 index 0000000..b1bbd06 Binary files /dev/null and b/figs/damped_iff_plates_plant_comp_diagonal.png differ diff --git a/figs/damped_iff_plates_plant_comp_off_diagonal.pdf b/figs/damped_iff_plates_plant_comp_off_diagonal.pdf new file mode 100644 index 0000000..b4d7777 Binary files /dev/null and b/figs/damped_iff_plates_plant_comp_off_diagonal.pdf differ diff --git a/figs/damped_iff_plates_plant_comp_off_diagonal.png b/figs/damped_iff_plates_plant_comp_off_diagonal.png new file mode 100644 index 0000000..9447338 Binary files /dev/null and b/figs/damped_iff_plates_plant_comp_off_diagonal.png differ diff --git a/figs/dvf_plant_comp_struts_plates.pdf b/figs/dvf_plant_comp_struts_plates.pdf new file mode 100644 index 0000000..a602135 Binary files /dev/null and b/figs/dvf_plant_comp_struts_plates.pdf differ diff --git a/figs/dvf_plant_comp_struts_plates.png b/figs/dvf_plant_comp_struts_plates.png new file mode 100644 index 0000000..2dfe3be Binary files /dev/null and b/figs/dvf_plant_comp_struts_plates.png differ diff --git a/figs/enc_plant_plates_effect_iff.pdf b/figs/enc_plant_plates_effect_iff.pdf new file mode 100644 index 0000000..d3fe191 Binary files /dev/null and b/figs/enc_plant_plates_effect_iff.pdf differ diff --git a/figs/enc_plant_plates_effect_iff.png b/figs/enc_plant_plates_effect_iff.png new file mode 100644 index 0000000..e32f215 Binary files /dev/null and b/figs/enc_plant_plates_effect_iff.png differ diff --git a/figs/enc_plates_dvf_coh.pdf b/figs/enc_plates_dvf_coh.pdf new file mode 100644 index 0000000..4ab52f3 Binary files /dev/null and b/figs/enc_plates_dvf_coh.pdf differ diff --git a/figs/enc_plates_dvf_coh.png b/figs/enc_plates_dvf_coh.png new file mode 100644 index 0000000..854976f Binary files /dev/null and b/figs/enc_plates_dvf_coh.png differ diff --git a/figs/enc_plates_dvf_comp_offdiag_simscape.pdf b/figs/enc_plates_dvf_comp_offdiag_simscape.pdf new file mode 100644 index 0000000..fb3ab99 Binary files /dev/null and b/figs/enc_plates_dvf_comp_offdiag_simscape.pdf differ diff --git a/figs/enc_plates_dvf_comp_offdiag_simscape.png b/figs/enc_plates_dvf_comp_offdiag_simscape.png new file mode 100644 index 0000000..028f08d Binary files /dev/null and b/figs/enc_plates_dvf_comp_offdiag_simscape.png differ diff --git a/figs/enc_plates_dvf_comp_simscape.pdf b/figs/enc_plates_dvf_comp_simscape.pdf new file mode 100644 index 0000000..49a5439 Binary files /dev/null and b/figs/enc_plates_dvf_comp_simscape.pdf differ diff --git a/figs/enc_plates_dvf_comp_simscape.png b/figs/enc_plates_dvf_comp_simscape.png new file mode 100644 index 0000000..9c76cd8 Binary files /dev/null and b/figs/enc_plates_dvf_comp_simscape.png differ diff --git a/figs/enc_plates_dvf_comp_simscape_all.pdf b/figs/enc_plates_dvf_comp_simscape_all.pdf new file mode 100644 index 0000000..ed3b102 Binary files /dev/null and b/figs/enc_plates_dvf_comp_simscape_all.pdf differ diff --git a/figs/enc_plates_dvf_comp_simscape_all.png b/figs/enc_plates_dvf_comp_simscape_all.png new file mode 100644 index 0000000..d0c9de5 Binary files /dev/null and b/figs/enc_plates_dvf_comp_simscape_all.png differ diff --git a/figs/enc_plates_dvf_frf.pdf b/figs/enc_plates_dvf_frf.pdf new file mode 100644 index 0000000..7c5ad0c Binary files /dev/null and b/figs/enc_plates_dvf_frf.pdf differ diff --git a/figs/enc_plates_dvf_frf.png b/figs/enc_plates_dvf_frf.png new file mode 100644 index 0000000..8a5f76d Binary files /dev/null and b/figs/enc_plates_dvf_frf.png differ diff --git a/figs/enc_plates_iff_coh.pdf b/figs/enc_plates_iff_coh.pdf new file mode 100644 index 0000000..4d32c0c Binary files /dev/null and b/figs/enc_plates_iff_coh.pdf differ diff --git a/figs/enc_plates_iff_coh.png b/figs/enc_plates_iff_coh.png new file mode 100644 index 0000000..bbe13cf Binary files /dev/null and b/figs/enc_plates_iff_coh.png differ diff --git a/figs/enc_plates_iff_comp_offdiag_simscape.pdf b/figs/enc_plates_iff_comp_offdiag_simscape.pdf new file mode 100644 index 0000000..0fa587d Binary files /dev/null and b/figs/enc_plates_iff_comp_offdiag_simscape.pdf differ diff --git a/figs/enc_plates_iff_comp_offdiag_simscape.png b/figs/enc_plates_iff_comp_offdiag_simscape.png new file mode 100644 index 0000000..e2070b7 Binary files /dev/null and b/figs/enc_plates_iff_comp_offdiag_simscape.png differ diff --git a/figs/enc_plates_iff_comp_simscape.pdf b/figs/enc_plates_iff_comp_simscape.pdf new file mode 100644 index 0000000..066296e Binary files /dev/null and b/figs/enc_plates_iff_comp_simscape.pdf differ diff --git a/figs/enc_plates_iff_comp_simscape.png b/figs/enc_plates_iff_comp_simscape.png new file mode 100644 index 0000000..020ac78 Binary files /dev/null and b/figs/enc_plates_iff_comp_simscape.png differ diff --git a/figs/enc_plates_iff_comp_simscape_all.pdf b/figs/enc_plates_iff_comp_simscape_all.pdf new file mode 100644 index 0000000..a58f5e6 Binary files /dev/null and b/figs/enc_plates_iff_comp_simscape_all.pdf differ diff --git a/figs/enc_plates_iff_comp_simscape_all.png b/figs/enc_plates_iff_comp_simscape_all.png new file mode 100644 index 0000000..932aff6 Binary files /dev/null and b/figs/enc_plates_iff_comp_simscape_all.png differ diff --git a/figs/enc_plates_iff_frf.pdf b/figs/enc_plates_iff_frf.pdf new file mode 100644 index 0000000..3bb922a Binary files /dev/null and b/figs/enc_plates_iff_frf.pdf differ diff --git a/figs/enc_plates_iff_frf.png b/figs/enc_plates_iff_frf.png new file mode 100644 index 0000000..da3853e Binary files /dev/null and b/figs/enc_plates_iff_frf.png differ diff --git a/figs/enc_plates_iff_gains_effect_dvf_plant.pdf b/figs/enc_plates_iff_gains_effect_dvf_plant.pdf new file mode 100644 index 0000000..d2fbae8 Binary files /dev/null and b/figs/enc_plates_iff_gains_effect_dvf_plant.pdf differ diff --git a/figs/enc_plates_iff_gains_effect_dvf_plant.png b/figs/enc_plates_iff_gains_effect_dvf_plant.png new file mode 100644 index 0000000..776b220 Binary files /dev/null and b/figs/enc_plates_iff_gains_effect_dvf_plant.png differ diff --git a/figs/enc_plates_opt_iff_comp_simscape_all.pdf b/figs/enc_plates_opt_iff_comp_simscape_all.pdf new file mode 100644 index 0000000..2115172 Binary files /dev/null and b/figs/enc_plates_opt_iff_comp_simscape_all.pdf differ diff --git a/figs/enc_plates_opt_iff_comp_simscape_all.png b/figs/enc_plates_opt_iff_comp_simscape_all.png new file mode 100644 index 0000000..4a915ab Binary files /dev/null and b/figs/enc_plates_opt_iff_comp_simscape_all.png differ diff --git a/figs/hac_iff_plates_exp_loop_gain_diag.pdf b/figs/hac_iff_plates_exp_loop_gain_diag.pdf new file mode 100644 index 0000000..7e6c374 Binary files /dev/null and b/figs/hac_iff_plates_exp_loop_gain_diag.pdf differ diff --git a/figs/hac_iff_plates_exp_loop_gain_diag.png b/figs/hac_iff_plates_exp_loop_gain_diag.png new file mode 100644 index 0000000..1b07afe Binary files /dev/null and b/figs/hac_iff_plates_exp_loop_gain_diag.png differ diff --git a/figs/hac_iff_struts_enc_plates_plant_bode.pdf b/figs/hac_iff_struts_enc_plates_plant_bode.pdf new file mode 100644 index 0000000..9aa24ab Binary files /dev/null and b/figs/hac_iff_struts_enc_plates_plant_bode.pdf differ diff --git a/figs/hac_iff_struts_enc_plates_plant_bode.png b/figs/hac_iff_struts_enc_plates_plant_bode.png new file mode 100644 index 0000000..68d2116 Binary files /dev/null and b/figs/hac_iff_struts_enc_plates_plant_bode.png differ diff --git a/figs/loop_gain_hac_iff_struts.pdf b/figs/loop_gain_hac_iff_struts.pdf new file mode 100644 index 0000000..1732683 Binary files /dev/null and b/figs/loop_gain_hac_iff_struts.pdf differ diff --git a/figs/loop_gain_hac_iff_struts.png b/figs/loop_gain_hac_iff_struts.png new file mode 100644 index 0000000..25cc1bc Binary files /dev/null and b/figs/loop_gain_hac_iff_struts.png differ diff --git a/figs/nano_hexapod_mounted.jpg b/figs/nano_hexapod_mounted.jpg new file mode 100644 index 0000000..476393b Binary files /dev/null and b/figs/nano_hexapod_mounted.jpg differ diff --git a/figs/open_loop_gain_feedforward_iff_struts.pdf b/figs/open_loop_gain_feedforward_iff_struts.pdf new file mode 100644 index 0000000..000f0a2 Binary files /dev/null and b/figs/open_loop_gain_feedforward_iff_struts.pdf differ diff --git a/figs/open_loop_gain_feedforward_iff_struts.png b/figs/open_loop_gain_feedforward_iff_struts.png new file mode 100644 index 0000000..f477db5 Binary files /dev/null and b/figs/open_loop_gain_feedforward_iff_struts.png differ diff --git a/figs/ref_track_hac_iff_struts_pos_error.pdf b/figs/ref_track_hac_iff_struts_pos_error.pdf new file mode 100644 index 0000000..17a36b8 Binary files /dev/null and b/figs/ref_track_hac_iff_struts_pos_error.pdf differ diff --git a/figs/ref_track_hac_iff_struts_pos_error.png b/figs/ref_track_hac_iff_struts_pos_error.png new file mode 100644 index 0000000..3a67579 Binary files /dev/null and b/figs/ref_track_hac_iff_struts_pos_error.png differ diff --git a/figs/ref_track_hac_iff_struts_yz_plane.pdf b/figs/ref_track_hac_iff_struts_yz_plane.pdf new file mode 100644 index 0000000..91b8945 Binary files /dev/null and b/figs/ref_track_hac_iff_struts_yz_plane.pdf differ diff --git a/figs/ref_track_hac_iff_struts_yz_plane.png b/figs/ref_track_hac_iff_struts_yz_plane.png new file mode 100644 index 0000000..e009ace Binary files /dev/null and b/figs/ref_track_hac_iff_struts_yz_plane.png differ diff --git a/figs/ref_track_hac_iff_struts_yz_time.pdf b/figs/ref_track_hac_iff_struts_yz_time.pdf new file mode 100644 index 0000000..fdb93f6 Binary files /dev/null and b/figs/ref_track_hac_iff_struts_yz_time.pdf differ diff --git a/figs/ref_track_hac_iff_struts_yz_time.png b/figs/ref_track_hac_iff_struts_yz_time.png new file mode 100644 index 0000000..c524dcc Binary files /dev/null and b/figs/ref_track_hac_iff_struts_yz_time.png differ diff --git a/figs/ref_track_test_nass.pdf b/figs/ref_track_test_nass.pdf new file mode 100644 index 0000000..5e17f03 Binary files /dev/null and b/figs/ref_track_test_nass.pdf differ diff --git a/figs/ref_track_test_nass.png b/figs/ref_track_test_nass.png new file mode 100644 index 0000000..485c830 Binary files /dev/null and b/figs/ref_track_test_nass.png differ diff --git a/matlab/mat/Khac_iff_struts.mat b/matlab/mat/Khac_iff_struts.mat new file mode 100644 index 0000000..a878320 Binary files /dev/null and b/matlab/mat/Khac_iff_struts.mat differ diff --git a/matlab/mat/Kiff.mat b/matlab/mat/Kiff.mat new file mode 100644 index 0000000..7b9285e Binary files /dev/null and b/matlab/mat/Kiff.mat differ diff --git a/matlab/mat/damped_plant_enc_plates.mat b/matlab/mat/damped_plant_enc_plates.mat new file mode 100644 index 0000000..57017ce Binary files /dev/null and b/matlab/mat/damped_plant_enc_plates.mat differ diff --git a/matlab/mat/feedforward_iff.mat b/matlab/mat/feedforward_iff.mat new file mode 100644 index 0000000..98ad26f Binary files /dev/null and b/matlab/mat/feedforward_iff.mat differ diff --git a/matlab/mat/identified_plants_enc_plates.mat b/matlab/mat/identified_plants_enc_plates.mat new file mode 100644 index 0000000..88143bf Binary files /dev/null and b/matlab/mat/identified_plants_enc_plates.mat differ diff --git a/matlab/mat/identified_plants_enc_struts.mat b/matlab/mat/identified_plants_enc_struts.mat new file mode 100644 index 0000000..a2dd16a Binary files /dev/null and b/matlab/mat/identified_plants_enc_struts.mat differ diff --git a/matlab/mat/reference_path.mat b/matlab/mat/reference_path.mat new file mode 100644 index 0000000..1789eee Binary files /dev/null and b/matlab/mat/reference_path.mat differ diff --git a/matlab/nano_hexapod_simscape.slx b/matlab/nano_hexapod_simscape.slx index a6e905f..efa9835 100644 Binary files a/matlab/nano_hexapod_simscape.slx and b/matlab/nano_hexapod_simscape.slx differ diff --git a/matlab/src/generateXYZTrajectory.m b/matlab/src/generateXYZTrajectory.m new file mode 100644 index 0000000..e3a15da --- /dev/null +++ b/matlab/src/generateXYZTrajectory.m @@ -0,0 +1,48 @@ +function [ref] = generateXYZTrajectory(args) +% generateXYZTrajectory - +% +% Syntax: [ref] = generateXYZTrajectory(args) +% +% Inputs: +% - args +% +% Outputs: +% - ref - Reference Signal + +arguments + args.points double {mustBeNumeric} = zeros(2, 3) % [m] + + args.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 % Time to go to first point and after last point [s] + args.tw (1,1) double {mustBeNumeric, mustBePositive} = 0.5 % Time wait between each point [s] + args.tm (1,1) double {mustBeNumeric, mustBePositive} = 1 % Motion time between points [s] + + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s] +end + +time_i = 0:args.Ts:args.ti; +time_w = 0:args.Ts:args.tw; +time_m = 0:args.Ts:args.tm; + +% Go to initial position +xyz = (args.points(1,:))'*(time_i/args.ti); + +% Wait +xyz = [xyz, xyz(:,end).*ones(size(time_w))]; + +% Scans +for i = 2:size(args.points, 1) + % Go to next point + xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)]; + % Wait a litle bit + xyz = [xyz, xyz(:,end).*ones(size(time_w))]; +end + +% End motion +xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)]; + +t = 0:args.Ts:args.Ts*(length(xyz) - 1); + +ref = zeros(length(xyz), 7); + +ref(:, 1) = t; +ref(:, 2:4) = xyz'; diff --git a/matlab/src/generateYZScanTrajectory.m b/matlab/src/generateYZScanTrajectory.m new file mode 100644 index 0000000..d40da82 --- /dev/null +++ b/matlab/src/generateYZScanTrajectory.m @@ -0,0 +1,83 @@ +function [ref] = generateYZScanTrajectory(args) +% generateYZScanTrajectory - +% +% Syntax: [ref] = generateYZScanTrajectory(args) +% +% Inputs: +% - args +% +% Outputs: +% - ref - Reference Signal + +arguments + args.y_tot (1,1) double {mustBeNumeric} = 10e-6 % [m] + args.z_tot (1,1) double {mustBeNumeric} = 10e-6 % [m] + + args.n (1,1) double {mustBeInteger, mustBePositive} = 10 % [-] + + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s] + + args.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] + args.tw (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] + args.ty (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] + args.tz (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] +end + +time_i = 0:args.Ts:args.ti; +time_w = 0:args.Ts:args.tw; +time_y = 0:args.Ts:args.ty; +time_z = 0:args.Ts:args.tz; + +% Go to initial position +y = (time_i/args.ti)*(args.y_tot/2); + +% Wait +y = [y, y(end)*ones(size(time_w))]; + +% Scans +for i = 1:args.n + if mod(i,2) == 0 + y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot]; + else + y = [y, (args.y_tot/2) - (time_y/args.ty)*args.y_tot]; + end + + if i < args.n + y = [y, y(end)*ones(size(time_z))]; + end +end + +% Wait a litle bit +y = [y, y(end)*ones(size(time_w))]; + +% End motion +y = [y, y(end) - y(end)*time_i/args.ti]; + +% Go to initial position +z = (time_i/args.ti)*(args.z_tot/2); + +% Wait +z = [z, z(end)*ones(size(time_w))]; + +% Scans +for i = 1:args.n + z = [z, z(end)*ones(size(time_y))]; + + if i < args.n + z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)]; + end +end + +% Wait a litle bit +z = [z, z(end)*ones(size(time_w))]; + +% End motion +z = [z, z(end) - z(end)*time_i/args.ti]; + +t = 0:args.Ts:args.Ts*(length(y) - 1); + +ref = zeros(length(y), 7); + +ref(:, 1) = t; +ref(:, 3) = y; +ref(:, 4) = z; diff --git a/matlab/src/getJacobianNanoHexapod.m b/matlab/src/getJacobianNanoHexapod.m new file mode 100644 index 0000000..f660c78 --- /dev/null +++ b/matlab/src/getJacobianNanoHexapod.m @@ -0,0 +1,34 @@ +function [J] = getJacobianNanoHexapod(Hbm) +% getJacobianNanoHexapod - +% +% Syntax: [J] = getJacobianNanoHexapod(Hbm) +% +% Inputs: +% - Hbm - Height of {B} w.r.t. {M} [m] +% +% Outputs: +% - J - Jacobian Matrix + +Fa = [[-86.05, -74.78, 22.49], + [ 86.05, -74.78, 22.49], + [ 107.79, -37.13, 22.49], + [ 21.74, 111.91, 22.49], + [-21.74, 111.91, 22.49], + [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m] + +Mb = [[-28.47, -106.25, -22.50], + [ 28.47, -106.25, -22.50], + [ 106.25, 28.47, -22.50], + [ 77.78, 77.78, -22.50], + [-77.78, 77.78, -22.50], + [-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m] + +H = 95e-3; % Stewart platform height [m] +Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m] + +si = Fb - Fa; +si = si./vecnorm(si); % Normalize + +Bb = Mb - [0; 0; Hbm]; + +J = [si', cross(Bb, si)']; diff --git a/matlab/src/getTransformationMatrixAcc.m b/matlab/src/getTransformationMatrixAcc.m new file mode 100644 index 0000000..cc4dd44 --- /dev/null +++ b/matlab/src/getTransformationMatrixAcc.m @@ -0,0 +1,23 @@ +function [M] = getTransformationMatrixAcc(Opm, Osm) +% getTransformationMatrixAcc - +% +% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm) +% +% Inputs: +% - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers +% - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation +% +% Outputs: +% - M - Transformation Matrix + +M = zeros(length(Opm), 6); + +for i = 1:length(Opm) + Ri = [0, Opm(3,i), -Opm(2,i); + -Opm(3,i), 0, Opm(1,i); + Opm(2,i), -Opm(1,i), 0]; + M(i, 1:3) = Osm(:,i)'; + M(i, 4:6) = Osm(:,i)'*Ri; +end + +end diff --git a/test-bench-nano-hexapod.html b/test-bench-nano-hexapod.html index 35adafe..bee3078 100644 --- a/test-bench-nano-hexapod.html +++ b/test-bench-nano-hexapod.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Nano-Hexapod - Test Bench @@ -39,54 +39,177 @@

Table of Contents

@@ -95,19 +218,19 @@

-This document is dedicated to the experimental study of the nano-hexapod shown in Figure 1. +This document is dedicated to the experimental study of the nano-hexapod shown in Figure 1.

-
+

IMG_20210608_152917.jpg

Figure 1: Nano-Hexapod

-
+

-Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure 2): +Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure 2):

  • Voltage Amplifier: PiezoDrive PD200
    • @@ -120,25 +243,25 @@ Here are the documentation of the equipment used for this test bench (lots of th
-
+

IMG_20210608_154722.jpg

Figure 2: Nano-Hexapod and the control electronics

-In Figure 3 is shown a block diagram of the experimental setup. -When possible, the notations are consistent with this diagram and summarized in Table 1. +In Figure 3 is shown a block diagram of the experimental setup. +When possible, the notations are consistent with this diagram and summarized in Table 1.

-
+

nano_hexapod_signals.png

Figure 3: Block diagram of the system with named signals

- +
@@ -259,20 +382,20 @@ When possible, the notations are consistent with this diagram and summarized in This document is divided in the following sections:

    -
  • Section 1: the encoders are fixed to the struts
    • -
  • Section 2: the encoders are fixed to the plates
    • +
  • Section 1: the encoders are fixed to the struts
    • +
  • Section 2: the encoders are fixed to the plates
-
-

1 Encoders fixed to the Struts

+
+

1 Encoders fixed to the Struts

- +

-
-

1.1 Introduction

+
+

1.1 Introduction

In this section, the encoders are fixed to the struts. @@ -282,23 +405,23 @@ In this section, the encoders are fixed to the struts. It is divided in the following sections:

    -
  • Section 1.2: the transfer function matrix from the actuators to the force sensors and to the encoders is experimentally identified.
    • -
  • Section 1.3: the obtained FRF matrix is compared with the dynamics of the simscape model
    • -
  • Section 1.4: decentralized Integral Force Feedback (IFF) is applied and its performances are evaluated.
    • -
  • Section 1.5: a modal analysis of the nano-hexapod is performed
    • +
  • Section 1.2: the transfer function matrix from the actuators to the force sensors and to the encoders is experimentally identified.
    • +
  • Section 1.3: the obtained FRF matrix is compared with the dynamics of the simscape model
    • +
  • Section 1.4: decentralized Integral Force Feedback (IFF) is applied and its performances are evaluated.
    • +
  • Section 1.5: a modal analysis of the nano-hexapod is performed
-
-

1.2 Identification of the dynamics

+
+

1.2 Identification of the dynamics

- +

-
-

1.2.1 Load Data

+
+

1.2.1 Load Measurement Data

%% Load Identification Data
@@ -313,8 +436,8 @@ meas_data_lf = {};
-
-

1.2.2 Spectral Analysis - Setup

+
+

1.2.2 Spectral Analysis - Setup

%% Setup useful variables
@@ -337,49 +460,49 @@ i_hf = f > 250; % Poi
-
-

1.2.3 DVF Plant

+
+

1.2.3 DVF Plant

-First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 4). +First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 4). 

%% Coherence
-coh_dvf_lf = zeros(length(f), 6, 6);
-coh_dvf_hf = zeros(length(f), 6, 6);
+coh_dvf = zeros(length(f), 6, 6);
 
 for i = 1:6
-    coh_dvf_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
-    coh_dvf_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
+    coh_dvf_lf = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
+    coh_dvf_hf = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
+    coh_dvf(:,:,i) = [coh_dvf_lf(i_lf, :); coh_dvf_hf(i_hf, :)];
 end
-
+

enc_struts_dvf_coh.png

Figure 4: Obtained coherence for the DVF plant

-Then the 6x6 transfer function matrix is estimated (Figure 5). +Then the 6x6 transfer function matrix is estimated (Figure 5). 

%% DVF Plant (transfer function from u to dLm)
-G_dvf_lf = zeros(length(f), 6, 6);
-G_dvf_hf = zeros(length(f), 6, 6);
+G_dvf = zeros(length(f), 6, 6);
 
 for i = 1:6
-    G_dvf_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
-    G_dvf_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
+    G_dvf_lf = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
+    G_dvf_hf = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
+    G_dvf(:,:,i) = [G_dvf_lf(i_lf, :); G_dvf_hf(i_hf, :)];
 end
-
+

enc_struts_dvf_frf.png

Figure 5: Measured FRF for the DVF plant

@@ -387,73 +510,92 @@ G_dvf_hf = zeros(length(f), 6, 6);
- -
-

1.2.4 IFF Plant

+
+

1.2.4 IFF Plant

-First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 6). +First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 6). 

%% Coherence for the IFF plant
-coh_iff_lf = zeros(length(f), 6, 6);
-coh_iff_hf = zeros(length(f), 6, 6);
+coh_iff = zeros(length(f), 6, 6);
 
 for i = 1:6
-    coh_iff_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
-    coh_iff_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
+    coh_iff_lf = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
+    coh_iff_hf = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
+    coh_iff(:,:,i) = [coh_iff_lf(i_lf, :); coh_iff_hf(i_hf, :)];
 end
-
-
+

enc_struts_iff_coh.png

Figure 6: Obtained coherence for the IFF plant

-Then the 6x6 transfer function matrix is estimated (Figure 7). +Then the 6x6 transfer function matrix is estimated (Figure 7). 

%% IFF Plant
-G_iff_lf = zeros(length(f), 6, 6);
-G_iff_hf = zeros(length(f), 6, 6);
+G_iff = zeros(length(f), 6, 6);
 
 for i = 1:6
-    G_iff_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
-    G_iff_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
+    G_iff_lf = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
+    G_iff_hf = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
+    G_iff(:,:,i) = [G_iff_lf(i_lf, :); G_iff_hf(i_hf, :)];
 end
-
+

enc_struts_iff_frf.png

Figure 7: Measured FRF for the IFF plant

+ +
+

1.2.5 Save Identified Plants

+
+
+
save('matlab/mat/identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
+
+
+
+
-
-

1.3 Comparison with the Simscape Model

+
+

1.3 Comparison with the Simscape Model

- +

In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.

-
-

1.3.1 Dynamics from Actuator to Force Sensors

+
+

1.3.1 Load measured FRF

+
%% Load data
+load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
+
+
+
+
+ +
+

1.3.2 Dynamics from Actuator to Force Sensors

+
+
%% Initialize Nano-Hexapod
 n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                        'flex_top_type', '4dof', ...
@@ -465,22 +607,22 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'<
 

 
%% Identify the IFF Plant (transfer function from u to taum)
 clear io; io_i = 1;
-io(io_i) = linio([mdl, '/F'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
-io(io_i) = linio([mdl, '/Fm'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dum'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
 
 Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
 

 

 
 
-

+

 
enc_struts_iff_comp_simscape.png
 

 
Figure 8: Diagonal elements of the IFF Plant

 

 
 
-

+

 
enc_struts_iff_comp_offdiag_simscape.png
 

 
Figure 9: Off diagonal elements of the IFF Plant

@@ -488,11 +630,395 @@ Giff = exp(-s*Ts)
 

 
-

-
1.3.2 Dynamics from Actuator to Encoder

-

+

+
1.3.3 Dynamics from Actuator to Encoder

+

 

 
%% Initialization of the Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible');
+

+

+
+

+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+
+Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+

+

+
+
+

+
enc_struts_dvf_comp_simscape.png
+

+
Figure 10: Diagonal elements of the DVF Plant

+

+
+
+

+
enc_struts_dvf_comp_offdiag_simscape.png
+

+
Figure 11: Off diagonal elements of the DVF Plant

+

+

+

+
+

+
1.3.4 Effect of a change in bending damping of the joints

+

+

+
%% Tested bending dampings [Nm/(rad/s)]
+cRs = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];
+

+

+
+

+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+

+

+
+

+Then the identification is performed for all the values of the bending damping.
+

+

+
%% Idenfity the transfer function from actuator to encoder for all bending dampins
+Gs = {zeros(length(cRs), 1)};
+
+for i = 1:length(cRs)
+    n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                           'flex_top_type', '4dof', ...
+                                           'motion_sensor_type', 'struts', ...
+                                           'actuator_type', 'flexible', ...
+                                           'flex_bot_cRx', cRs(i), ...
+                                           'flex_bot_cRy', cRs(i), ...
+                                           'flex_top_cRx', cRs(i), ...
+                                           'flex_top_cRy', cRs(i));
+
+    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+    G.InputName  = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
+    G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
+
+    Gs(i) = {G};
+end
+

+

+
+
    
+
  • Could be nice
    • 
+
  • Actual damping is very small
    • 
+

+

+

+
+

+
1.3.5 Effect of a change in damping factor of the APA

+

+

+
%% Tested bending dampings [Nm/(rad/s)]
+xis = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];
+

+

+
+

+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+

+

+
+

+
%% Idenfity the transfer function from actuator to encoder for all bending dampins
+Gs = {zeros(length(xis), 1)};
+
+for i = 1:length(xis)
+    n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                           'flex_top_type', '4dof', ...
+                                           'motion_sensor_type', 'struts', ...
+                                           'actuator_type', 'flexible', ...
+                                           'actuator_xi', xis(i));
+
+    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+    G.InputName  = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
+    G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
+
+    Gs(i) = {G};
+end
+

+

+
+
+

+
bode_Va_dL_effect_xi_damp.png
+

+
Figure 12: Effect of the APA damping factor \(\xi\) on the dynamics from \(u\) to \(d\mathcal{L}\)

+

+
+

+

+Damping factor \(\xi\) has a large impact on the damping of the “spurious resonances” at 200Hz and 300Hz.
+

+
+

+
+

+

+Why is the damping factor does not change the damping of the first peak?
+

+
+

+

+

+
+

+
1.3.6 Effect of a change in stiffness damping coef of the APA

+

+

+
m_coef = 1e1;
+

+

+
+

+
%% Tested bending dampings [Nm/(rad/s)]
+k_coefs = [1e-6, 5e-6, 1e-5, 5e-5, 1e-4];
+

+

+
+

+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+

+

+
+

+
%% Idenfity the transfer function from actuator to encoder for all bending dampins
+Gs = {zeros(length(k_coefs), 1)};
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible');
+
+for i = 1:length(k_coefs)
+    k_coef = k_coefs(i);
+
+    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+    G.InputName  = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
+    G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
+
+    Gs(i) = {G};
+end
+

+

+
+
+

+
bode_Va_dL_effect_k_coef.png
+

+
Figure 13: Effect of a change of the damping “stiffness coeficient” on the transfer function from \(u\) to \(d\mathcal{L}\)

+

+

+

+
+

+
1.3.7 Effect of a change in mass damping coef of the APA

+

+

+
k_coef = 1e-6;
+

+

+
+

+
%% Tested bending dampings [Nm/(rad/s)]
+m_coefs = [1e1, 5e1, 1e2, 5e2, 1e3];
+

+

+
+

+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+

+

+
+

+
%% Idenfity the transfer function from actuator to encoder for all bending dampins
+Gs = {zeros(length(m_coefs), 1)};
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible');
+
+for i = 1:length(m_coefs)
+    m_coef = m_coefs(i);
+
+    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+    G.InputName  = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
+    G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
+
+    Gs(i) = {G};
+end
+

+

+
+
+

+
bode_Va_dL_effect_m_coef.png
+

+
Figure 14: Effect of a change of the damping “mass coeficient” on the transfer function from \(u\) to \(d\mathcal{L}\)

+

+

+

+
+

+
1.3.8 Using Flexible model

+

+

+
d_aligns = [[-0.05,  -0.3,  0];
+            [ 0,      0.5,  0];
+            [-0.1,   -0.3,  0];
+            [ 0,      0.3,  0];
+            [-0.05,   0.05, 0];
+            [0,       0,    0]]*1e-3;
+

+

+
+

+
d_aligns = zeros(6,3);
+% d_aligns(1,:) = [-0.05,  -0.3,  0]*1e-3;
+d_aligns(2,:) = [ 0,      0.3,  0]*1e-3;
+

+

+
+

+
%% Initialize Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible', ...
+                                       'actuator_d_align', d_aligns);
+

+

+
+

+

+Why do we have smaller resonances when using flexible APA?
+On the test bench we have the same resonance as the 2DoF model.
+Could it be due to the compliance in other dof of the flexible model?
+

+
+

+
+

+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+
+Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+

+

+
+

+
%% Identify the IFF Plant (transfer function from u to taum)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dum'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
+
+Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+

+

+
+

+
+

+

+

+

+
+

+
1.3.9 Flexible model + encoders fixed to the plates

+

+

+
%% Identify the IFF Plant (transfer function from u to taum)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
+

+

+
+

+
d_aligns = [[-0.05,  -0.3,  0];
+            [ 0,      0.5,  0];
+            [-0.1,   -0.3,  0];
+            [ 0,      0.3,  0];
+            [-0.05,   0.05, 0];
+            [0,       0,    0]]*1e-3;
+

+

+
+

+
%% Initialize Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible', ...
+                                       'actuator_d_align', d_aligns);
+

+

+
+

+
Gdvf_struts = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+

+

+
+

+
%% Initialize Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible', ...
+                                       'actuator_d_align', d_aligns);
+

+

+
+

+
Gdvf_plates = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+

+

+
+
+

+
dvf_plant_comp_struts_plates.png
+

+
Figure 15: Comparison of the dynamics from \(V_a\) to \(d_L\) when the encoders are fixed to the struts (blue) and to the plates (red). APA are modeled as a flexible element.

+

+

+

+

+
+

+
1.4 Integral Force Feedback

+

+

+
+

+

+

+
1.4.1 Identification of the IFF Plant

+

+

+
%% Initialize Nano-Hexapod
 n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                        'flex_top_type', '4dof', ...
                                        'motion_sensor_type', 'struts', ...
@@ -501,56 +1027,34 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'<
 

 
 

-
%% Identify the DVF Plant (transfer function from u to dLm)
+
%% Identify the IFF Plant (transfer function from u to taum)
 clear io; io_i = 1;
-io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
-io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Encoders
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dum'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
 
-Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
 

 

-
-
-

-
enc_struts_dvf_comp_simscape.png
-

-
Figure 10: Diagonal elements of the DVF Plant

-

-
-
-

-
enc_struts_dvf_comp_offdiag_simscape.png
-

-
Figure 11: Off diagonal elements of the DVF Plant

-

-

 

 

 
-

-
1.4 Integral Force Feedback

-

-

-
-

-

-

-
1.4.1 Root Locus and Decentralized Loop gain

-

+

+
1.4.2 Root Locus and Decentralized Loop gain

+

 

 
%% IFF Controller
-Kiff_g1 = (1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
-          (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
-          (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
-          eye(6); % Diagonal 6x6 controller
+Kiff_g1 = -(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
+           (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
+           (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
+           eye(6); % Diagonal 6x6 controller
 

 

 
 
-

+

 
enc_struts_iff_root_locus.png
 

-
Figure 12: Root Locus for the IFF control strategy

+
Figure 16: Root Locus for the IFF control strategy

 

 
 

@@ -562,18 +1066,23 @@ Kiff = g*Kiff_g1;
+
+
save('matlab/mat/Kiff.mat', 'Kiff')
+
+
-
+ +

enc_struts_iff_opt_loop_gain.png

-

Figure 13: Bode plot of the “decentralized loop gain” \(G_\text{iff}(i,i) \times K_\text{iff}(i,i)\)

+

Figure 17: Bode plot of the “decentralized loop gain” \(G_\text{iff}(i,i) \times K_\text{iff}(i,i)\)

-
-

1.4.2 Multiple Gains - Simulation

-
+
+

1.4.3 Multiple Gains - Simulation

+
%% Tested IFF gains
 iff_gains = [4, 10, 20, 40, 100, 200, 400];
@@ -595,7 +1104,7 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'<
 Gd_iff = {zeros(1, length(iff_gains))};
 
 clear io; io_i = 1;
-io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
 io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
 
 for i = 1:length(iff_gains)
@@ -608,25 +1117,25 @@ io(io_i) = linio([mdl, '/D'],  1, 
+

 
enc_struts_iff_gains_effect_dvf_plant.png
 

-
Figure 14: Effect of the IFF gain \(g\) on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)

+
Figure 18: Effect of the IFF gain \(g\) on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)
-
-

1.4.3 Experimental Results - Gains

-
+
+

1.4.4 Experimental Results - Gains

+

Let’s look at the damping introduced by IFF as a function of the IFF gain and compare that with the results obtained using the Simscape model.

-
-
1.4.3.1 Load Data
-
+
+
1.4.4.1 Load Data
+
%% Load Identification Data
 meas_iff_gains = {};
@@ -639,9 +1148,9 @@ meas_iff_gains = {};
-
-
1.4.3.2 Spectral Analysis - Setup
-
+
+
1.4.4.2 Spectral Analysis - Setup
+
%% Setup useful variables
 % Sampling Time [s]
@@ -660,9 +1169,9 @@ win = hanning(ceil(1*Fs));
-
-
1.4.3.3 DVF Plant
-
+
+
1.4.4.3 DVF Plant
+
%% DVF Plant (transfer function from u to dLm)
 G_iff_gains = {};
@@ -674,20 +1183,20 @@ G_iff_gains = {};
-
+

comp_iff_gains_dvf_plant.png

-

Figure 15: Transfer function from \(u\) to \(d\mathcal{L}_m\) for multiple values of the IFF gain

+

Figure 19: Transfer function from \(u\) to \(d\mathcal{L}_m\) for multiple values of the IFF gain

-
+

comp_iff_gains_dvf_plant_zoom.png

-

Figure 16: Transfer function from \(u\) to \(d\mathcal{L}_m\) for multiple values of the IFF gain (Zoom)

+

Figure 20: Transfer function from \(u\) to \(d\mathcal{L}_m\) for multiple values of the IFF gain (Zoom)

-
+

The IFF control strategy is very effective for the damping of the suspension modes. It however does not damp the modes at 200Hz, 300Hz and 400Hz (flexible modes of the APA). @@ -702,17 +1211,17 @@ Also, the experimental results and the models obtained from the Simscape model a

-
-
1.4.3.4 Experimental Results - Comparison of the un-damped and fully damped system
-
+
+
1.4.4.4 Experimental Results - Comparison of the un-damped and fully damped system
+
-
+

comp_undamped_opt_iff_gain_diagonal.png

-

Figure 17: Comparison of the diagonal elements of the tranfer function from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) without active damping and with optimal IFF gain

+

Figure 21: Comparison of the diagonal elements of the tranfer function from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) without active damping and with optimal IFF gain

-
+

A series of modes at around 205Hz are also damped.

@@ -726,17 +1235,17 @@ Are these damped modes at 205Hz additional “suspension” modes or fle
-
-

1.4.4 Experimental Results - Damped Plant with Optimal gain

-
+
+

1.4.5 Experimental Results - Damped Plant with Optimal gain

+

Let’s now look at the \(6 \times 6\) damped plant with the optimal gain \(g = 400\).

-
-
1.4.4.1 Load Data
-
+
+
1.4.5.1 Load Data
+
%% Load Identification Data
 meas_iff_struts = {};
@@ -749,9 +1258,9 @@ meas_iff_struts = {};
-
-
1.4.4.2 Spectral Analysis - Setup
-
+
+
1.4.5.2 Spectral Analysis - Setup
+
%% Setup useful variables
 % Sampling Time [s]
@@ -770,9 +1279,9 @@ win = hanning(ceil(1*Fs));
-
-
1.4.4.3 DVF Plant
-
+
+
1.4.5.3 DVF Plant
+
%% DVF Plant (transfer function from u to dLm)
 G_iff_opt = {};
@@ -784,20 +1293,20 @@ G_iff_opt = {};
-
+

damped_iff_plant_comp_diagonal.png

-

Figure 18: Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)

+

Figure 22: Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)

-
+

damped_iff_plant_comp_off_diagonal.png

-

Figure 19: Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)

+

Figure 23: Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)

-
+

With the IFF control strategy applied and the optimal gain used, the suspension modes are very well damped. Remains the undamped flexible modes of the APA (200Hz, 300Hz, 400Hz), and the modes of the plates (700Hz). @@ -813,36 +1322,36 @@ The Simscape model and the experimental results are in very good agreement.

-
-

1.5 Modal Analysis

+
+

1.5 Modal Analysis

- +

-Several 3-axis accelerometers are fixed on the top platform of the nano-hexapod as shown in Figure 22. +Several 3-axis accelerometers are fixed on the top platform of the nano-hexapod as shown in Figure 26.

-
+

accelerometers_nano_hexapod.jpg

-

Figure 20: Location of the accelerometers on top of the nano-hexapod

+

Figure 24: Location of the accelerometers on top of the nano-hexapod

-The top platform is then excited using an instrumented hammer as shown in Figure 21. +The top platform is then excited using an instrumented hammer as shown in Figure 25.

-
+

hammer_excitation_compliance_meas.jpg

-

Figure 21: Example of an excitation using an instrumented hammer

+

Figure 25: Example of an excitation using an instrumented hammer

-
-

1.5.1 Effectiveness of the IFF Strategy - Compliance

+
+

1.5.1 Effectiveness of the IFF Strategy - Compliance

In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance. @@ -876,19 +1385,19 @@ d_frf_iff = 10/5*(fr

-The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure 22. +The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure 26.

-
+

compliance_vertical_comp_iff.png

-

Figure 22: Measured vertical compliance with and without IFF

+

Figure 26: Measured vertical compliance with and without IFF

-
+

-From Figure 22, it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapode. +From Figure 26, it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapode. It also has the effect of degrading (slightly) the vertical compliance at low frequency.

@@ -900,8 +1409,8 @@ It also seems some damping can be added to the modes at around 205Hz which are f
-
-

1.5.2 Comparison with the Simscape Model

+
+

1.5.2 Comparison with the Simscape Model

Let’s now compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model. @@ -909,52 +1418,52 @@ Let’s now compare the measured vertical compliance with the vertical compl

The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model. -The comparison is done in Figure 23. +The comparison is done in Figure 27. Again, the model is quite accurate!

-
+

compliance_vertical_comp_model_iff.png

-

Figure 23: Measured vertical compliance with and without IFF

+

Figure 27: Measured vertical compliance with and without IFF

-
-

1.5.3 Obtained Mode Shapes

+
+

1.5.3 Obtained Mode Shapes

Then, several excitation are performed using the instrumented Hammer and the mode shapes are extracted.

-We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure 24. +We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure 28.

-
+

mode_shapes_annotated.gif

-

Figure 24: Measured mode shapes for the first six modes

+

Figure 28: Measured mode shapes for the first six modes

-Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure 24). +Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure 29).

-
+

ModeShapeFlex1_crop.gif

-

Figure 25: First flexible mode at 692Hz

+

Figure 29: First flexible mode at 692Hz

-The obtained modes are summarized in Table 2. +The obtained modes are summarized in Table 2.

-
Table 1: List of signals
+
@@ -1018,20 +1527,1691 @@ The obtained modes are summarized in Table 2. + +
+

1.6 Accelerometers fixed on the top platform

+
+ +
+

acc_top_plat_top_view.jpg +

+

Figure 30: Accelerometers fixed on the top platform

+
-
-

2 Encoders fixed to the plates

+
+

1.6.1 Experimental Identification

+
+
+
%% Load Identification Data
+meas_acc = {};
+
+for i = 1:6
+    meas_acc(i) = {load(sprintf('mat/meas_acc_top_plat_strut_%i.mat', i), 't', 'Va', 'de', 'Am')};
+end
+
+
+ +
+
%% Setup useful variables
+% Sampling Time [s]
+Ts = (meas_acc{1}.t(end) - (meas_acc{1}.t(1)))/(length(meas_acc{1}.t)-1);
+
+% Sampling Frequency [Hz]
+Fs = 1/Ts;
+
+% Hannning Windows
+win = hanning(ceil(1*Fs));
+
+% And we get the frequency vector
+[~, f] = tfestimate(meas_acc{1}.Va, meas_acc{1}.de, win, [], [], 1/Ts);
+
+
+ +

+The sensibility of the accelerometers are \(0.1 V/g \approx 0.01 V/(m/s^2)\). +

+
+
%% Compute the 6x6 transfer function matrix
+G_acc = zeros(length(f), 6, 6);
+
+for i = 1:6
+    G_acc(:,:,i) = tfestimate(meas_acc{i}.Va, 1/0.01*meas_acc{i}.Am, win, [], [], 1/Ts);
+end
+
+
+
+
+ +
+

1.6.2 Location and orientation of accelerometers

+
+
+
Opm = [ 0.047, -0.112, 10e-3;
+        0.047, -0.112, 10e-3;
+       -0.113,  0.011, 10e-3;
+       -0.113,  0.011, 10e-3;
+        0.040,  0.113, 10e-3;
+        0.040,  0.113, 10e-3]';
+
+Osm = [-1,  0, 0;
+        0,  0, 1;
+        0, -1, 0;
+        0,  0, 1;
+       -1,  0, 0;
+        0,  0, 1]';
+
+
+
+
+
+ +
+

1.6.3 COM

+
+
+
Hbm = -15e-3;
+
+M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm);
+J = getJacobianNanoHexapod(Hbm);
+
+
+ +
+
G_acc_CoM = zeros(size(G_acc));
+
+for i = 1:length(f)
+    G_acc_CoM(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J');
+end
+
+
+
+
+ +
+

1.6.4 COK

+
+
+
Hbm = -42.3e-3;
+
+M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm);
+J = getJacobianNanoHexapod(Hbm);
+
+
+ +
+
G_acc_CoK = zeros(size(G_acc));
+
+for i = 1:length(f)
+    G_acc_CoK(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J');
+end
+
+
+
+
+ +
+

1.6.5 Comp with the Simscape Model

+
+
+
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible', ...
+                                       'MO_B', -42.3e-3);
+
+
+ +
+
%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
+
+G = linearize(mdl, io, 0.0, options);
+G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
+G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
+
+
+ +

+Then use the Jacobian matrices to obtain the “cartesian” centralized plant. +

+
+
Gc = inv(n_hexapod.geometry.J)*...
+     G*...
+     inv(n_hexapod.geometry.J');
+
+
+
+
+
+
+ +
+

2 Encoders fixed to the plates

- +

+

+In this section, the encoders are fixed to the plates rather than to the struts as shown in Figure 31. +

+ + +
+

IMG_20210625_083801.jpg +

+

Figure 31: Nano-Hexapod with encoders fixed to the struts

+
+ +

+It is structured as follow: +

+
    +
  • Section 2.1: The dynamics of the nano-hexapod is identified
    • +
  • Section 2.2: The identified dynamics is compared with the Simscape model
    • +
  • Section 2.3: The Integral Force Feedback (IFF) control strategy is applied and the dynamics of the damped nano-hexapod is identified and compare with the Simscape model
    • +
  • Section 2.4: The High Authority Control (HAC) in the frame of the struts is developed
    • +
  • Section 2.5: Some reference tracking tests are performed in order to experimentally validate the HAC-LAC control strategy.
    • +
+
+ +
+

2.1 Identification of the dynamics

+
+

+ +

+
+
+

2.1.1 Load Measurement Data

+
+
+
%% Load Identification Data
+meas_data_lf = {};
+
+for i = 1:6
+    meas_data_lf(i) = {load(sprintf('mat/frf_exc_strut_%i_enc_plates_noise.mat', i), 't', 'Va', 'Vs', 'de')};
+end
+
+
+
+
+ +
+

2.1.2 Spectral Analysis - Setup

+
+
+
%% Setup useful variables
+% Sampling Time [s]
+Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
+
+% Sampling Frequency [Hz]
+Fs = 1/Ts;
+
+% Hannning Windows
+win = hanning(ceil(1*Fs));
+
+% And we get the frequency vector
+[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
+
+
+
+
+ +
+

2.1.3 DVF Plant

+
+

+First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 32). +

+ +
+
%% Coherence
+coh_dvf = zeros(length(f), 6, 6);
+
+for i = 1:6
+    coh_dvf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
+end
+
+
+ + +
+

enc_plates_dvf_coh.png +

+

Figure 32: Obtained coherence for the DVF plant

+
+ +

+Then the 6x6 transfer function matrix is estimated (Figure 33). +

+
+
%% DVF Plant (transfer function from u to dLm)
+G_dvf = zeros(length(f), 6, 6);
+
+for i = 1:6
+    G_dvf(:,:,i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
+end
+
+
+ + +
+

enc_plates_dvf_frf.png +

+

Figure 33: Measured FRF for the DVF plant

+
+
+
+ +
+

2.1.4 IFF Plant

+
+

+First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 34). +

+ +
+
%% Coherence for the IFF plant
+coh_iff = zeros(length(f), 6, 6);
+
+for i = 1:6
+    coh_iff(:,:,i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
+end
+
+
+ + +
+

enc_plates_iff_coh.png +

+

Figure 34: Obtained coherence for the IFF plant

+
+ +

+Then the 6x6 transfer function matrix is estimated (Figure 35). +

+
+
%% IFF Plant
+G_iff = zeros(length(f), 6, 6);
+
+for i = 1:6
+    G_iff(:,:,i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
+end
+
+
+ + +
+

enc_plates_iff_frf.png +

+

Figure 35: Measured FRF for the IFF plant

+
+
+
+ +
+

2.1.5 Save Identified Plants

+
+
+
save('matlab/mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
+
+
+
+
+
+ +
+

2.2 Comparison with the Simscape Model

+
+

+ +

+

+In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model. +

+
+
+

2.2.1 Load measured FRF

+
+
+
%% Load data
+load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
+
+
+
+
+ +
+

2.2.2 Dynamics from Actuator to Force Sensors

+
+
+
%% Initialize Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible');
+
+
+ +
+
%% Identify the IFF Plant (transfer function from u to taum)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dum'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
+
+Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+ + +
+

enc_plates_iff_comp_simscape_all.png +

+

Figure 36: IFF Plant for the first actuator input and all the force senosrs

+
+ + +
+

enc_plates_iff_comp_simscape.png +

+

Figure 37: Diagonal elements of the IFF Plant

+
+ + +
+

enc_plates_iff_comp_offdiag_simscape.png +

+

Figure 38: Off diagonal elements of the IFF Plant

+
+
+
+ +
+

2.2.3 Dynamics from Actuator to Encoder

+
+
+
%% Initialization of the Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible');
+
+
+ +
+
%% Identify the DVF Plant (transfer function from u to dLm)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
+
+Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+ + +
+

enc_plates_dvf_comp_simscape_all.png +

+

Figure 39: DVF Plant for the first actuator input and all the encoders

+
+ + +
+

enc_plates_dvf_comp_simscape.png +

+

Figure 40: Diagonal elements of the DVF Plant

+
+ + +
+

enc_plates_dvf_comp_offdiag_simscape.png +

+

Figure 41: Off diagonal elements of the DVF Plant

+
+
+
+
+ +
+

2.3 Integral Force Feedback

+
+

+ +

+ +
+

control_architecture_iff.png +

+

Figure 42: Integral Force Feedback Strategy

+
+
+
+

2.3.1 Identification of the IFF Plant

+
+
+
%% Initialize Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', '2dof');
+
+
+ +
+
%% Identify the IFF Plant (transfer function from u to taum)
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/Fm'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors
+
+Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+
+
+ +
+

2.3.2 Effect of IFF on the plant - Simscape Model

+
+
+
load('Kiff.mat', 'Kiff')
+
+
+ +
+
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder)
+
+
+ +
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible');
+
+
+ +
+
Gd_ol = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+ +
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible', ...
+                                       'controller_type', 'iff');
+
+
+ +
+
Gd_iff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+ +
+1
+
+ + + +
+

enc_plates_iff_gains_effect_dvf_plant.png +

+

Figure 43: Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)

+
+
+
+ +
+

2.3.3 Experimental Results - Damped Plant with Optimal gain

+
+

+Let’s now look at the \(6 \times 6\) damped plant with the optimal gain \(g = 400\). +

+
+ +
+
2.3.3.1 Load Data
+
+
+
%% Load Identification Data
+meas_iff_plates = {};
+
+for i = 1:6
+    meas_iff_plates(i) = {load(sprintf('mat/frf_exc_iff_strut_%i_enc_plates_noise.mat', i), 't', 'Va', 'Vs', 'de', 'u')};
+end
+
+
+
+
+ +
+
2.3.3.2 Spectral Analysis - Setup
+
+
+
%% Setup useful variables
+% Sampling Time [s]
+Ts = (meas_iff_plates{1}.t(end) - (meas_iff_plates{1}.t(1)))/(length(meas_iff_plates{1}.t)-1);
+
+% Sampling Frequency [Hz]
+Fs = 1/Ts;
+
+% Hannning Windows
+win = hanning(ceil(1*Fs));
+
+% And we get the frequency vector
+[~, f] = tfestimate(meas_iff_plates{1}.Va, meas_iff_plates{1}.de, win, [], [], 1/Ts);
+
+
+
+
+ +
+
2.3.3.3 Simscape Model
+
+
+
load('Kiff.mat', 'Kiff')
+
+
+ +
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible', ...
+                                       'controller_type', 'iff');
+
+
+ +
+
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'], 1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder)
+
+
+ +
+
Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+
+
+ +
+
2.3.3.4 DVF Plant
+
+
+
%% IFF Plant
+G_enc_iff_opt = zeros(length(f), 6, 6);
+
+for i = 1:6
+    G_enc_iff_opt(:,:,i) = tfestimate(meas_iff_plates{i}.Va, meas_iff_plates{i}.de, win, [], [], 1/Ts);
+end
+
+
+ + +
+

enc_plates_opt_iff_comp_simscape_all.png +

+

Figure 44: FRF from one actuator to all the encoders when the plant is damped using IFF

+
+ + +
+

damped_iff_plates_plant_comp_diagonal.png +

+

Figure 45: Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)

+
+ + +
+

damped_iff_plates_plant_comp_off_diagonal.png +

+

Figure 46: Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)

+
+
+
+
+ +
+

2.3.4 Effect of IFF on the plant - FRF

+
+
+
load('identified_plants_enc_plates.mat', 'f', 'G_dvf');
+
+
+ + +
+

enc_plant_plates_effect_iff.png +

+

Figure 47: Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)

+
+
+
+ +
+

2.3.5 Save Damped Plant

+
+
+
save('matlab/mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
+
+
+
+
+
+ +
+

2.4 HAC Control - Frame of the struts

+
+

+ +

+

+In a first approximation, the Jacobian matrix can be used instead of using the inverse kinematic equations. +

+ + +
+

control_architecture_hac_iff_L.png +

+

Figure 48: HAC-LAC: IFF + Control in the frame of the legs

+
+
+
+

2.4.1 Simscape Model

+
+

+Let’s start with the Simscape model and the damped plant. +

+ +

+Apply HAC control and verify the system is stable. +

+ +

+Then, try the control strategy on the real plant. +

+
+
load('Kiff.mat', 'Kiff')
+
+
+ +
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible', ...
+                                       'controller_type', 'iff');
+
+
+ +
+
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder)
+
+
+ +
+
Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+
+
+ +
+
isstable(Gd_iff_opt)
+
+
+ +
+1
+
+ + + +
+

hac_iff_struts_enc_plates_plant_bode.png +

+

Figure 49: Transfer function from \(u\) to \(d\mathcal{L}_m\) with IFF (diagonal elements)

+
+
+
+ +
+

2.4.2 HAC Controller

+
+

+Let’s try to have 100Hz bandwidth: +

+
+
%% Lead
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s]
+
+H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+%% Low Pass filter
+H_lpf = 1/(1 + s/2/pi/200);
+
+%% Notch
+gm = 0.02;
+xi = 0.3;
+wn = 2*pi*700;
+
+H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+
+ +
+
Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain
+                  H_lead * ...       % Lead
+                  H_notch * ...      % Notch
+                  (2*pi*100/s) * ... % Integrator
+                  eye(6);            % 6x6 Diagonal
+
+
+ +
+
save('matlab/mat/Khac_iff_struts.mat', 'Khac_iff_struts')
+
+
+ +
+
Lhac_iff_struts = Khac_iff_struts*Gd_iff_opt;
+
+
+ + +
+

loop_gain_hac_iff_struts.png +

+

Figure 50: Diagonal and off-diagonal elements of the Loop gain for “HAC-IFF-Struts”

+
+
+
+ + +
+

2.4.3 Experimental Loop Gain

+
+
    +
  • [ ] Find a more clever way to do the multiplication
    • +
+ +
+
L_frf = zeros(size(G_enc_iff_opt));
+
+for i = 1:size(G_enc_iff_opt, 1)
+    L_frf(i, :, :) = squeeze(G_enc_iff_opt(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
+end
+
+
+ + +
+

hac_iff_plates_exp_loop_gain_diag.png +

+

Figure 51: Diagonal and Off-diagonal elements of the Loop gain (experimental data)

+
+
+
+ +
+

2.4.4 Verification of the Stability using the Simscape model

+
+
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', 'flexible', ...
+                                       'controller_type', 'hac-iff-struts');
+
+
+ +
+
Gd_iff_hac_opt = linearize(mdl, io, 0.0, options);
+
+
+ +
+
isstable(Gd_iff_hac_opt)
+
+
+ +
+1
+
+
+
+
+ +
+

2.5 Reference Tracking

+
+

+ +

+
+
+

2.5.1 Load

+
+
+
load('Khac_iff_struts.mat', 'Khac_iff_struts')
+
+
+
+
+ +
+

2.5.2 Y-Z Scans

+
+
+
+
2.5.2.1 Generate the Scan
+
+
+
Rx_yz = generateYZScanTrajectory(...
+    'y_tot', 4e-6, ...
+    'z_tot', 8e-6, ...
+    'n', 5, ...
+    'Ts', 1e-3, ...
+    'ti', 2, ...
+    'tw', 0.5, ...
+    'ty', 2, ...
+    'tz', 1);
+
+
+ +
+
figure;
+hold on;
+plot(Rx_yz(:,1), Rx_yz(:,3), ...
+     'DisplayName', 'Y motion')
+plot(Rx_yz(:,1), Rx_yz(:,4), ...
+     'DisplayName', 'Z motion')
+hold off;
+xlabel('Time [s]');
+ylabel('Displacement [m]');
+legend('location', 'northeast');
+
+
+ +
+
figure;
+plot(Rx_yz(:,3), Rx_yz(:,4));
+xlabel('y [m]'); ylabel('z [m]');
+
+
+
+
+ +
+
2.5.2.2 Reference Signal for the Strut lengths
+
+

+Let’s use the Jacobian to estimate the wanted strut length as a function of time. +

+
+
dL_ref = [n_hexapod.geometry.J*Rx_yz(:, 2:7)']';
+
+
+ +
+
figure;
+hold on;
+for i=1:6
+    plot(Rx_yz(:,1), dL_ref(:, i))
+end
+xlabel('Time [s]'); ylabel('Displacement [m]');
+
+
+
+
+ +
+
2.5.2.3 Time domain simulation with 2DoF model
+
+
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ...
+                                       'flex_top_type', '3dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', '2dof', ...
+                                       'controller_type', 'hac-iff-struts');
+
+
+ +
+
set_param(mdl,'StopTime', num2str(Rx_yz(end,1)))
+set_param(mdl,'SimulationCommand','start')
+
+
+ +
+
out.X.Data = out.X.Data - out.X.Data(1,:);
+
+
+ + +
+

ref_track_hac_iff_struts_yz_plane.png +

+

Figure 52: Simulated Y-Z motion

+
+ + +
+

ref_track_hac_iff_struts_yz_time.png +

+

Figure 53: Y and Z motion as a function of time as well as the reference signals

+
+ + +
+

ref_track_hac_iff_struts_pos_error.png +

+

Figure 54: Positioning errors as a function of time

+
+
+
+
+ +
+

2.5.3 “NASS” reference path

+
+
+
+
2.5.3.1 Generate Path
+
+
+
ref_path = [ ...
+    0,  0, 0;
+    0,  0, 1; % N
+    0,  4, 1;
+    3,  0, 1;
+    3,  4, 1;
+    3,  4, 0;
+    4,  0, 0;
+    4,  0, 1; % A
+    4,  3, 1;
+    5,  4, 1;
+    6,  4, 1;
+    7,  3, 1;
+    7,  2, 1;
+    4,  2, 1;
+    4,  3, 1;
+    5,  4, 1;
+    6,  4, 1;
+    7,  3, 1;
+    7,  0, 1;
+    7,  0, 0;
+    8,  0, 0;
+    8,  0, 1; % S
+    11, 0, 1;
+    11, 2, 1;
+    8,  2, 1;
+    8,  4, 1;
+    11, 4, 1;
+    11, 4, 0;
+    12, 0, 0;
+    12, 0, 1; % S
+    15, 0, 1;
+    15, 2, 1;
+    12, 2, 1;
+    12, 4, 1;
+    15, 4, 1;
+    15, 4, 0;
+           ];
+
+% Center the trajectory arround zero
+ref_path = ref_path - (max(ref_path) - min(ref_path))/2;
+
+% Define the X-Y-Z cuboid dimensions containing the trajectory
+X_max = 10e-6;
+Y_max =  4e-6;
+Z_max =  2e-6;
+
+ref_path = ([X_max, Y_max, Z_max]./max(ref_path)).*ref_path; % [m]
+
+
+ +
+
Rx_nass = generateXYZTrajectory('points', ref_path);
+
+
+ +
+
figure;
+plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k.')
+xlabel('X [$\mu m$]');
+ylabel('Y [$\mu m$]');
+axis equal;
+xlim(1e6*[min(Rx_nass(:,2)), max(Rx_nass(:,2))]);
+ylim(1e6*[min(Rx_nass(:,3)), max(Rx_nass(:,3))]);
+
+
+ + +
+

ref_track_test_nass.png +

+

Figure 55: Reference path corresponding to the “NASS” acronym

+
+ +
+
figure;
+plot3(Rx_nass(:,2), Rx_nass(:,3), Rx_nass(:,4), 'k-');
+xlabel('x');
+ylabel('y');
+zlabel('z');
+
+
+ +
+
figure;
+hold on;
+plot(Rx_nass(:,1), Rx_nass(:,2));
+plot(Rx_nass(:,1), Rx_nass(:,3));
+plot(Rx_nass(:,1), Rx_nass(:,4));
+hold off;
+
+
+ +
+
figure;
+scatter(Rx_nass(:,2), Rx_nass(:,3), 1, Rx_nass(:,4), 'filled')
+colormap winter
+
+
+
+
+
+
2.5.3.2 Simscape Simulations
+
+
+
%% Initialize the Simscape model in closed loop
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ...
+                                       'flex_top_type', '3dof', ...
+                                       'motion_sensor_type', 'plates', ...
+                                       'actuator_type', '2dof', ...
+                                       'controller_type', 'hac-iff-struts');
+
+
+ +
+
set_param(mdl,'StopTime', num2str(Rx_nass(end,1)))
+set_param(mdl,'SimulationCommand','start')
+
+
+ +
+
out.X.Data = out.X.Data - out.X.Data(1,:);
+
+
+
+
+
+ +
+

2.5.4 Save Reference paths

+
+
+
save('matlab/mat/reference_path.mat', 'Rx_yz', 'Rx_nass')
+
+
+
+
+ +
+

2.5.5 Experimental Results

+
+
+ +
+

2.6 Feedforward (Open-Loop) Control

+
+
+
+

2.6.1 Introduction

+
+ +
+

control_architecture_iff_feedforward.png +

+

Figure 56: Feedforward control in the frame of the legs

+
+
+
+ +
+

2.6.2 Simple Feedforward Controller

+
+

+Let’s estimate the mean DC gain for the damped plant (diagonal elements:) +

+
+1.773e-05
+
+ + +

+The feedforward controller is then taken as the inverse of this gain (the minus sign is there manually added as it is “removed” by the abs function): +

+
+
Kff_iff_L = -1/mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:))))));
+
+
+ +

+The open-loop gain (feedforward controller times the damped plant) is shown in Figure 57. +

+ + +
+

open_loop_gain_feedforward_iff_struts.png +

+

Figure 57: Diagonal elements of the “open loop gain”

+
+ +

+And save the feedforward controller for further use: +

+
+
Kff_iff_L = zpk(Kff_iff_L)*eye(6);
+
+
+ +
+
save('matlab/mat/feedforward_iff.mat', 'Kff_iff_L')
+
+
+
+
+ +
+

2.6.3 Test with Simscape Model

+
+
+
load('reference_path.mat', 'Rx_yz');
+
+
+
+
+
+ +
+

2.7 Feedback/Feedforward control in the frame of the struts

+
+ +
+

control_architecture_hac_iff_L_feedforward.png +

+

Figure 58: Feedback/Feedforward control in the frame of the legs

+
+
+
+
+ + +
+

3 Functions

+
+
+
+

3.1 generateXYZTrajectory

+
+

+ +

+
+ +
+

Function description

+
+
+
function [ref] = generateXYZTrajectory(args)
+% generateXYZTrajectory -
+%
+% Syntax: [ref] = generateXYZTrajectory(args)
+%
+% Inputs:
+%    - args
+%
+% Outputs:
+%    - ref - Reference Signal
+
+
+
+
+ +
+

Optional Parameters

+
+
+
arguments
+    args.points double {mustBeNumeric} = zeros(2, 3) % [m]
+
+    args.ti    (1,1) double {mustBeNumeric, mustBePositive} = 1 % Time to go to first point and after last point [s]
+    args.tw    (1,1) double {mustBeNumeric, mustBePositive} = 0.5 % Time wait between each point [s]
+    args.tm    (1,1) double {mustBeNumeric, mustBePositive} = 1 % Motion time between points [s]
+
+    args.Ts    (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s]
+end
+
+
+
+
+ +
+

Initialize Time Vectors

+
+
+
time_i = 0:args.Ts:args.ti;
+time_w = 0:args.Ts:args.tw;
+time_m = 0:args.Ts:args.tm;
+
+
+
+
+ +
+

XYZ Trajectory

+
+
+
% Go to initial position
+xyz = (args.points(1,:))'*(time_i/args.ti);
+
+% Wait
+xyz = [xyz, xyz(:,end).*ones(size(time_w))];
+
+% Scans
+for i = 2:size(args.points, 1)
+    % Go to next point
+    xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)];
+    % Wait a litle bit
+    xyz = [xyz, xyz(:,end).*ones(size(time_w))];
+end
+
+% End motion
+xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)];
+
+
+
+
+ +
+

Reference Signal

+
+
+
t = 0:args.Ts:args.Ts*(length(xyz) - 1);
+
+
+ +
+
ref = zeros(length(xyz), 7);
+
+ref(:, 1) = t;
+ref(:, 2:4) = xyz';
+
+
+
+
+
+ +
+

3.2 generateYZScanTrajectory

+
+

+ +

+
+ +
+

Function description

+
+
+
function [ref] = generateYZScanTrajectory(args)
+% generateYZScanTrajectory -
+%
+% Syntax: [ref] = generateYZScanTrajectory(args)
+%
+% Inputs:
+%    - args
+%
+% Outputs:
+%    - ref - Reference Signal
+
+
+
+
+ +
+

Optional Parameters

+
+
+
arguments
+    args.y_tot (1,1) double {mustBeNumeric} = 10e-6 % [m]
+    args.z_tot (1,1) double {mustBeNumeric} = 10e-6 % [m]
+
+    args.n     (1,1) double {mustBeInteger, mustBePositive} = 10 % [-]
+
+    args.Ts    (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s]
+
+    args.ti    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+    args.tw    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+    args.ty    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+    args.tz    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+end
+
+
+
+
+ +
+

Initialize Time Vectors

+
+
+
time_i = 0:args.Ts:args.ti;
+time_w = 0:args.Ts:args.tw;
+time_y = 0:args.Ts:args.ty;
+time_z = 0:args.Ts:args.tz;
+
+
+
+
+ +
+

Y and Z vectors

+
+
+
% Go to initial position
+y = (time_i/args.ti)*(args.y_tot/2);
+
+% Wait
+y = [y, y(end)*ones(size(time_w))];
+
+% Scans
+for i = 1:args.n
+    if mod(i,2) == 0
+        y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot];
+    else
+        y = [y,  (args.y_tot/2) - (time_y/args.ty)*args.y_tot];
+    end
+
+    if i < args.n
+        y = [y, y(end)*ones(size(time_z))];
+    end
+end
+
+% Wait a litle bit
+y = [y, y(end)*ones(size(time_w))];
+
+% End motion
+y = [y, y(end) - y(end)*time_i/args.ti];
+
+
+ +
+
% Go to initial position
+z = (time_i/args.ti)*(args.z_tot/2);
+
+% Wait
+z = [z, z(end)*ones(size(time_w))];
+
+% Scans
+for i = 1:args.n
+    z = [z, z(end)*ones(size(time_y))];
+
+    if i < args.n
+        z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)];
+    end
+end
+
+% Wait a litle bit
+z = [z, z(end)*ones(size(time_w))];
+
+% End motion
+z = [z, z(end) - z(end)*time_i/args.ti];
+
+
+
+
+ +
+

Reference Signal

+
+
+
t = 0:args.Ts:args.Ts*(length(y) - 1);
+
+
+ +
+
ref = zeros(length(y), 7);
+
+ref(:, 1) = t;
+ref(:, 3) = y;
+ref(:, 4) = z;
+
+
+
+
+
+ +
+

3.3 getTransformationMatrixAcc

+
+

+ +

+
+ +
+

Function description

+
+
+
function [M] = getTransformationMatrixAcc(Opm, Osm)
+% getTransformationMatrixAcc -
+%
+% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm)
+%
+% Inputs:
+%    - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers
+%    - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation
+%
+% Outputs:
+%    - M - Transformation Matrix
+
+
+
+
+ +
+

Transformation matrix from motion of the solid body to accelerometer measurements

+
+

+Let’s try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in \(\{O\}\). +For any point \(p_i\) of the solid body (corresponding to an accelerometer), we can write: +

+\begin{equation} +\begin{bmatrix} +a_{i,x} \\ a_{i,y} \\ a_{i,z} +\end{bmatrix} = \begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z +\end{bmatrix} + p_i \times \begin{bmatrix} +\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} +\end{equation} + +

+We can write the cross product as a matrix product using the skew-symmetric transformation: +

+\begin{equation} +\begin{bmatrix} +a_{i,x} \\ a_{i,y} \\ a_{i,z} +\end{bmatrix} = \begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z +\end{bmatrix} + \underbrace{\begin{bmatrix} + 0 & p_{i,z} & -p_{i,y} \\ + -p_{i,z} & 0 & p_{i,x} \\ + p_{i,y} & -p_{i,x} & 0 +\end{bmatrix}}_{P_{i,[\times]}} \cdot \begin{bmatrix} +\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} +\end{equation} + +

+If we now want to know the (scalar) acceleration \(a_i\) of the point \(p_i\) in the direction of the accelerometer direction \(\hat{s}_i\), we can just project the 3d acceleration on \(\hat{s}_i\): +

+\begin{equation} +a_i = \hat{s}_i^T \cdot \begin{bmatrix} +a_{i,x} \\ a_{i,y} \\ a_{i,z} +\end{bmatrix} = \hat{s}_i^T \cdot \begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z +\end{bmatrix} + \left( \hat{s}_i^T \cdot P_{i,[\times]} \right) \cdot \begin{bmatrix} +\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} +\end{equation} + +

+Which is equivalent as a simple vector multiplication: +

+\begin{equation} +a_i = \begin{bmatrix} +\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]} +\end{bmatrix} +\begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} = \begin{bmatrix} +\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]} +\end{bmatrix} {}^O\vec{x} +\end{equation} + +

+And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form. +We obtain Eq. \eqref{eq:M_matrix}. +

+
+

+The transformation from solid body acceleration \({}^O\vec{x}\) from sensor measured acceleration \(\vec{a}\) is: +

+\begin{equation} \label{eq:M_matrix} +\vec{a} = \underbrace{\begin{bmatrix} +\hat{s}_1^T & \hat{s}_1^T \cdot P_{1,[\times]} \\ +\vdots & \vdots \\ +\hat{s}_6^T & \hat{s}_6^T \cdot P_{6,[\times]} +\end{bmatrix}}_{M} {}^O\vec{x} +\end{equation} + +

+with \(\hat{s}_i\) the unit vector representing the measured direction of the i’th accelerometer expressed in frame \(\{O\}\) and \(P_{i,[\times]}\) the skew-symmetric matrix representing the cross product of the position of the i’th accelerometer expressed in frame \(\{O\}\). +

+ +
+ +

+Let’s define such matrix using matlab: +

+
+
M = zeros(length(Opm), 6);
+
+for i = 1:length(Opm)
+    Ri = [0,         Opm(3,i), -Opm(2,i);
+         -Opm(3,i),  0,         Opm(1,i);
+          Opm(2,i), -Opm(1,i),  0];
+    M(i, 1:3) = Osm(:,i)';
+    M(i, 4:6) = Osm(:,i)'*Ri;
+end
+
+
+ +
+
end
+
+
+
+
+
+ + +
+

3.4 getJacobianNanoHexapod

+
+

+ +

+
+ +
+

Function description

+
+
+
function [J] = getJacobianNanoHexapod(Hbm)
+% getJacobianNanoHexapod -
+%
+% Syntax: [J] = getJacobianNanoHexapod(Hbm)
+%
+% Inputs:
+%    - Hbm - Height of {B} w.r.t. {M} [m]
+%
+% Outputs:
+%    - J - Jacobian Matrix
+
+
+
+
+ +
+

Transformation matrix from motion of the solid body to accelerometer measurements

+
+
+
Fa = [[-86.05,  -74.78, 22.49],
+      [ 86.05,  -74.78, 22.49],
+      [ 107.79, -37.13, 22.49],
+      [ 21.74,  111.91, 22.49],
+      [-21.74,  111.91, 22.49],
+      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
+
+Mb = [[-28.47, -106.25, -22.50],
+      [ 28.47, -106.25, -22.50],
+      [ 106.25, 28.47,  -22.50],
+      [ 77.78,  77.78,  -22.50],
+      [-77.78,  77.78,  -22.50],
+      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
+
+H = 95e-3; % Stewart platform height [m]
+Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m]
+
+si = Fb - Fa;
+si = si./vecnorm(si); % Normalize
+
+Bb = Mb - [0; 0; Hbm];
+
+J = [si', cross(Bb, si)'];
+
+
+
+

Author: Dehaeze Thomas

-

Created: 2021-06-14 lun. 18:07

+

Created: 2021-06-30 mer. 22:41

diff --git a/test-bench-nano-hexapod.org b/test-bench-nano-hexapod.org index 36a5366..6979056 100644 --- a/test-bench-nano-hexapod.org +++ b/test-bench-nano-hexapod.org @@ -154,7 +154,11 @@ It is divided in the following sections: - Section [[sec:enc_struts_iff]]: decentralized Integral Force Feedback (IFF) is applied and its performances are evaluated. - Section [[sec:enc_struts_modal_analysis]]: a modal analysis of the nano-hexapod is performed -** Matlab Init :noexport:ignore: +** Identification of the dynamics +<> +*** Introduction :ignore: + +*** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src @@ -174,9 +178,7 @@ addpath('./mat/'); addpath('./src/'); #+end_src -** Identification of the dynamics -<> -*** Load Data +*** Load Measurement Data #+begin_src matlab %% Load Identification Data meas_data_lf = {}; @@ -211,12 +213,12 @@ First, let's compute the coherence from the excitation voltage and the displacem #+begin_src matlab %% Coherence -coh_dvf_lf = zeros(length(f), 6, 6); -coh_dvf_hf = zeros(length(f), 6, 6); +coh_dvf = zeros(length(f), 6, 6); for i = 1:6 - coh_dvf_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts); - coh_dvf_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts); + coh_dvf_lf = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts); + coh_dvf_hf = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts); + coh_dvf(:,:,i) = [coh_dvf_lf(i_lf, :); coh_dvf_hf(i_hf, :)]; end #+end_src @@ -226,21 +228,16 @@ figure; hold on; for i = 1:5 for j = i+1:6 - plot(f(i_lf), coh_dvf_lf(i_lf, i, j), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), coh_dvf_hf(i_hf, i, j), 'color', [0, 0, 0, 0.2], ... + plot(f, coh_dvf(:, i, j), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), coh_dvf_lf(i_lf,i, i), ... + plot(f, coh_dvf(:, i, i), ... 'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i)); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), coh_dvf_hf(i_hf,i, i), ... - 'HandleVisibility', 'off'); end -plot(f(i_lf), coh_dvf_lf(i_lf, 1, 2), 'color', [0, 0, 0, 0.2], ... +plot(f, coh_dvf(:, 1, 2), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$G_{dvf}(i,j)$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -261,12 +258,12 @@ exportFig('figs/enc_struts_dvf_coh.pdf', 'width', 'wide', 'height', 'normal'); Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_dvf_frf]]). #+begin_src matlab %% DVF Plant (transfer function from u to dLm) -G_dvf_lf = zeros(length(f), 6, 6); -G_dvf_hf = zeros(length(f), 6, 6); +G_dvf = zeros(length(f), 6, 6); for i = 1:6 - G_dvf_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts); - G_dvf_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts); + G_dvf_lf = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts); + G_dvf_hf = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts); + G_dvf(:,:,i) = [G_dvf_lf(i_lf, :); G_dvf_hf(i_hf, :)]; end #+end_src @@ -279,21 +276,19 @@ ax1 = nexttile([2,1]); hold on; for i = 1:5 for j = i+1:6 - plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ... + plot(f, abs(G_dvf(:, i, j)), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ... + plot(f, abs(G_dvf(:,i, i)), ... 'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i)); set(gca,'ColorOrderIndex',i) - plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ... + plot(f, abs(G_dvf(:,i, i)), ... 'HandleVisibility', 'off'); end -plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ... +plot(f, abs(G_dvf(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$G_{dvf}(i,j)$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -305,9 +300,7 @@ ax2 = nexttile; hold on; for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i))); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i))); + plot(f, 180/pi*angle(G_dvf(:,i, i))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -328,20 +321,18 @@ exportFig('figs/enc_struts_dvf_frf.pdf', 'width', 'wide', 'height', 'tall'); #+RESULTS: [[file:figs/enc_struts_dvf_frf.png]] - *** IFF Plant First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_iff_coh]]). #+begin_src matlab %% Coherence for the IFF plant -coh_iff_lf = zeros(length(f), 6, 6); -coh_iff_hf = zeros(length(f), 6, 6); +coh_iff = zeros(length(f), 6, 6); for i = 1:6 - coh_iff_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts); - coh_iff_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts); + coh_iff_lf = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts); + coh_iff_hf = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts); + coh_iff(:,:,i) = [coh_iff_lf(i_lf, :); coh_iff_hf(i_hf, :)]; end - #+end_src #+begin_src matlab :exports none @@ -350,21 +341,16 @@ figure; hold on; for i = 1:5 for j = i+1:6 - plot(f(i_lf), coh_iff_lf(i_lf, i, j), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), coh_iff_hf(i_hf, i, j), 'color', [0, 0, 0, 0.2], ... + plot(f, coh_iff(:, i, j), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), coh_iff_lf(i_lf,i, i), ... + plot(f, coh_iff(:,i, i), ... 'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i)); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), coh_iff_hf(i_hf,i, i), ... - 'HandleVisibility', 'off'); end -plot(f(i_lf), coh_iff_lf(i_lf, 1, 2), 'color', [0, 0, 0, 0.2], ... +plot(f, coh_iff(:, 1, 2), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$G_{iff}(i,j)$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -385,12 +371,12 @@ exportFig('figs/enc_struts_iff_coh.pdf', 'width', 'wide', 'height', 'normal'); Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_iff_frf]]). #+begin_src matlab %% IFF Plant -G_iff_lf = zeros(length(f), 6, 6); -G_iff_hf = zeros(length(f), 6, 6); +G_iff = zeros(length(f), 6, 6); for i = 1:6 - G_iff_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts); - G_iff_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts); + G_iff_lf = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts); + G_iff_hf = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts); + G_iff(:,:,i) = [G_iff_lf(i_lf, :); G_iff_hf(i_hf, :)]; end #+end_src @@ -403,21 +389,16 @@ ax1 = nexttile([2,1]); hold on; for i = 1:5 for j = i+1:6 - plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ... + plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), ... + plot(f, abs(G_iff(:,i , i)), ... 'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i)); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), ... - 'HandleVisibility', 'off'); end -plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ... +plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$G_{iff}(i,j)$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -429,9 +410,7 @@ ax2 = nexttile; hold on; for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i))); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i))); + plot(f, 180/pi*angle(G_iff(:,i, i))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -452,6 +431,15 @@ exportFig('figs/enc_struts_iff_frf.pdf', 'width', 'wide', 'height', 'tall'); #+RESULTS: [[file:figs/enc_struts_iff_frf.png]] +*** Save Identified Plants +#+begin_src matlab :tangle no +save('matlab/mat/identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + ** Jacobian :noexport: *** Introduction :ignore: The Jacobian is used to transform the excitation force in the cartesian frame as well as the displacements. @@ -494,12 +482,36 @@ First, we load the Jacobian matrix (same for the actuators and sensors). load('jacobian.mat', 'J'); #+end_src +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab +load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +load('jacobian.mat', 'J'); +#+end_src + *** DVF Plant The transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}}$ is computed and shown in Figure [[fig:enc_struts_dvf_cart_frf]]. #+begin_src matlab -G_dvf_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_lf, [2 3 1]), inv(J'))), [3 1 2]); -G_dvf_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_hf, [2 3 1]), inv(J'))), [3 1 2]); +G_dvf_J = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf, [2 3 1]), inv(J'))), [3 1 2]); #+end_src #+begin_src matlab :exports none @@ -512,21 +524,16 @@ ax1 = nexttile([2,1]); hold on; for i = 1:5 for j = i+1:6 - plot(f(i_lf), abs(G_dvf_J_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), abs(G_dvf_J_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ... + plot(f, abs(G_dvf_J(:, i, j)), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), abs(G_dvf_J_lf(i_lf,i, i)), ... + plot(f, abs(G_dvf_J(:,i , i)), ... 'DisplayName', labels{i}); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), abs(G_dvf_J_hf(i_hf,i, i)), ... - 'HandleVisibility', 'off'); end -plot(f(i_lf), abs(G_dvf_J_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ... +plot(f, abs(G_dvf_J(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_i/F_j$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -538,9 +545,7 @@ ax2 = nexttile; hold on; for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), 180/pi*angle(G_dvf_J_lf(i_lf,i, i))); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), 180/pi*angle(G_dvf_J_hf(i_hf,i, i))); + plot(f, 180/pi*angle(G_dvf_J(:,i , i))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -565,8 +570,7 @@ exportFig('figs/enc_struts_dvf_cart_frf.pdf', 'width', 'wide', 'height', 'tall') The transfer function from $\bm{\mathcal{F}}$ to $\bm{\mathcal{F}}_m$ is computed and shown in Figure [[fig:enc_struts_iff_cart_frf]]. #+begin_src matlab -G_iff_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_lf, [2 3 1]), inv(J'))), [3 1 2]); -G_iff_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_hf, [2 3 1]), inv(J'))), [3 1 2]); +G_iff_J = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff, [2 3 1]), inv(J'))), [3 1 2]); #+end_src #+begin_src matlab :exports none @@ -579,21 +583,16 @@ ax1 = nexttile([2,1]); hold on; for i = 1:5 for j = i+1:6 - plot(f(i_lf), abs(G_iff_J_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), abs(G_iff_J_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ... + plot(f, abs(G_iff_J(:, i, j)), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), abs(G_iff_J_lf(i_lf,i, i)), ... + plot(f, abs(G_iff_J(:,i, i)), ... 'DisplayName', labels{i}); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), abs(G_iff_J_hf(i_hf,i, i)), ... - 'HandleVisibility', 'off'); end -plot(f(i_lf), abs(G_iff_J_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ... +plot(f, abs(G_iff_J(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_i/F_j$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -605,9 +604,7 @@ ax2 = nexttile; hold on; for i =1:6 set(gca,'ColorOrderIndex',i) - plot(f(i_lf), 180/pi*angle(G_iff_J_lf(i_lf,i, i))); - set(gca,'ColorOrderIndex',i) - plot(f(i_hf), 180/pi*angle(G_iff_J_hf(i_hf,i, i))); + plot(f, 180/pi*angle(G_iff_J(:,i, i))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -633,10 +630,28 @@ exportFig('figs/enc_struts_iff_cart_frf.pdf', 'width', 'wide', 'height', 'tall') *** Introduction :ignore: In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model. -*** Initialize :noexport: +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + #+begin_src matlab :tangle no %% Add all useful folders to the path -addpath('matlab/') addpath('matlab/nass-simscape/matlab/nano_hexapod/') addpath('matlab/nass-simscape/STEPS/nano_hexapod/') addpath('matlab/nass-simscape/STEPS/png/') @@ -660,9 +675,18 @@ mdl = 'nano_hexapod_simscape'; options = linearizeOptions; options.SampleTime = 0; +Rx = zeros(1, 7); + open(mdl) #+end_src +*** Load measured FRF + +#+begin_src matlab +%% Load data +load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + *** Dynamics from Actuator to Force Sensors #+begin_src matlab %% Initialize Nano-Hexapod @@ -675,8 +699,8 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... #+begin_src matlab %% Identify the IFF Plant (transfer function from u to taum) clear io; io_i = 1; -io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); #+end_src @@ -690,14 +714,11 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; -plot(f(i_lf), abs(G_iff_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ... +plot(f, abs(G_iff(:,1, 1)), 'color', [0,0,0,0.2], ... 'DisplayName', '$\tau_{m,i}/u_i$ - FRF') for i = 2:6 set(gca,'ColorOrderIndex',2) - plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - set(gca,'ColorOrderIndex',2) - plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ... + plot(f, abs(G_iff(:,i, i)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); end set(gca,'ColorOrderIndex',2); @@ -716,8 +737,7 @@ legend('location', 'southeast'); ax2 = nexttile; hold on; for i = 1:6 - plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]); - plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2]); + plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [0,0,0,0.2]); end for i = 1:6 set(gca,'ColorOrderIndex',2); @@ -749,13 +769,11 @@ freqs = 2*logspace(1, 3, 1000); figure; hold on; % Off diagonal terms -plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), 'color', [0,0,0,0.2], ... +plot(f, abs(G_iff(:, 1, 2)), 'color', [0,0,0,0.2], ... 'DisplayName', '$\tau_{m,i}/u_j$ - FRF') for i = 1:5 for j = i+1:6 - plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0,0,0,0.2], ... + plot(f, abs(G_iff(:, i, j)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); end end @@ -791,13 +809,13 @@ exportFig('figs/enc_struts_iff_comp_offdiag_simscape.pdf', 'width', 'wide', 'hei n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '4dof', ... 'motion_sensor_type', 'struts', ... - 'actuator_type', '2dof'); + 'actuator_type', 'flexible'); #+end_src #+begin_src matlab %% Identify the DVF Plant (transfer function from u to dLm) clear io; io_i = 1; -io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options); @@ -812,14 +830,11 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; -plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ... +plot(f, abs(G_dvf(:,1, 1)), 'color', [0,0,0,0.2], ... 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF') for i = 2:6 set(gca,'ColorOrderIndex',2) - plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - set(gca,'ColorOrderIndex',2) - plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ... + plot(f, abs(G_dvf(:,i, i)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); end set(gca,'ColorOrderIndex',2); @@ -839,8 +854,7 @@ legend('location', 'northeast'); ax2 = nexttile; hold on; for i = 1:6 - plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2]); - plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)), 'color', [0,0,0,0.2]); + plot(f, 180/pi*angle(G_dvf(:,i, i)), 'color', [0,0,0,0.2]); end for i = 1:6 set(gca,'ColorOrderIndex',2); @@ -872,13 +886,11 @@ freqs = 2*logspace(1, 3, 1000); figure; hold on; % Off diagonal terms -plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), 'color', [0,0,0,0.2], ... +plot(f, abs(G_dvf(:, 1, 2)), 'color', [0,0,0,0.2], ... 'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF') for i = 1:5 for j = i+1:6 - plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), 'color', [0,0,0,0.2], ... + plot(f, abs(G_dvf(:, i, j)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); end end @@ -908,15 +920,826 @@ exportFig('figs/enc_struts_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'hei #+RESULTS: [[file:figs/enc_struts_dvf_comp_offdiag_simscape.png]] +*** Effect of a change in bending damping of the joints +#+begin_src matlab +%% Tested bending dampings [Nm/(rad/s)] +cRs = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1]; +#+end_src + +#+begin_src matlab +%% Identify the DVF Plant (transfer function from u to dLm) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders +#+end_src + +Then the identification is performed for all the values of the bending damping. +#+begin_src matlab +%% Idenfity the transfer function from actuator to encoder for all bending dampins +Gs = {zeros(length(cRs), 1)}; + +for i = 1:length(cRs) + n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible', ... + 'flex_bot_cRx', cRs(i), ... + 'flex_bot_cRy', cRs(i), ... + 'flex_top_cRx', cRs(i), ... + 'flex_top_cRy', cRs(i)); + + G = exp(-s*Ts)*linearize(mdl, io, 0.0, options); + G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'}; + G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'}; + + Gs(i) = {G}; +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the obtained direct transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:length(cRs) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); + +ax2 = nexttile; +hold on; +for i = 1:length(cRs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz')))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :exports none +%% Plot the obtained coupling transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +hold on; +for i = 1:length(cRs) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); +xlim([20, 2e3]); +#+end_src + +- Could be nice +- Actual damping is very small + +*** Effect of a change in damping factor of the APA +#+begin_src matlab +%% Tested bending dampings [Nm/(rad/s)] +xis = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1]; +#+end_src + +#+begin_src matlab +%% Identify the DVF Plant (transfer function from u to dLm) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders +#+end_src + +#+begin_src matlab +%% Idenfity the transfer function from actuator to encoder for all bending dampins +Gs = {zeros(length(xis), 1)}; + +for i = 1:length(xis) + n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible', ... + 'actuator_xi', xis(i)); + + G = exp(-s*Ts)*linearize(mdl, io, 0.0, options); + G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'}; + G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'}; + + Gs(i) = {G}; +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the obtained direct transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:length(xis) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$\\xi = %.3f$', xis(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); + +ax2 = nexttile; +hold on; +for i = 1:length(xis) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz')))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_Va_dL_effect_xi_damp.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:bode_Va_dL_effect_xi_damp +#+caption: Effect of the APA damping factor $\xi$ on the dynamics from $u$ to $d\mathcal{L}$ +#+RESULTS: +[[file:figs/bode_Va_dL_effect_xi_damp.png]] + +#+begin_src matlab :exports none +%% Plot the obtained coupling transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +hold on; +for i = 1:length(xis) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', xis(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); +xlim([20, 2e3]); +#+end_src + +#+begin_important +Damping factor $\xi$ has a large impact on the damping of the "spurious resonances" at 200Hz and 300Hz. +#+end_important + +#+begin_question +Why is the damping factor does not change the damping of the first peak? +#+end_question + +*** Effect of a change in stiffness damping coef of the APA +#+begin_src matlab +m_coef = 1e1; +#+end_src + +#+begin_src matlab +%% Tested bending dampings [Nm/(rad/s)] +k_coefs = [1e-6, 5e-6, 1e-5, 5e-5, 1e-4]; +#+end_src + +#+begin_src matlab +%% Identify the DVF Plant (transfer function from u to dLm) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders +#+end_src + +#+begin_src matlab +%% Idenfity the transfer function from actuator to encoder for all bending dampins +Gs = {zeros(length(k_coefs), 1)}; +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible'); + +for i = 1:length(k_coefs) + k_coef = k_coefs(i); + + G = exp(-s*Ts)*linearize(mdl, io, 0.0, options); + G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'}; + G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'}; + + Gs(i) = {G}; +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the obtained direct transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:length(k_coefs) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('kcoef = %.0e', k_coefs(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); + +ax2 = nexttile; +hold on; +for i = 1:length(k_coefs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz')))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_Va_dL_effect_k_coef.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:bode_Va_dL_effect_k_coef +#+caption: Effect of a change of the damping "stiffness coeficient" on the transfer function from $u$ to $d\mathcal{L}$ +#+RESULTS: +[[file:figs/bode_Va_dL_effect_k_coef.png]] + +#+begin_src matlab :exports none +%% Plot the obtained coupling transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +hold on; +for i = 1:length(xis) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', xis(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); +xlim([20, 2e3]); +#+end_src + +*** Effect of a change in mass damping coef of the APA +#+begin_src matlab +k_coef = 1e-6; +#+end_src + +#+begin_src matlab +%% Tested bending dampings [Nm/(rad/s)] +m_coefs = [1e1, 5e1, 1e2, 5e2, 1e3]; +#+end_src + +#+begin_src matlab +%% Identify the DVF Plant (transfer function from u to dLm) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders +#+end_src + +#+begin_src matlab +%% Idenfity the transfer function from actuator to encoder for all bending dampins +Gs = {zeros(length(m_coefs), 1)}; +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible'); + +for i = 1:length(m_coefs) + m_coef = m_coefs(i); + + G = exp(-s*Ts)*linearize(mdl, io, 0.0, options); + G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'}; + G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'}; + + Gs(i) = {G}; +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the obtained direct transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:length(m_coefs) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('mcoef = %.0e', m_coefs(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); + +ax2 = nexttile; +hold on; +for i = 1:length(m_coefs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz')))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_Va_dL_effect_m_coef.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:bode_Va_dL_effect_m_coef +#+caption: Effect of a change of the damping "mass coeficient" on the transfer function from $u$ to $d\mathcal{L}$ +#+RESULTS: +[[file:figs/bode_Va_dL_effect_m_coef.png]] + +#+begin_src matlab :exports none +%% Plot the obtained coupling transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +hold on; +for i = 1:length(xis) + plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', xis(i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); +xlim([20, 2e3]); +#+end_src + +*** TODO Using Flexible model +#+begin_src matlab +d_aligns = [[-0.05, -0.3, 0]; + [ 0, 0.5, 0]; + [-0.1, -0.3, 0]; + [ 0, 0.3, 0]; + [-0.05, 0.05, 0]; + [0, 0, 0]]*1e-3; +#+end_src + +#+begin_src matlab +d_aligns = zeros(6,3); +% d_aligns(1,:) = [-0.05, -0.3, 0]*1e-3; +d_aligns(2,:) = [ 0, 0.3, 0]*1e-3; +#+end_src + +#+begin_src matlab +%% Initialize Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible', ... + 'actuator_d_align', d_aligns); +#+end_src + +#+begin_question +Why do we have smaller resonances when using flexible APA? +On the test bench we have the same resonance as the 2DoF model. +Could it be due to the compliance in other dof of the flexible model? +#+end_question + +#+begin_src matlab +%% Identify the DVF Plant (transfer function from u to dLm) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders + +Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Comparison of the plants (encoder output) when tuning the misalignment +freqs = 2*logspace(0, 3, 1000); + +figure; +tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 1, 1))); +plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]'); + +ax2 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 2, 2))); +plot(freqs, abs(squeeze(freqresp(Gdvf(2,2), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); + +ax3 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 3, 3)), 'DisplayName', 'Meas.'); +plot(freqs, abs(squeeze(freqresp(Gdvf(3,3), freqs, 'Hz'))), ... + 'DisplayName', 'Model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +legend('location', 'southwest', 'FontSize', 8); + +ax4 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 4, 4))); +plot(freqs, abs(squeeze(freqresp(Gdvf(4,4), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); + +ax5 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 5, 5))); +plot(freqs, abs(squeeze(freqresp(Gdvf(5,5), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); + +ax6 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 6, 6))); +plot(freqs, abs(squeeze(freqresp(Gdvf(6,6), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); + +linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy'); +% xlim([20, 2e3]); ylim([1e-8, 1e-3]); +xlim([50, 5e2]); ylim([1e-6, 1e-3]); +#+end_src + +#+begin_src matlab :exports none +%% Diagonal elements of the DVF plant +freqs = 6*logspace(1, 2, 2000); + +i_strut = 1; + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_dvf(:,i_strut, 2)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF') +plot(freqs, abs(squeeze(freqresp(Gdvf(2,2), freqs, 'Hz'))), '-', ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-3]); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +plot(f, 180/pi*angle(G_dvf(:,2, 2)), 'color', [0,0,0,0.2]); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(2,2), freqs, 'Hz'))), '-'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab :exports none +%% Diagonal elements of the DVF plant +freqs = 6*logspace(1, 2, 2000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_dvf(:,1, 1)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF') +for i = 2:6 + set(gca,'ColorOrderIndex',2) + plot(f, abs(G_dvf(:,i, i)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); +end +set(gca,'ColorOrderIndex',2); +plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model') +for i = 2:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-3]); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_dvf(:,i, i)), 'color', [0,0,0,0.2]); +end +for i = 1:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab +%% Identify the IFF Plant (transfer function from u to taum) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors + +Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff(:,1, 1)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$\tau_{m,i}/u_i$ - FRF') +for i = 2:6 + set(gca,'ColorOrderIndex',2) + plot(f, abs(G_iff(:,i, i)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); +end +set(gca,'ColorOrderIndex',2); +plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', '$\tau_{m,i}/u_i$ - Model') +for i = 2:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast'); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [0,0,0,0.2]); +end +for i = 1:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab +#+end_src + +*** Flexible model + encoders fixed to the plates +#+begin_src matlab +%% Identify the IFF Plant (transfer function from u to taum) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors +#+end_src + +#+begin_src matlab +d_aligns = [[-0.05, -0.3, 0]; + [ 0, 0.5, 0]; + [-0.1, -0.3, 0]; + [ 0, 0.3, 0]; + [-0.05, 0.05, 0]; + [0, 0, 0]]*1e-3; +#+end_src + +#+begin_src matlab +%% Initialize Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible', ... + 'actuator_d_align', d_aligns); +#+end_src + +#+begin_src matlab +Gdvf_struts = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab +%% Initialize Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'actuator_d_align', d_aligns); +#+end_src + +#+begin_src matlab +Gdvf_plates = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Plot the obtained direct transfer functions for all the bending stiffnesses +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(Gdvf_struts(1, 1), freqs, 'Hz'))), ... + 'DisplayName', 'Struts'); +plot(freqs, abs(squeeze(freqresp(Gdvf_plates(1, 1), freqs, 'Hz'))), ... + 'DisplayName', 'Plates'); +for i = 2:6 + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(Gdvf_struts(i, i), freqs, 'Hz'))), ... + 'HandleVisibility', 'off'); + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Gdvf_plates(i, i), freqs, 'Hz'))), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +hold off; +ylim([1e-8, 1e-3]); +legend('location', 'southwest'); + +ax2 = nexttile; +hold on; +for i = 1:6 + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_struts(i, i), freqs, 'Hz')))); + set(gca,'ColorOrderIndex',2); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_plates(i, i), freqs, 'Hz')))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/dvf_plant_comp_struts_plates.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:dvf_plant_comp_struts_plates +#+caption: Comparison of the dynamics from $V_a$ to $d_L$ when the encoders are fixed to the struts (blue) and to the plates (red). APA are modeled as a flexible element. +#+RESULTS: +[[file:figs/dvf_plant_comp_struts_plates.png]] + ** Integral Force Feedback <> +*** Introduction :ignore: + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab +load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +Rx = zeros(1, 7); + +open(mdl) +#+end_src + +*** Identification of the IFF Plant +#+begin_src matlab +%% Initialize Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', '2dof'); +#+end_src + +#+begin_src matlab +%% Identify the IFF Plant (transfer function from u to taum) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors + +Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + *** Root Locus and Decentralized Loop gain #+begin_src matlab %% IFF Controller -Kiff_g1 = (1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz) - (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain - (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances - eye(6); % Diagonal 6x6 controller +Kiff_g1 = -(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz) + (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain + (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances + eye(6); % Diagonal 6x6 controller #+end_src #+begin_src matlab :exports none @@ -964,6 +1787,14 @@ Then the "optimal" IFF controller is: Kiff = g*Kiff_g1; #+end_src +#+begin_src matlab :tangle no +save('matlab/mat/Kiff.mat', 'Kiff') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/Kiff.mat', 'Kiff') +#+end_src + #+begin_src matlab :exports none %% Bode plot of the "decentralized loop gain" freqs = 2*logspace(1, 3, 1000); @@ -973,14 +1804,11 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; -plot(f(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ... +plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz')).*G_iff(:, 1, 1)), 'color', [0,0,0,0.2], ... 'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - FRF') for i = 2:6 set(gca,'ColorOrderIndex',2) - plot(f(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - set(gca,'ColorOrderIndex',2) - plot(f(i_hf), abs(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ... + plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz')).*G_iff(:, i, i)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); end set(gca,'ColorOrderIndex',2); @@ -999,8 +1827,7 @@ legend('location', 'northeast'); ax2 = nexttile; hold on; for i = 1:6 - plot(f(i_lf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]); - plot(f(i_hf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2]); + plot(f, 180/pi*angle(squeeze(freqresp(Kiff(1,1), f, 'Hz')).*G_iff(:, i, i)), 'color', [0,0,0,0.2]); end for i = 1:6 set(gca,'ColorOrderIndex',2); @@ -1045,7 +1872,7 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... Gd_iff = {zeros(1, length(iff_gains))}; clear io; io_i = 1; -io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder) for i = 1:length(iff_gains) @@ -1222,17 +2049,11 @@ ax1 = nexttile([2,1]); hold on; % Un Damped measurement set(gca,'ColorOrderIndex',1) -plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), ... +plot(f, abs(G_dvf(:, 1, 1)), ... 'DisplayName', 'Un-Damped') -set(gca,'ColorOrderIndex',1) -plot(f(i_hf), abs(G_dvf_hf(i_hf,1, 1)), ... - 'HandleVisibility', 'off'); for i = 2:6 set(gca,'ColorOrderIndex',1) - plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ... - 'HandleVisibility', 'off'); - set(gca,'ColorOrderIndex',1) - plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ... + plot(f, abs(G_dvf(:,i , i)), ... 'HandleVisibility', 'off'); end @@ -1255,9 +2076,7 @@ ax2 = nexttile; hold on; for i =1:6 set(gca,'ColorOrderIndex',1) - plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i))); - set(gca,'ColorOrderIndex',1) - plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i))); + plot(f, 180/pi*angle(G_dvf(i,i, i))); set(gca,'ColorOrderIndex',2) plot(f, 180/pi*angle(G_iff_opt{i}(:,i))); end @@ -1481,6 +2300,56 @@ The top platform is then excited using an instrumented hammer as shown in Figure #+attr_latex: :width \linewidth [[file:figs/hammer_excitation_compliance_meas.jpg]] +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +Rx = zeros(1, 7); + +open(mdl) +#+end_src + *** Effectiveness of the IFF Strategy - Compliance In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance. @@ -1538,7 +2407,7 @@ The transfer function from a vertical external force to the absolute motion of t #+begin_src matlab :exports none %% Identify the IFF Plant (transfer function from u to taum) clear io; io_i = 1; -io(io_i) = linio([mdl, '/Fz_ext'], 1, 'openinput'); io_i = io_i + 1; % External - Vertical force +io(io_i) = linio([mdl, '/duz_ext'], 1, 'openinput'); io_i = io_i + 1; % External - Vertical force io(io_i) = linio([mdl, '/Z_top_plat'], 1, 'openoutput'); io_i = io_i + 1; % Absolute vertical motion of top platform %% Initialize Nano-Hexapod in Open Loop @@ -1603,9 +2472,9 @@ We can observe the mode shapes of the first 6 modes that are the suspension mode #+attr_latex: :width \linewidth [[file:figs/mode_shapes_annotated.gif]] -Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure [[fig:mode_shapes_annotated]]). +Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure [[fig:mode_shapes_flexible_annotated]]). -#+name: fig:mode_shapes_annotated +#+name: fig:mode_shapes_flexible_annotated #+caption: First flexible mode at 692Hz #+attr_latex: :width 0.3\linewidth [[file:figs/ModeShapeFlex1_crop.gif]] @@ -1626,7 +2495,2989 @@ The obtained modes are summarized in Table [[tab:description_modes]]. | 6 | 180 | Suspension Mode: Z-rotation | | 7 | 692 | (flexible) Membrane mode of the top platform | +** Accelerometers fixed on the top platform +*** Introduction :ignore: + +#+name: fig:acc_top_plat_top_view +#+caption: Accelerometers fixed on the top platform +#+attr_latex: :width \linewidth +[[file:figs/acc_top_plat_top_view.jpg]] + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +Rx = zeros(1, 7); + +open(mdl) +#+end_src + +*** Experimental Identification +#+begin_src matlab +%% Load Identification Data +meas_acc = {}; + +for i = 1:6 + meas_acc(i) = {load(sprintf('mat/meas_acc_top_plat_strut_%i.mat', i), 't', 'Va', 'de', 'Am')}; +end +#+end_src + +#+begin_src matlab +%% Setup useful variables +% Sampling Time [s] +Ts = (meas_acc{1}.t(end) - (meas_acc{1}.t(1)))/(length(meas_acc{1}.t)-1); + +% Sampling Frequency [Hz] +Fs = 1/Ts; + +% Hannning Windows +win = hanning(ceil(1*Fs)); + +% And we get the frequency vector +[~, f] = tfestimate(meas_acc{1}.Va, meas_acc{1}.de, win, [], [], 1/Ts); +#+end_src + +The sensibility of the accelerometers are $0.1 V/g \approx 0.01 V/(m/s^2)$. +#+begin_src matlab +%% Compute the 6x6 transfer function matrix +G_acc = zeros(length(f), 6, 6); + +for i = 1:6 + G_acc(:,:,i) = tfestimate(meas_acc{i}.Va, 1/0.01*meas_acc{i}.Am, win, [], [], 1/Ts); +end +#+end_src + +*** Location and orientation of accelerometers +#+begin_src matlab +Opm = [ 0.047, -0.112, 10e-3; + 0.047, -0.112, 10e-3; + -0.113, 0.011, 10e-3; + -0.113, 0.011, 10e-3; + 0.040, 0.113, 10e-3; + 0.040, 0.113, 10e-3]'; + +Osm = [-1, 0, 0; + 0, 0, 1; + 0, -1, 0; + 0, 0, 1; + -1, 0, 0; + 0, 0, 1]'; + +#+end_src + +*** COM +#+begin_src matlab +Hbm = -15e-3; + +M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm); +J = getJacobianNanoHexapod(Hbm); +#+end_src + +#+begin_src matlab +G_acc_CoM = zeros(size(G_acc)); + +for i = 1:length(f) + G_acc_CoM(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J'); +end +#+end_src + +#+begin_src matlab :exports none +labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'}; + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:2 + for j = i+1:3 + plot(f, abs(G_acc_CoM(:, i, j)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i =1:3 + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_acc_CoM(:,i , i)./(-(2*pi*f).^2)), ... + 'DisplayName', labels{i}); +end +plot(f, abs(G_acc_CoM(:, 1, 2)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_i/F_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $A_m/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-5]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:3 + set(gca,'ColorOrderIndex',i) + plot(f, 180/pi*angle(G_acc_CoM(:,i , i)./(-(2*pi*f).^2))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :exports none +labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'}; + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_acc_CoM(:,i , i)./(-(2*pi*f).^2)), ... + 'DisplayName', labels{i}); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $A_m/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-3]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, 180/pi*angle(G_acc_CoM(:,i , i)./(-(2*pi*f).^2))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); + +linkaxes([ax1,ax2],'x'); +xlim([50, 5e2]); +#+end_src + +*** COK +#+begin_src matlab +Hbm = -42.3e-3; + +M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm); +J = getJacobianNanoHexapod(Hbm); +#+end_src + +#+begin_src matlab +G_acc_CoK = zeros(size(G_acc)); + +for i = 1:length(f) + G_acc_CoK(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J'); +end +#+end_src + +#+begin_src matlab :exports none +labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'}; + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:2 + for j = i+1:3 + plot(f, abs(G_acc_CoK(:, i, j)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i =1:3 + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_acc_CoK(:,i , i)./(-(2*pi*f).^2)), ... + 'DisplayName', labels{i}); +end +plot(f, abs(G_acc_CoK(:, 1, 2)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_i/F_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $A_m/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-5]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:3 + set(gca,'ColorOrderIndex',i) + plot(f, 180/pi*angle(G_acc_CoK(:,i , i)./(-(2*pi*f).^2))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :exports none +labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ... + '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; + +figure; +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_acc_CoK(:,i,i)./(-(2*pi*f).^2)), ... + 'DisplayName', labels{i}); +end +plot(f, abs(G_acc_CoK(:,1,2)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', 'Off-Diagonal'); +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_acc_CoK(:,i,j)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude $X_m/V_a$ [m/V]'); +xlim([50, 5e2]); ylim([1e-7, 1e-1]); +legend('location', 'southwest'); +#+end_src + +*** Comp with the Simscape Model +#+begin_src matlab +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', 'flexible', ... + 'MO_B', -42.3e-3); +#+end_src + +#+begin_src matlab +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs + +G = linearize(mdl, io, 0.0, options); +G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; +G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; +#+end_src + +Then use the Jacobian matrices to obtain the "cartesian" centralized plant. +#+begin_src matlab +Gc = inv(n_hexapod.geometry.J)*... + G*... + inv(n_hexapod.geometry.J'); +#+end_src + +#+begin_src matlab :exports none +freqs = 2*logspace(1, 3, 1000); + +labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ... + '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; + +figure; +hold on; +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gc(i,i), freqs, 'Hz'))), '-', ... + 'DisplayName', labels{i}); +end +plot(freqs, abs(squeeze(freqresp(Gc(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', 'Off-Diagonal'); +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N,rad/N/m]'); +xlim([50, 5e2]); ylim([1e-7, 1e-1]); +legend('location', 'southwest'); +#+end_src + * Encoders fixed to the plates <> ** Introduction :ignore: +In this section, the encoders are fixed to the plates rather than to the struts as shown in Figure [[fig:enc_fixed_to_struts]]. + +#+name: fig:enc_fixed_to_struts +#+caption: Nano-Hexapod with encoders fixed to the struts +#+attr_latex: :width \linewidth +[[file:figs/IMG_20210625_083801.jpg]] + +It is structured as follow: +- Section [[sec:enc_plates_plant_id]]: The dynamics of the nano-hexapod is identified +- Section [[sec:enc_plates_comp_simscape]]: The identified dynamics is compared with the Simscape model +- Section [[sec:enc_plates_iff]]: The Integral Force Feedback (IFF) control strategy is applied and the dynamics of the damped nano-hexapod is identified and compare with the Simscape model +- Section [[sec:hac_iff_struts]]: The High Authority Control (HAC) in the frame of the struts is developed +- Section [[sec:hac_iff_struts_ref_track]]: Some reference tracking tests are performed in order to experimentally validate the HAC-LAC control strategy. + +** Identification of the dynamics +<> +*** Introduction :ignore: + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +*** Load Measurement Data +#+begin_src matlab +%% Load Identification Data +meas_data_lf = {}; + +for i = 1:6 + meas_data_lf(i) = {load(sprintf('mat/frf_exc_strut_%i_enc_plates_noise.mat', i), 't', 'Va', 'Vs', 'de')}; +end +#+end_src + +*** Spectral Analysis - Setup +#+begin_src matlab +%% Setup useful variables +% Sampling Time [s] +Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1); + +% Sampling Frequency [Hz] +Fs = 1/Ts; + +% Hannning Windows +win = hanning(ceil(1*Fs)); + +% And we get the frequency vector +[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts); +#+end_src + +*** DVF Plant +First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_plates_dvf_coh]]). + +#+begin_src matlab +%% Coherence +coh_dvf = zeros(length(f), 6, 6); + +for i = 1:6 + coh_dvf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Coherence for the transfer function from u to dLm +figure; +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, coh_dvf(:, i, j), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, coh_dvf(:, i, i), ... + 'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i)); +end +plot(f, coh_dvf(:, 1, 2), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$G_{dvf}(i,j)$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Coherence [-]'); +xlim([20, 2e3]); ylim([0, 1]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_dvf_coh.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:enc_plates_dvf_coh +#+caption: Obtained coherence for the DVF plant +#+RESULTS: +[[file:figs/enc_plates_dvf_coh.png]] + +Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_plates_dvf_frf]]). +#+begin_src matlab +%% DVF Plant (transfer function from u to dLm) +G_dvf = zeros(length(f), 6, 6); + +for i = 1:6 + G_dvf(:,:,i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_dvf(:, i, j)), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_dvf(:,i, i)), ... + 'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i)); + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_dvf(:,i, i)), ... + 'HandleVisibility', 'off'); +end +plot(f, abs(G_dvf(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$G_{dvf}(i,j)$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-3]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, 180/pi*angle(G_dvf(:,i, i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_dvf_frf.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_dvf_frf +#+caption: Measured FRF for the DVF plant +#+RESULTS: +[[file:figs/enc_plates_dvf_frf.png]] + +*** IFF Plant +First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_plates_iff_coh]]). + +#+begin_src matlab +%% Coherence for the IFF plant +coh_iff = zeros(length(f), 6, 6); + +for i = 1:6 + coh_iff(:,:,i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Coherence of the IFF Plant (transfer function from u to taum) +figure; +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, coh_iff(:, i, j), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, coh_iff(:,i, i), ... + 'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i)); +end +plot(f, coh_iff(:, 1, 2), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$G_{iff}(i,j)$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Coherence [-]'); +xlim([20, 2e3]); ylim([0, 1]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_iff_coh.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:enc_plates_iff_coh +#+caption: Obtained coherence for the IFF plant +#+RESULTS: +[[file:figs/enc_plates_iff_coh.png]] + +Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_plates_iff_frf]]). +#+begin_src matlab +%% IFF Plant +G_iff = zeros(length(f), 6, 6); + +for i = 1:6 + G_iff(:,:,i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the IFF Plant (transfer function from u to taum) +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, abs(G_iff(:,i , i)), ... + 'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i)); +end +plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$G_{iff}(i,j)$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); +ylim([1e-3, 1e2]); + +ax2 = nexttile; +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i) + plot(f, 180/pi*angle(G_iff(:,i, i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_iff_frf.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_iff_frf +#+caption: Measured FRF for the IFF plant +#+RESULTS: +[[file:figs/enc_plates_iff_frf.png]] + +*** Save Identified Plants +#+begin_src matlab :tangle no +save('matlab/mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + +** Comparison with the Simscape Model +<> +*** Introduction :ignore: +In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model. + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +Rx = zeros(1, 7); + +open(mdl) +#+end_src + +*** Load measured FRF +#+begin_src matlab +%% Load data +load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + +*** Dynamics from Actuator to Force Sensors +#+begin_src matlab +%% Initialize Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible'); +#+end_src + +#+begin_src matlab +%% Identify the IFF Plant (transfer function from u to taum) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors + +Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Comparison of the plants (encoder output) when tuning the misalignment +freqs = 2*logspace(1, 3, 1000); + +i_input = 1; + +figure; +tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(f, abs(G_iff(:, 1, i_input))); +plot(freqs, abs(squeeze(freqresp(Giff(1, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]'); +title(sprintf('$d\\tau_{m1}/u_{%i}$', i_input)); + +ax2 = nexttile(); +hold on; +plot(f, abs(G_iff(:, 2, i_input))); +plot(freqs, abs(squeeze(freqresp(Giff(2, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +title(sprintf('$d\\tau_{m2}/u_{%i}$', i_input)); + +ax3 = nexttile(); +hold on; +plot(f, abs(G_iff(:, 3, i_input)), ... + 'DisplayName', 'Meas.'); +plot(freqs, abs(squeeze(freqresp(Giff(3, i_input), freqs, 'Hz'))), ... + 'DisplayName', 'Model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8); +title(sprintf('$d\\tau_{m3}/u_{%i}$', i_input)); + +ax4 = nexttile(); +hold on; +plot(f, abs(G_iff(:, 4, i_input))); +plot(freqs, abs(squeeze(freqresp(Giff(4, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +title(sprintf('$d\\tau_{m4}/u_{%i}$', i_input)); + +ax5 = nexttile(); +hold on; +plot(f, abs(G_iff(:, 5, i_input))); +plot(freqs, abs(squeeze(freqresp(Giff(5, i_input), freqs, 'Hz')))); +hold off; +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +title(sprintf('$d\\tau_{m5}/u_{%i}$', i_input)); + +ax6 = nexttile(); +hold on; +plot(f, abs(G_iff(:, 6, i_input))); +plot(freqs, abs(squeeze(freqresp(Giff(6, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); +title(sprintf('$d\\tau_{m6}/u_{%i}$', i_input)); + +linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy'); +xlim([20, 2e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_iff_comp_simscape_all.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_iff_comp_simscape_all +#+caption: IFF Plant for the first actuator input and all the force senosrs +#+RESULTS: +[[file:figs/enc_plates_iff_comp_simscape_all.png]] + +#+begin_src matlab :exports none +%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff(:,1, 1)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$\tau_{m,i}/u_i$ - FRF') +for i = 2:6 + set(gca,'ColorOrderIndex',2) + plot(f, abs(G_iff(:,i, i)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); +end +set(gca,'ColorOrderIndex',2); +plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', '$\tau_{m,i}/u_i$ - Model') +for i = 2:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast'); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [0,0,0,0.2]); +end +for i = 1:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_iff_comp_simscape.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_iff_comp_simscape +#+caption: Diagonal elements of the IFF Plant +#+RESULTS: +[[file:figs/enc_plates_iff_comp_simscape.png]] + +#+begin_src matlab :exports none +%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data (off-diagonal elements) +freqs = 2*logspace(1, 3, 1000); + +figure; +hold on; +% Off diagonal terms +plot(f, abs(G_iff(:, 1, 2)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$\tau_{m,i}/u_j$ - FRF') +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_iff(:, i, j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',2); +plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), ... + 'DisplayName', '$\tau_{m,i}/u_j$ - Model') +for i = 1:5 + for j = i+1:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]'); +xlim([freqs(1), freqs(end)]); ylim([1e-3, 1e2]); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_iff_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:enc_plates_iff_comp_offdiag_simscape +#+caption: Off diagonal elements of the IFF Plant +#+RESULTS: +[[file:figs/enc_plates_iff_comp_offdiag_simscape.png]] + +*** Dynamics from Actuator to Encoder +#+begin_src matlab +%% Initialization of the Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible'); +#+end_src + +#+begin_src matlab +%% Identify the DVF Plant (transfer function from u to dLm) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Encoders + +Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Comparison of the plants (encoder output) when tuning the misalignment +freqs = 2*logspace(1, 3, 1000); + +i_input = 3; + +figure; +tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 1, i_input))); +plot(freqs, abs(squeeze(freqresp(Gdvf(1, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]'); +title(sprintf('$d\\mathcal{L}_{m1}/u_{%i}$', i_input)); + +ax2 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 2, i_input))); +plot(freqs, abs(squeeze(freqresp(Gdvf(2, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +title(sprintf('$d\\mathcal{L}_{m2}/u_{%i}$', i_input)); + +ax3 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 3, i_input)), ... + 'DisplayName', 'Meas.'); +plot(freqs, abs(squeeze(freqresp(Gdvf(3, i_input), freqs, 'Hz'))), ... + 'DisplayName', 'Model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8); +title(sprintf('$d\\mathcal{L}_{m3}/u_{%i}$', i_input)); + +ax4 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 4, i_input))); +plot(freqs, abs(squeeze(freqresp(Gdvf(4, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +title(sprintf('$d\\mathcal{L}_{m4}/u_{%i}$', i_input)); + +ax5 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 5, i_input))); +plot(freqs, abs(squeeze(freqresp(Gdvf(5, i_input), freqs, 'Hz')))); +hold off; +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +title(sprintf('$d\\mathcal{L}_{m5}/u_{%i}$', i_input)); + +ax6 = nexttile(); +hold on; +plot(f, abs(G_dvf(:, 6, i_input))); +plot(freqs, abs(squeeze(freqresp(Gdvf(6, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); +title(sprintf('$d\\mathcal{L}_{m6}/u_{%i}$', i_input)); + +linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy'); +xlim([40, 4e2]); ylim([1e-8, 1e-2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_dvf_comp_simscape_all.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_dvf_comp_simscape_all +#+caption: DVF Plant for the first actuator input and all the encoders +#+RESULTS: +[[file:figs/enc_plates_dvf_comp_simscape_all.png]] + +#+begin_src matlab :exports none +%% Diagonal elements of the DVF plant +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_dvf(:,1, 1)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF') +for i = 2:6 + set(gca,'ColorOrderIndex',2) + plot(f, abs(G_dvf(:,i, i)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); +end +set(gca,'ColorOrderIndex',2); +plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model') +for i = 2:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-3]); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_dvf(:,i, i)), 'color', [0,0,0,0.2]); +end +for i = 1:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_dvf_comp_simscape.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_dvf_comp_simscape +#+caption: Diagonal elements of the DVF Plant +#+RESULTS: +[[file:figs/enc_plates_dvf_comp_simscape.png]] + +#+begin_src matlab :exports none +%% Off-diagonal elements of the DVF plant +freqs = 2*logspace(1, 3, 1000); + +figure; +hold on; +% Off diagonal terms +plot(f, abs(G_dvf(:, 1, 2)), 'color', [0,0,0,0.2], ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF') +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_dvf(:, i, j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',2); +plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model') +for i = 1:5 + for j = i+1:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +xlim([freqs(1), freqs(end)]); ylim([1e-8, 1e-3]); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:enc_plates_dvf_comp_offdiag_simscape +#+caption: Off diagonal elements of the DVF Plant +#+RESULTS: +[[file:figs/enc_plates_dvf_comp_offdiag_simscape.png]] + +** Integral Force Feedback +<> +*** Introduction :ignore: + + +#+begin_src latex :file control_architecture_iff.pdf + \begin{tikzpicture} + % Blocs + \node[block={3.0cm}{3.0cm}] (P) {Plant}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); + \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); + + \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; + \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {}; + + % Connections and labels + \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; + \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$}; + + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.north); + \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$}; + \draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{u}^\prime$}; + \end{tikzpicture} +#+end_src + +#+name: fig:control_architecture_iff +#+caption: Integral Force Feedback Strategy +#+RESULTS: +[[file:figs/control_architecture_iff.png]] + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab +load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +open(mdl) + +Rx = zeros(1, 7); + +colors = colororder; +#+end_src + +*** Identification of the IFF Plant +#+begin_src matlab +%% Initialize Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof'); +#+end_src + +#+begin_src matlab +%% Identify the IFF Plant (transfer function from u to taum) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors + +Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +*** Effect of IFF on the plant - Simscape Model +#+begin_src matlab +load('Kiff.mat', 'Kiff') +#+end_src + +#+begin_src matlab +%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder) +#+end_src + +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible'); +#+end_src + +#+begin_src matlab +Gd_ol = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'controller_type', 'iff'); +#+end_src + +#+begin_src matlab +Gd_iff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :results value replace :exports results :tangle no +isstable(Gd_iff) +#+end_src + +#+RESULTS: +: 1 + +#+begin_src matlab :exports none +%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(Gd_ol(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', 'OL - Diag'); +plot(freqs, abs(squeeze(freqresp(Gd_iff(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', 'IFF - Diag'); +for i = 2:6 + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(Gd_ol(1,1), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end +for i = 2:6 + set(gca,'ColorOrderIndex',2); + plot(freqs, abs(squeeze(freqresp(Gd_iff(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end + +plot(freqs, abs(squeeze(freqresp(Gd_ol(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'DisplayName', 'OL - Off-diag') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gd_ol(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(Gd_iff(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', 'IFF - Off-diag') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gd_iff(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i = 1:6 + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_ol(1,1), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); + set(gca,'ColorOrderIndex',2); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_iff_gains_effect_dvf_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_iff_gains_effect_dvf_plant +#+caption: Effect of the IFF control strategy on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$ +#+RESULTS: +[[file:figs/enc_plates_iff_gains_effect_dvf_plant.png]] + +*** Experimental Results - Damped Plant with Optimal gain +**** Introduction :ignore: +Let's now look at the $6 \times 6$ damped plant with the optimal gain $g = 400$. + +**** Load Data +#+begin_src matlab +%% Load Identification Data +meas_iff_plates = {}; + +for i = 1:6 + meas_iff_plates(i) = {load(sprintf('mat/frf_exc_iff_strut_%i_enc_plates_noise.mat', i), 't', 'Va', 'Vs', 'de', 'u')}; +end +#+end_src + +**** Spectral Analysis - Setup +#+begin_src matlab +%% Setup useful variables +% Sampling Time [s] +Ts = (meas_iff_plates{1}.t(end) - (meas_iff_plates{1}.t(1)))/(length(meas_iff_plates{1}.t)-1); + +% Sampling Frequency [Hz] +Fs = 1/Ts; + +% Hannning Windows +win = hanning(ceil(1*Fs)); + +% And we get the frequency vector +[~, f] = tfestimate(meas_iff_plates{1}.Va, meas_iff_plates{1}.de, win, [], [], 1/Ts); +#+end_src + +**** Simscape Model +#+begin_src matlab +load('Kiff.mat', 'Kiff') +#+end_src + +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'controller_type', 'iff'); +#+end_src + +#+begin_src matlab +%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder) +#+end_src + +#+begin_src matlab +Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +**** DVF Plant +#+begin_src matlab +%% IFF Plant +G_enc_iff_opt = zeros(length(f), 6, 6); + +for i = 1:6 + G_enc_iff_opt(:,:,i) = tfestimate(meas_iff_plates{i}.Va, meas_iff_plates{i}.de, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Comparison of the plants (encoder output) when tuning the misalignment +freqs = 2*logspace(1, 3, 1000); + +i_input = 1; + +figure; +tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(f, abs(G_enc_iff_opt(:, 1, i_input))); +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(1, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]'); +title(sprintf('$d\\tau_{m1}/u_{%i}$', i_input)); + +ax2 = nexttile(); +hold on; +plot(f, abs(G_enc_iff_opt(:, 2, i_input))); +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(2, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +title(sprintf('$d\\tau_{m2}/u_{%i}$', i_input)); + +ax3 = nexttile(); +hold on; +plot(f, abs(G_enc_iff_opt(:, 3, i_input)), ... + 'DisplayName', 'Meas.'); +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(3, i_input), freqs, 'Hz'))), ... + 'DisplayName', 'Model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8); +title(sprintf('$d\\tau_{m3}/u_{%i}$', i_input)); + +ax4 = nexttile(); +hold on; +plot(f, abs(G_enc_iff_opt(:, 4, i_input))); +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(4, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +title(sprintf('$d\\tau_{m4}/u_{%i}$', i_input)); + +ax5 = nexttile(); +hold on; +plot(f, abs(G_enc_iff_opt(:, 5, i_input))); +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(5, i_input), freqs, 'Hz')))); +hold off; +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +title(sprintf('$d\\tau_{m5}/u_{%i}$', i_input)); + +ax6 = nexttile(); +hold on; +plot(f, abs(G_enc_iff_opt(:, 6, i_input))); +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(6, i_input), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); +title(sprintf('$d\\tau_{m6}/u_{%i}$', i_input)); + +linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy'); +xlim([20, 2e3]); ylim([1e-8, 1e-4]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plates_opt_iff_comp_simscape_all.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plates_opt_iff_comp_simscape_all +#+caption: FRF from one actuator to all the encoders when the plant is damped using IFF +#+RESULTS: +[[file:figs/enc_plates_opt_iff_comp_simscape_all.png]] + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Diagonal Elements FRF +plot(f, abs(G_enc_iff_opt(:,1,1)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF') +for i = 2:6 + plot(f, abs(G_enc_iff_opt(:,i,i)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); +end + +% Diagonal Elements Model +set(gca,'ColorOrderIndex',2) +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model') +for i = 2:6 + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); +end + +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-4]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(G_enc_iff_opt(:,i,i)), 'color', [colors(1,:), 0.2]); + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz')))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/damped_iff_plates_plant_comp_diagonal.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:damped_iff_plates_plant_comp_diagonal +#+caption: Comparison of the diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$ +#+RESULTS: +[[file:figs/damped_iff_plates_plant_comp_diagonal.png]] + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Off diagonal FRF +plot(f, abs(G_enc_iff_opt(:,1,2)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF') +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_enc_iff_opt(:,i,j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end + +% Off diagonal Model +set(gca,'ColorOrderIndex',2) +plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(1,2), freqs, 'Hz'))), '-', ... + 'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model') +for i = 1:5 + for j = i+1:6 + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(i,j), freqs, 'Hz'))), ... + 'HandleVisibility', 'off'); + end +end + +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-4]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +% Off diagonal FRF +for i = 1:5 + for j = i+1:6 + plot(f, 180/pi*angle(G_enc_iff_opt(:,i,j)), 'color', [colors(1,:), 0.2]); + end +end + +% Off diagonal Model +for i = 1:5 + for j = i+1:6 + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff_opt(i,j), freqs, 'Hz')))); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/damped_iff_plates_plant_comp_off_diagonal.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:damped_iff_plates_plant_comp_off_diagonal +#+caption: Comparison of the off-diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$ +#+RESULTS: +[[file:figs/damped_iff_plates_plant_comp_off_diagonal.png]] + +*** Effect of IFF on the plant - FRF +#+begin_src matlab :tangle no +load('identified_plants_enc_plates.mat', 'f', 'G_dvf'); +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_dvf(:,1,1)), '-', ... + 'DisplayName', 'OL - Diag'); +plot(f, abs(G_enc_iff_opt(:,1,1)), '-', ... + 'DisplayName', 'IFF - Diag'); +for i = 2:6 + set(gca,'ColorOrderIndex',1); + plot(f, abs(G_dvf(:,1,1)), '-', ... + 'HandleVisibility', 'off'); +end +for i = 2:6 + set(gca,'ColorOrderIndex',2); + plot(f, abs(G_enc_iff_opt(:,i,i)), '-', ... + 'HandleVisibility', 'off'); +end + +plot(f, abs(G_dvf(:,1,2)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', 'OL - Off-diag') +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_dvf(:,i,j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_enc_iff_opt(:,1,2)), 'color', [colors(2,:), 0.2], ... + 'DisplayName', 'IFF - Off-diag') +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_enc_iff_opt(:,i,j)), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i = 1:6 + set(gca,'ColorOrderIndex',1); + plot(f, 180/pi*angle(G_dvf(:,1,1)), '-') + set(gca,'ColorOrderIndex',2); + plot(f, 180/pi*angle(G_enc_iff_opt(:,i,i)), '-') +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/enc_plant_plates_effect_iff.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:enc_plant_plates_effect_iff +#+caption: Effect of the IFF control strategy on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$ +#+RESULTS: +[[file:figs/enc_plant_plates_effect_iff.png]] + +*** Save Damped Plant +#+begin_src matlab :tangle no +save('matlab/mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt') +#+end_src + +** HAC Control - Frame of the struts +<> +*** Introduction :ignore: + +In a first approximation, the Jacobian matrix can be used instead of using the inverse kinematic equations. + +#+begin_src latex :file control_architecture_hac_iff_L.pdf +\begin{tikzpicture} + % Blocs + \node[block={3.0cm}{3.0cm}] (P) {Plant}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); + \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); + + \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; + \node[addb, left= of inputF] (addF) {}; + \node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$}; + \node[addb, left= of K] (subr) {}; + \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics}; + + % Connections and labels + \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.north); + \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$}; + + \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south); + \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$}; + \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$}; + + \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$}; + + \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; + \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0); +\end{tikzpicture} +#+end_src + +#+name: fig:control_architecture_hac_iff_L +#+caption: HAC-LAC: IFF + Control in the frame of the legs +#+RESULTS: +[[file:figs/control_architecture_hac_iff_L.png]] + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab +load('damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt') +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +open(mdl) + +Rx = zeros(1, 7); + +colors = colororder; +#+end_src + +*** Simscape Model +Let's start with the Simscape model and the damped plant. + +Apply HAC control and verify the system is stable. + +Then, try the control strategy on the real plant. +#+begin_src matlab +load('Kiff.mat', 'Kiff') +#+end_src + +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'controller_type', 'iff'); +#+end_src + +#+begin_src matlab +%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder) +#+end_src + +#+begin_src matlab +Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :results value replace :exports both :tangle no +isstable(Gd_iff_opt) +#+end_src + +#+RESULTS: +: 1 + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Diagonal Elements Model +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))), 'k-'); +end + +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-4]); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))), 'k-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/hac_iff_struts_enc_plates_plant_bode.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:hac_iff_struts_enc_plates_plant_bode +#+caption: Transfer function from $u$ to $d\mathcal{L}_m$ with IFF (diagonal elements) +#+RESULTS: +[[file:figs/hac_iff_struts_enc_plates_plant_bode.png]] + +*** HAC Controller +Let's try to have 100Hz bandwidth: +#+begin_src matlab +%% Lead +a = 2; % Amount of phase lead / width of the phase lead / high frequency gain +wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s] + +H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); + +%% Low Pass filter +H_lpf = 1/(1 + s/2/pi/200); + +%% Notch +gm = 0.02; +xi = 0.3; +wn = 2*pi*700; + +H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); +#+end_src + +#+begin_src matlab +Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain + H_lead * ... % Lead + H_notch * ... % Notch + (2*pi*100/s) * ... % Integrator + eye(6); % 6x6 Diagonal +#+end_src + +#+begin_src matlab :tangle no +save('matlab/mat/Khac_iff_struts.mat', 'Khac_iff_struts') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/Khac_iff_struts.mat', 'Khac_iff_struts') +#+end_src + +#+begin_src matlab +Lhac_iff_struts = Khac_iff_struts*Gd_iff_opt; +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = 2*logspace(0, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Diagonal Elements Model +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Lhac_iff_struts(i,i), freqs, 'Hz'))), 'k-'); +end +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Lhac_iff_struts(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2]); + end +end + +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e2]); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Lhac_iff_struts(i,i), freqs, 'Hz'))), 'k-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([2, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/loop_gain_hac_iff_struts.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:loop_gain_hac_iff_struts +#+caption: Diagonal and off-diagonal elements of the Loop gain for "HAC-IFF-Struts" +#+RESULTS: +[[file:figs/loop_gain_hac_iff_struts.png]] + + +*** Experimental Loop Gain +- [ ] Find a more clever way to do the multiplication + +#+begin_src matlab +L_frf = zeros(size(G_enc_iff_opt)); + +for i = 1:size(G_enc_iff_opt, 1) + L_frf(i, :, :) = squeeze(G_enc_iff_opt(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz'); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Diagonal Elements FRF +plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ... + 'DisplayName', 'Diagonal'); +for i = 2:6 + plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ... + 'DisplayName', 'Off-Diag'); +for i = 1:5 + for j = i+1:6 + plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e2]); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([1, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/hac_iff_plates_exp_loop_gain_diag.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:hac_iff_plates_exp_loop_gain_diag +#+caption: Diagonal and Off-diagonal elements of the Loop gain (experimental data) +#+RESULTS: +[[file:figs/hac_iff_plates_exp_loop_gain_diag.png]] + +*** Verification of the Stability using the Simscape model +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'controller_type', 'hac-iff-struts'); +#+end_src + +#+begin_src matlab +Gd_iff_hac_opt = linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :results value replace :exports both +isstable(Gd_iff_hac_opt) +#+end_src + +#+RESULTS: +: 1 + +** Reference Tracking +<> +*** Introduction :ignore: + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +open(mdl) + +colors = colororder; +#+end_src + +*** Load +#+begin_src matlab +load('Khac_iff_struts.mat', 'Khac_iff_struts') +#+end_src + +*** Y-Z Scans +**** Generate the Scan +#+begin_src matlab +Rx_yz = generateYZScanTrajectory(... + 'y_tot', 4e-6, ... + 'z_tot', 8e-6, ... + 'n', 5, ... + 'Ts', 1e-3, ... + 'ti', 2, ... + 'tw', 0.5, ... + 'ty', 2, ... + 'tz', 1); +#+end_src + +#+begin_src matlab +figure; +hold on; +plot(Rx_yz(:,1), Rx_yz(:,3), ... + 'DisplayName', 'Y motion') +plot(Rx_yz(:,1), Rx_yz(:,4), ... + 'DisplayName', 'Z motion') +hold off; +xlabel('Time [s]'); +ylabel('Displacement [m]'); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab +figure; +plot(Rx_yz(:,3), Rx_yz(:,4)); +xlabel('y [m]'); ylabel('z [m]'); +#+end_src + +**** Reference Signal for the Strut lengths +Let's use the Jacobian to estimate the wanted strut length as a function of time. +#+begin_src matlab +dL_ref = [n_hexapod.geometry.J*Rx_yz(:, 2:7)']'; +#+end_src + +#+begin_src matlab +figure; +hold on; +for i=1:6 + plot(Rx_yz(:,1), dL_ref(:, i)) +end +xlabel('Time [s]'); ylabel('Displacement [m]'); +#+end_src + +**** Time domain simulation with 2DoF model +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ... + 'flex_top_type', '3dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof', ... + 'controller_type', 'hac-iff-struts'); +#+end_src + +#+begin_src matlab +set_param(mdl,'StopTime', num2str(Rx_yz(end,1))) +set_param(mdl,'SimulationCommand','start') +#+end_src + +#+begin_src matlab +out.X.Data = out.X.Data - out.X.Data(1,:); +#+end_src + +#+begin_src matlab :exports none +figure; +hold on; +set(gca,'ColorOrderIndex',2) +plot(1e6*out.X.Data(:,2), 1e6*out.X.Data(:,3), '-', ... + 'DisplayName', 'Meas. Motion') +plot(1e6*Rx_yz(:,3), 1e6*Rx_yz(:,4), 'k--', ... + 'DisplayName', 'Reference Path') +hold off; +xlabel('X displacement [$\mu m$]'); ylabel('Y displacement [$\mu m$]'); +legend('location', 'southwest'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_hac_iff_struts_yz_plane.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:ref_track_hac_iff_struts_yz_plane +#+caption: Simulated Y-Z motion +#+RESULTS: +[[file:figs/ref_track_hac_iff_struts_yz_plane.png]] + +#+begin_src matlab :exports none +figure; +hold on; +set(gca,'ColorOrderIndex',2) +plot(out.X.Time, out.X.Data(:,2), '-', ... + 'DisplayName', 'Meas. - Y') +plot(Rx_yz(:,1), Rx_yz(:,3), 'k--', ... + 'DisplayName', 'Ref - Y') +plot(out.X.Time, out.X.Data(:,3), '-', ... + 'DisplayName', 'Meas - Z') +plot(Rx_yz(:,1), Rx_yz(:,4), 'k-.', ... + 'DisplayName', 'Ref - Z') +hold off; +xlabel('Time [s]'); ylabel('Y Displacement [$\mu m$]'); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_hac_iff_struts_yz_time.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:ref_track_hac_iff_struts_yz_time +#+caption: Y and Z motion as a function of time as well as the reference signals +#+RESULTS: +[[file:figs/ref_track_hac_iff_struts_yz_time.png]] + +#+begin_src matlab :exports none +figure; +tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(out.X.Time, 1e9*(out.X.Data(:,1) - Rx_yz(:,2)), 'DisplayName', '$\epsilon_x$') +plot(out.X.Time, 1e9*(out.X.Data(:,2) - Rx_yz(:,3)), 'DisplayName', '$\epsilon_y$') +plot(out.X.Time, 1e9*(out.X.Data(:,3) - Rx_yz(:,4)), 'DisplayName', '$\epsilon_z$') +hold off; +xlabel('Time [s]'); ylabel('Position Errors [nm]'); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +plot(out.X.Time, 1e6*(out.X.Data(:,4) - Rx_yz(:,5)), 'DisplayName', '$\epsilon_{R_x}$') +plot(out.X.Time, 1e6*(out.X.Data(:,5) - Rx_yz(:,6)), 'DisplayName', '$\epsilon_{R_y}$') +plot(out.X.Time, 1e6*(out.X.Data(:,6) - Rx_yz(:,7)), 'DisplayName', '$\epsilon_{R_z}$') +hold off; +xlabel('Time [s]'); ylabel('Orientation Errors [$\mu rad$]'); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_hac_iff_struts_pos_error.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:ref_track_hac_iff_struts_pos_error +#+caption: Positioning errors as a function of time +#+RESULTS: +[[file:figs/ref_track_hac_iff_struts_pos_error.png]] + +*** "NASS" reference path +**** Generate Path +#+begin_src matlab +ref_path = [ ... + 0, 0, 0; + 0, 0, 1; % N + 0, 4, 1; + 3, 0, 1; + 3, 4, 1; + 3, 4, 0; + 4, 0, 0; + 4, 0, 1; % A + 4, 3, 1; + 5, 4, 1; + 6, 4, 1; + 7, 3, 1; + 7, 2, 1; + 4, 2, 1; + 4, 3, 1; + 5, 4, 1; + 6, 4, 1; + 7, 3, 1; + 7, 0, 1; + 7, 0, 0; + 8, 0, 0; + 8, 0, 1; % S + 11, 0, 1; + 11, 2, 1; + 8, 2, 1; + 8, 4, 1; + 11, 4, 1; + 11, 4, 0; + 12, 0, 0; + 12, 0, 1; % S + 15, 0, 1; + 15, 2, 1; + 12, 2, 1; + 12, 4, 1; + 15, 4, 1; + 15, 4, 0; + ]; + +% Center the trajectory arround zero +ref_path = ref_path - (max(ref_path) - min(ref_path))/2; + +% Define the X-Y-Z cuboid dimensions containing the trajectory +X_max = 10e-6; +Y_max = 4e-6; +Z_max = 2e-6; + +ref_path = ([X_max, Y_max, Z_max]./max(ref_path)).*ref_path; % [m] +#+end_src + +#+begin_src matlab +Rx_nass = generateXYZTrajectory('points', ref_path); +#+end_src + +#+begin_src matlab +figure; +plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k.') +xlabel('X [$\mu m$]'); +ylabel('Y [$\mu m$]'); +axis equal; +xlim(1e6*[min(Rx_nass(:,2)), max(Rx_nass(:,2))]); +ylim(1e6*[min(Rx_nass(:,3)), max(Rx_nass(:,3))]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_test_nass.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:ref_track_test_nass +#+caption: Reference path corresponding to the "NASS" acronym +#+RESULTS: +[[file:figs/ref_track_test_nass.png]] + +#+begin_src matlab +figure; +plot3(Rx_nass(:,2), Rx_nass(:,3), Rx_nass(:,4), 'k-'); +xlabel('x'); +ylabel('y'); +zlabel('z'); +#+end_src + +#+begin_src matlab +figure; +hold on; +plot(Rx_nass(:,1), Rx_nass(:,2)); +plot(Rx_nass(:,1), Rx_nass(:,3)); +plot(Rx_nass(:,1), Rx_nass(:,4)); +hold off; +#+end_src + +#+begin_src matlab +figure; +scatter(Rx_nass(:,2), Rx_nass(:,3), 1, Rx_nass(:,4), 'filled') +colormap winter +#+end_src +**** Simscape Simulations +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ... + 'flex_top_type', '3dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof', ... + 'controller_type', 'hac-iff-struts'); +#+end_src + +#+begin_src matlab +set_param(mdl,'StopTime', num2str(Rx_nass(end,1))) +set_param(mdl,'SimulationCommand','start') +#+end_src + +#+begin_src matlab +out.X.Data = out.X.Data - out.X.Data(1,:); +#+end_src + +#+begin_src matlab :exports none +figure; +hold on; +set(gca,'ColorOrderIndex',2) +plot(1e6*out.X.Data(out.X.Data(:,3)>0, 1), 1e6*out.X.Data(out.X.Data(:,3)>0, 2), '-', ... + 'DisplayName', 'Meas. Motion') +plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k--', ... + 'DisplayName', 'Reference Path') +hold off; +xlabel('X displacement [$\mu m$]'); ylabel('Y displacement [$\mu m$]'); +legend('location', 'southwest'); +#+end_src + +*** Save Reference paths +#+begin_src matlab :tangle no +save('matlab/mat/reference_path.mat', 'Rx_yz', 'Rx_nass') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/reference_path.mat', 'Rx_yz', 'Rx_nass') +#+end_src + +*** Experimental Results + +** Feedforward (Open-Loop) Control +*** Introduction + +#+begin_src latex :file control_architecture_iff_feedforward.pdf +\begin{tikzpicture} + % Blocs + \node[block={3.0cm}{3.0cm}] (P) {Plant}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); + \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); + + \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; + \node[addb, left= of inputF] (addF) {}; + \node[block, left= of addF] (Kff) {$\bm{K}_{\mathcal{L},\text{ff}}$}; + \node[block, align=center, left= of Kff] (J) {Inverse\\Kinematics}; + + % Connections and labels + \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.north); + \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$}; + + \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$}; + + \draw[->] (Kff.east) -- (addF.west) node[above left]{$\bm{u}_{\text{ff}}$}; + \draw[->] (J.east) -- (Kff.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; + \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0); +\end{tikzpicture} +#+end_src + +#+name: fig:control_architecture_iff_feedforward +#+caption: Feedforward control in the frame of the legs +#+RESULTS: +[[file:figs/control_architecture_iff_feedforward.png]] + +*** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no +addpath('./matlab/mat/'); +addpath('./matlab/src/'); +addpath('./matlab/'); +#+end_src + +#+begin_src matlab :eval no +addpath('./mat/'); +addpath('./src/'); +#+end_src + +#+begin_src matlab +load('damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt') +#+end_src + +#+begin_src matlab :tangle no +%% Add all useful folders to the path +addpath('matlab/nass-simscape/matlab/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/nano_hexapod/') +addpath('matlab/nass-simscape/STEPS/png/') +addpath('matlab/nass-simscape/src/') +addpath('matlab/nass-simscape/mat/') +#+end_src + +#+begin_src matlab :eval no +%% Add all useful folders to the path +addpath('nass-simscape/matlab/nano_hexapod/') +addpath('nass-simscape/STEPS/nano_hexapod/') +addpath('nass-simscape/STEPS/png/') +addpath('nass-simscape/src/') +addpath('nass-simscape/mat/') +#+end_src + +#+begin_src matlab +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +open(mdl) + +Rx = zeros(1, 7); + +colors = colororder; +#+end_src + +*** Simple Feedforward Controller +Let's estimate the mean DC gain for the damped plant (diagonal elements:) +#+begin_src matlab :results value replace :exports results :tangle no +mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:)))))) +#+end_src + +#+RESULTS: +: 1.773e-05 + +The feedforward controller is then taken as the inverse of this gain (the minus sign is there manually added as it is "removed" by the =abs= function): +#+begin_src matlab +Kff_iff_L = -1/mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:)))))); +#+end_src + +The open-loop gain (feedforward controller times the damped plant) is shown in Figure [[fig:open_loop_gain_feedforward_iff_struts]]. + +#+begin_src matlab :exports none +%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:6 + set(gca,'ColorOrderIndex',1); + plot(f, abs(Kff_iff_L*G_enc_iff_opt(:,i,i)), 'k-'); +end + +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [-]'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e1]); + +ax2 = nexttile; +hold on; +for i = 1:6 + set(gca,'ColorOrderIndex',1); + plot(f, 180/pi*angle(Kff_iff_L*G_enc_iff_opt(:,i,i)), 'k-') +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([1, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/open_loop_gain_feedforward_iff_struts.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:open_loop_gain_feedforward_iff_struts +#+caption: Diagonal elements of the "open loop gain" +#+RESULTS: +[[file:figs/open_loop_gain_feedforward_iff_struts.png]] + +And save the feedforward controller for further use: +#+begin_src matlab +Kff_iff_L = zpk(Kff_iff_L)*eye(6); +#+end_src + +#+begin_src matlab :tangle no +save('matlab/mat/feedforward_iff.mat', 'Kff_iff_L') +#+end_src + +#+begin_src matlab :exports none :eval no +save('mat/feedforward_iff.mat', 'Kff_iff_L') +#+end_src + +*** Test with Simscape Model +#+begin_src matlab +load('reference_path.mat', 'Rx_yz'); +#+end_src + +** Feedback/Feedforward control in the frame of the struts +*** Introduction :ignore: + +#+begin_src latex :file control_architecture_hac_iff_L_feedforward.pdf +\begin{tikzpicture} + % Blocs + \node[block={3.0cm}{3.0cm}] (P) {Plant}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); + \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); + + \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; + \node[addb, left= of inputF] (addF) {}; + \node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$}; + \node[block, above= of K] (Kff) {$\bm{K}_{\mathcal{L},\text{ff}}$}; + \node[addb, left= of K] (subr) {}; + \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics}; + + % Connections and labels + \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.north); + \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$}; + + \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south); + \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$}; + \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$}; + + \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}_n$}; + + \draw[->] (J.east) -- (subr.west); + \draw[->] ($(J.east) + (0.4, 0)$)node[branch]{} node[below]{$\bm{r}_{d\mathcal{L}}$} |- (Kff.west); + \draw[->] (Kff.east) -- ++(0.5, 0) -- (addF.north west); + + \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0); +\end{tikzpicture} +#+end_src + +#+name: fig:control_architecture_hac_iff_L_feedforward +#+caption: Feedback/Feedforward control in the frame of the legs +#+RESULTS: +[[file:figs/control_architecture_hac_iff_L_feedforward.png]] + + +* Functions +** =generateXYZTrajectory= +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/generateXYZTrajectory.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +function [ref] = generateXYZTrajectory(args) +% generateXYZTrajectory - +% +% Syntax: [ref] = generateXYZTrajectory(args) +% +% Inputs: +% - args +% +% Outputs: +% - ref - Reference Signal +#+end_src + +*** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +arguments + args.points double {mustBeNumeric} = zeros(2, 3) % [m] + + args.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 % Time to go to first point and after last point [s] + args.tw (1,1) double {mustBeNumeric, mustBePositive} = 0.5 % Time wait between each point [s] + args.tm (1,1) double {mustBeNumeric, mustBePositive} = 1 % Motion time between points [s] + + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s] +end +#+end_src + +*** Initialize Time Vectors +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +time_i = 0:args.Ts:args.ti; +time_w = 0:args.Ts:args.tw; +time_m = 0:args.Ts:args.tm; +#+end_src + +*** XYZ Trajectory +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +% Go to initial position +xyz = (args.points(1,:))'*(time_i/args.ti); + +% Wait +xyz = [xyz, xyz(:,end).*ones(size(time_w))]; + +% Scans +for i = 2:size(args.points, 1) + % Go to next point + xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)]; + % Wait a litle bit + xyz = [xyz, xyz(:,end).*ones(size(time_w))]; +end + +% End motion +xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)]; +#+end_src + +*** Reference Signal +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +t = 0:args.Ts:args.Ts*(length(xyz) - 1); +#+end_src + +#+begin_src matlab +ref = zeros(length(xyz), 7); + +ref(:, 1) = t; +ref(:, 2:4) = xyz'; +#+end_src + +** =generateYZScanTrajectory= +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/generateYZScanTrajectory.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +function [ref] = generateYZScanTrajectory(args) +% generateYZScanTrajectory - +% +% Syntax: [ref] = generateYZScanTrajectory(args) +% +% Inputs: +% - args +% +% Outputs: +% - ref - Reference Signal +#+end_src + +*** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +arguments + args.y_tot (1,1) double {mustBeNumeric} = 10e-6 % [m] + args.z_tot (1,1) double {mustBeNumeric} = 10e-6 % [m] + + args.n (1,1) double {mustBeInteger, mustBePositive} = 10 % [-] + + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s] + + args.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] + args.tw (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] + args.ty (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] + args.tz (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] +end +#+end_src + +*** Initialize Time Vectors +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +time_i = 0:args.Ts:args.ti; +time_w = 0:args.Ts:args.tw; +time_y = 0:args.Ts:args.ty; +time_z = 0:args.Ts:args.tz; +#+end_src + +*** Y and Z vectors +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +% Go to initial position +y = (time_i/args.ti)*(args.y_tot/2); + +% Wait +y = [y, y(end)*ones(size(time_w))]; + +% Scans +for i = 1:args.n + if mod(i,2) == 0 + y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot]; + else + y = [y, (args.y_tot/2) - (time_y/args.ty)*args.y_tot]; + end + + if i < args.n + y = [y, y(end)*ones(size(time_z))]; + end +end + +% Wait a litle bit +y = [y, y(end)*ones(size(time_w))]; + +% End motion +y = [y, y(end) - y(end)*time_i/args.ti]; +#+end_src + +#+begin_src matlab +% Go to initial position +z = (time_i/args.ti)*(args.z_tot/2); + +% Wait +z = [z, z(end)*ones(size(time_w))]; + +% Scans +for i = 1:args.n + z = [z, z(end)*ones(size(time_y))]; + + if i < args.n + z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)]; + end +end + +% Wait a litle bit +z = [z, z(end)*ones(size(time_w))]; + +% End motion +z = [z, z(end) - z(end)*time_i/args.ti]; +#+end_src + +*** Reference Signal +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +t = 0:args.Ts:args.Ts*(length(y) - 1); +#+end_src + +#+begin_src matlab +ref = zeros(length(y), 7); + +ref(:, 1) = t; +ref(:, 3) = y; +ref(:, 4) = z; +#+end_src + +** =getTransformationMatrixAcc= +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/getTransformationMatrixAcc.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +function [M] = getTransformationMatrixAcc(Opm, Osm) +% getTransformationMatrixAcc - +% +% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm) +% +% Inputs: +% - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers +% - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation +% +% Outputs: +% - M - Transformation Matrix +#+end_src + +*** Transformation matrix from motion of the solid body to accelerometer measurements +:PROPERTIES: +:UNNUMBERED: t +:END: + +Let's try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in $\{O\}$. +For any point $p_i$ of the solid body (corresponding to an accelerometer), we can write: +\begin{equation} +\begin{bmatrix} +a_{i,x} \\ a_{i,y} \\ a_{i,z} +\end{bmatrix} = \begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z +\end{bmatrix} + p_i \times \begin{bmatrix} +\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} +\end{equation} + +We can write the cross product as a matrix product using the skew-symmetric transformation: +\begin{equation} +\begin{bmatrix} +a_{i,x} \\ a_{i,y} \\ a_{i,z} +\end{bmatrix} = \begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z +\end{bmatrix} + \underbrace{\begin{bmatrix} + 0 & p_{i,z} & -p_{i,y} \\ + -p_{i,z} & 0 & p_{i,x} \\ + p_{i,y} & -p_{i,x} & 0 +\end{bmatrix}}_{P_{i,[\times]}} \cdot \begin{bmatrix} +\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} +\end{equation} + +If we now want to know the (scalar) acceleration $a_i$ of the point $p_i$ in the direction of the accelerometer direction $\hat{s}_i$, we can just project the 3d acceleration on $\hat{s}_i$: +\begin{equation} +a_i = \hat{s}_i^T \cdot \begin{bmatrix} +a_{i,x} \\ a_{i,y} \\ a_{i,z} +\end{bmatrix} = \hat{s}_i^T \cdot \begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z +\end{bmatrix} + \left( \hat{s}_i^T \cdot P_{i,[\times]} \right) \cdot \begin{bmatrix} +\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} +\end{equation} + +Which is equivalent as a simple vector multiplication: +\begin{equation} +a_i = \begin{bmatrix} +\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]} +\end{bmatrix} +\begin{bmatrix} +\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z +\end{bmatrix} = \begin{bmatrix} +\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]} +\end{bmatrix} {}^O\vec{x} +\end{equation} + +And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form. +We obtain Eq. eqref:eq:M_matrix. +#+begin_important +The transformation from solid body acceleration ${}^O\vec{x}$ from sensor measured acceleration $\vec{a}$ is: +\begin{equation} \label{eq:M_matrix} +\vec{a} = \underbrace{\begin{bmatrix} +\hat{s}_1^T & \hat{s}_1^T \cdot P_{1,[\times]} \\ +\vdots & \vdots \\ +\hat{s}_6^T & \hat{s}_6^T \cdot P_{6,[\times]} +\end{bmatrix}}_{M} {}^O\vec{x} +\end{equation} + +with $\hat{s}_i$ the unit vector representing the measured direction of the i'th accelerometer expressed in frame $\{O\}$ and $P_{i,[\times]}$ the skew-symmetric matrix representing the cross product of the position of the i'th accelerometer expressed in frame $\{O\}$. +#+end_important + +Let's define such matrix using matlab: +#+begin_src matlab +M = zeros(length(Opm), 6); + +for i = 1:length(Opm) + Ri = [0, Opm(3,i), -Opm(2,i); + -Opm(3,i), 0, Opm(1,i); + Opm(2,i), -Opm(1,i), 0]; + M(i, 1:3) = Osm(:,i)'; + M(i, 4:6) = Osm(:,i)'*Ri; +end +#+end_src + +#+begin_src matlab +end +#+end_src + + +** =getJacobianNanoHexapod= +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/getJacobianNanoHexapod.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab +function [J] = getJacobianNanoHexapod(Hbm) +% getJacobianNanoHexapod - +% +% Syntax: [J] = getJacobianNanoHexapod(Hbm) +% +% Inputs: +% - Hbm - Height of {B} w.r.t. {M} [m] +% +% Outputs: +% - J - Jacobian Matrix +#+end_src + +*** Transformation matrix from motion of the solid body to accelerometer measurements +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab +Fa = [[-86.05, -74.78, 22.49], + [ 86.05, -74.78, 22.49], + [ 107.79, -37.13, 22.49], + [ 21.74, 111.91, 22.49], + [-21.74, 111.91, 22.49], + [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m] + +Mb = [[-28.47, -106.25, -22.50], + [ 28.47, -106.25, -22.50], + [ 106.25, 28.47, -22.50], + [ 77.78, 77.78, -22.50], + [-77.78, 77.78, -22.50], + [-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m] + +H = 95e-3; % Stewart platform height [m] +Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m] + +si = Fb - Fa; +si = si./vecnorm(si); % Normalize + +Bb = Mb - [0; 0; Hbm]; + +J = [si', cross(Bb, si)']; +#+end_src diff --git a/test-bench-nano-hexapod.pdf b/test-bench-nano-hexapod.pdf index a0367be..6ebd7f2 100644 Binary files a/test-bench-nano-hexapod.pdf and b/test-bench-nano-hexapod.pdf differ
Table 2: Description of the identified modes