From cc9a842f0b631eda1a35d7046a358efbab86c89a Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Sat, 21 Sep 2024 15:43:05 +0800 Subject: [PATCH] add R1000 identification --- Identification_main.asv | 10 ------- Identification_main.m | 2 +- get_regressor.asv | 65 ----------------------------------------- get_robot.asv | 52 --------------------------------- get_robot_R1000.m | 28 ++++++++++++++---- 5 files changed, 24 insertions(+), 133 deletions(-) delete mode 100644 Identification_main.asv delete mode 100644 get_regressor.asv delete mode 100644 get_robot.asv diff --git a/Identification_main.asv b/Identification_main.asv deleted file mode 100644 index 16c898d..0000000 --- a/Identification_main.asv +++ /dev/null @@ -1,10 +0,0 @@ -file = []; -opt.robot_def = 'direct'; -opt.KM_method = 'MDH'; -opt.LD_method = 'direct'; - -robot = get_robot(file,opt); -robot.theta = [1,1,0]; -robot = get_Kinematics(robot, opt); - -robot = get \ No newline at end of file diff --git a/Identification_main.m b/Identification_main.m index d6a1369..93e4a6a 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -7,7 +7,7 @@ opt.debug = true; opt.robotName = 'Two_bar'; opt.Isreal = false; -robot = get_robot(file,opt); +robot = get_robot_R1000(file,opt); % robot.theta = [1,1,0]; robot = get_Kinematics(robot, opt); diff --git a/get_regressor.asv b/get_regressor.asv deleted file mode 100644 index da5e0a7..0000000 --- a/get_regressor.asv +++ /dev/null @@ -1,65 +0,0 @@ -function robot = get_regressor(robot, opt) -% Create symbolic generilized coordiates, their first and second deriatives -ndof = robot.ndof; -q_sym = sym('q%d',[ndof+1,1],'real'); -qd_sym = sym('qd%d',[ndof+1,1],'real'); -q2d_sym = sym('q2d%d',[ndof+1,1],'real'); -% init regressor -robot.regressor.m = sym('m%d',[ndof,1],'real'); -robot.regressor.com = sym('com%d',[ndof,1],'real'); -robot.regressor.I = sym('I%d',[ndof,1],'real'); -robot.regressor.I_vec = inertiaMatrix2Vector(robot.regressor.I); -robot.regressor.mc = robot.regressor.m.*robot.regressor.com; -robot.regressor.pi = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)]; -% init matrix -R = robot.R; -P = robot.t; -w = robot.vel.w ; -dw = robot.vel.dw ; -dv = robot.vel.dv ; -switch opt.LD_method - case 'Direct' - switch opt.KM_method - case 'MDH' - for i = 2:ndof+1 - p_skew(:,:,i) = vec2skewSymMat(P(:,:,i)); - w_skew(:,:,i) = vec2skewSymMat(w(:,i)); - dw_skew(:,:,i) = vec2skewSymMat(dw(:,i)); - dv_skew(:,:,i) = vec2skewSymMat(dv(:,i)); - w_l(:,:,i) = vec2linearSymMat(w(:,i)); - dw_l(:,:,i) = vec2linearSymMat(dw(:,i)); - % size of matrix A is 6*10, need to -1 - robot.regressor.A(:,:,i-1) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ... - zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)]; - end - % construct matrix U, size: [6*n,10*n] - % U_ = sym(zeros([6*ndof,10*ndof])); - U_ = []; - for i = 1:ndof - % tricky - for j = i:ndof - if(j == i) - TT = eye(6,6); - U_row = TT*robot.regressor.A(:,:,j); - else - TT = TT*Adjoint(RpToTrans(R(:,:,j),P(:,:,j))); - U_row = [U_row,TT*robot.regressor.A(:,:,j)]; - end - end - U_ = [U_;zeros(6,(i-1)*10),U_row]; - end - robot.regressor.U = U_; - if(opt.debug) - sprintf('size of U_=%dx%d.',size(robot.regressor.U)); - end - robot.regressor.K = [zeros(1,3),Z0]*; - end -% matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',... -% 'Vars',{q_sym,qd_sym,q2d_sym}); - case 'Lagrange' - disp('TODO opt.LD_method Lagrange!') - return; - otherwise - disp('Bad opt.KM_method!') - return; -end \ No newline at end of file diff --git a/get_robot.asv b/get_robot.asv deleted file mode 100644 index 6db59f5..0000000 --- a/get_robot.asv +++ /dev/null @@ -1,52 +0,0 @@ -function robot = get_robot(file,opt) -switch opt.robot_def - case 'direct' - ndof = 2; - % Kinematics parameters - switch opt.Isreal - case - switch opt.KM_method - case 'SDH' - case 'MDH' - robot.theta = zeros([1,ndof+1]); - robot.a = [0,1,1]; - robot.d = [0,0,0]; - robot.alpha = [0,0,0]; - otherwise - disp('Bad opt.KM_method!') - return; - end - % Dynamics parameters - link_mass = [1,1]; - axis_of_rot(:,:,1) = [0;0;1]; - axis_of_rot(:,:,2) = [0;0;1]; - com_pos(:,:,1) = [1/2;0;0]; - com_pos(:,:,2) = [1/2;0;0]; - % the inertia tensor wrt the frame oriented as the body frame and with the - % origin in the COM - link_inertia(:,:,1) = diag([1,1,1]); - link_inertia(:,:,2) = diag([1,1,1]); - % manipulator regressor - for i = 1:ndof - robot.m(i) = link_mass(i); - robot.axis(:,i) = axis_of_rot(i); - robot.com(:,i) = com_pos(i); - robot.I(:,:,i) = link_inertia(i); - robot.mc(:,i) = link_mass*com_pos(i); - % the inertia tensor wrt the frame oriented as the body frame and with the - % origin in the Joint i - com_vec2mat = vec2skewSymMat(com_pos); - robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-... - link_mass(i)*com_vec2mat*com_vec2mat); - robot.pi(:,i) = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)]; - end - case 'urdf' - robot = parse_urdf(file); - case 'mat' - robot = []; - disp('TODO mat robot define options!') - otherwise - robot = []; - disp('Bad robot define options!') - return -end \ No newline at end of file diff --git a/get_robot_R1000.m b/get_robot_R1000.m index 8b4b22c..cf76a75 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -1,7 +1,7 @@ -function robot = get_robot(file,opt) +function robot = get_robot_R1000(file,opt) switch opt.robot_def case 'direct' - ndof = 2; + ndof = 8; robot.ndof = ndof; % Kinematics parameters if(opt.Isreal) @@ -31,7 +31,7 @@ switch opt.robot_def case 'SDH' case 'MDH' robot.a = [0,0.2,0.5,0.45,0.12,0,0,0]; - robot.d = [0,0,0,0,0,0,0,0]; + robot.d = [0,0,0,0,0,0.28,0.40,0]; robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2]; otherwise disp('Bad opt.KM_method!') @@ -43,13 +43,31 @@ switch opt.robot_def %TODO in process, seems axis_of_rot useless axis_of_rot(:,:,1) = [0;0;1]; axis_of_rot(:,:,2) = [0;0;1]; + axis_of_rot(:,:,3) = [0;0;1]; + axis_of_rot(:,:,4) = [0;0;1]; + axis_of_rot(:,:,5) = [0;0;1]; + axis_of_rot(:,:,6) = [0;0;1]; + axis_of_rot(:,:,7) = [0;0;1]; + axis_of_rot(:,:,8) = [0;0;1]; % 画图 - com_pos(:,:,1) = [1/2;0;0]; - com_pos(:,:,2) = [1/2;0;0]; + com_pos(:,:,1) = [0;0;0]; + com_pos(:,:,2) = [0.2/2;0;0]; + com_pos(:,:,3) = [0.5/2;0;0]; + com_pos(:,:,4) = [0.45/2;0;0]; + com_pos(:,:,5) = [0.12/2;0;0]; + com_pos(:,:,6) = [0.28/2;0;0]; + com_pos(:,:,7) = [0.4/2;0;0]; + com_pos(:,:,8) = [0.2772/2;0;0]; % the inertia tensor wrt the frame oriented as the body frame and with the % origin in the COM link_inertia(:,:,1) = diag([1,1,1]); link_inertia(:,:,2) = diag([1,1,1]); + link_inertia(:,:,3) = diag([1,1,1]); + link_inertia(:,:,4) = diag([1,1,1]); + link_inertia(:,:,5) = diag([1,1,1]); + link_inertia(:,:,6) = diag([1,1,1]); + link_inertia(:,:,7) = diag([1,1,1]); + link_inertia(:,:,8) = diag([1,1,1]); % manipulator regressor for i = 1:ndof robot.m(i) = link_mass(i);