From af96a4b64f80ad10fd46e4d3047b8bc076cfdb5f Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Sun, 7 Jan 2024 19:27:38 +0800 Subject: [PATCH] robot.regressor.K have bug and the result need to verify --- Identification_main.asv | 10 +++++ Identification_main.m | 12 +++++- get_Kinematics.m | 85 ++++++++++++++++++++++++++++------------- get_regressor.asv | 65 +++++++++++++++++++++++++++++++ get_regressor.m | 72 ++++++++++++++++++++++++++++++++++ get_robot.asv | 52 +++++++++++++++++++++++++ get_robot.m | 49 ++++++++++++++++++------ get_velocity.asv | 31 +++++++++++++++ get_velocity.m | 38 ++++++++++++++++++ vec2linearSymMat.m | 5 +++ 10 files changed, 380 insertions(+), 39 deletions(-) create mode 100644 Identification_main.asv create mode 100644 get_regressor.asv create mode 100644 get_regressor.m create mode 100644 get_robot.asv create mode 100644 get_velocity.asv create mode 100644 get_velocity.m create mode 100644 vec2linearSymMat.m diff --git a/Identification_main.asv b/Identification_main.asv new file mode 100644 index 0000000..16c898d --- /dev/null +++ b/Identification_main.asv @@ -0,0 +1,10 @@ +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 da7990d..5bd6817 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -1,6 +1,16 @@ file = []; opt.robot_def = 'direct'; opt.KM_method = 'MDH'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = true; + + +opt.Isreal = false; robot = get_robot(file,opt); -robot.theta = [1,0,0]; +% robot.theta = [1,1,0]; robot = get_Kinematics(robot, opt); + +opt.Isreal = false; +robot = get_velocity(robot, opt); +robot = get_regressor(robot,opt); \ No newline at end of file diff --git a/get_Kinematics.m b/get_Kinematics.m index 35efb6c..17abdbc 100644 --- a/get_Kinematics.m +++ b/get_Kinematics.m @@ -1,29 +1,60 @@ function robot = get_Kinematics(robot, opt) -switch opt.KM_method - case 'SDH' - case 'MDH' - theta = robot.theta; - alpha = robot.alpha; - a = robot.a; - d = robot.d; - robot.Fkine = eye(4,4); - ndof = length(theta); % special for MDH - % init transform matrix - robot.R = zeros([3,3,ndof]); - robot.t = zeros([3,1,ndof]); - robot.T = zeros([4,4,ndof]); - for i = 1:ndof - robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;... - sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));... - sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))]; - robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))]; - Transform = eye(4,4); - Transform(1:3,1:3) = robot.R(:,:,i); - Transform(1:3,4) = robot.t(:,:,i); - robot.T(:,:,i) = Transform; - robot.Fkine = robot.Fkine*robot.T(:,:,i); - end - otherwise - disp('Bad opt.KM_method!') - return; +if(opt.Isreal) + switch opt.KM_method + case 'SDH' + case 'MDH' + theta = robot.theta; + alpha = robot.alpha; + a = robot.a; + d = robot.d; + robot.Fkine = eye(4,4); + ndof = length(theta); % special for MDH + % init transform matrix + robot.R = zeros([3,3,ndof]); + robot.t = zeros([3,1,ndof]); + robot.T = zeros([4,4,ndof]); + for i = 1:ndof + robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;... + sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));... + sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))]; + robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))]; + Transform = eye(4,4); + Transform(1:3,1:3) = robot.R(:,:,i); + Transform(1:3,4) = robot.t(:,:,i); + robot.T(:,:,i) = Transform; + robot.Fkine = robot.Fkine*robot.T(:,:,i); + end + otherwise + disp('Bad opt.KM_method!') + return; + end +else + switch opt.KM_method + case 'SDH' + case 'MDH' + theta = robot.theta; + alpha = robot.alpha; + a = robot.a; + d = robot.d; + robot.Fkine = eye(4,4); + ndof = length(theta); % special for MDH + % init transform matrix + robot.R = sym(zeros([3,3,ndof])); + robot.t = sym(zeros([3,1,ndof])); + robot.T = sym(zeros([4,4,ndof])); + for i = 1:ndof + robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;... + sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));... + sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))]; + robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))]; + Transform = sym(eye(4,4)); + Transform(1:3,1:3) = robot.R(:,:,i); + Transform(1:3,4) = robot.t(:,:,i); + robot.T(:,:,i) = Transform; + robot.Fkine = robot.Fkine*robot.T(:,:,i); + end + otherwise + disp('Bad opt.KM_method!') + return; + end end \ No newline at end of file diff --git a/get_regressor.asv b/get_regressor.asv new file mode 100644 index 0000000..da5e0a7 --- /dev/null +++ b/get_regressor.asv @@ -0,0 +1,65 @@ +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_regressor.m b/get_regressor.m new file mode 100644 index 0000000..d9b8e46 --- /dev/null +++ b/get_regressor.m @@ -0,0 +1,72 @@ +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_; + delta_ = zeros([ndof,6*ndof]); + for i = 1:ndof + for j = 1:ndof + delta_(i,6*j) = 1; + end + end + robot.regressor.K = delta_*robot.regressor.U; + if(opt.debug) + sprintf('size of U=%dx%d.',size(robot.regressor.U)); + sprintf('size of K=%dx%d.',size(robot.regressor.K)); + end + 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 new file mode 100644 index 0000000..6db59f5 --- /dev/null +++ b/get_robot.asv @@ -0,0 +1,52 @@ +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.m b/get_robot.m index 325b46f..1178687 100644 --- a/get_robot.m +++ b/get_robot.m @@ -2,17 +2,41 @@ function robot = get_robot(file,opt) switch opt.robot_def case 'direct' ndof = 2; + robot.ndof = ndof; % Kinematics parameters - 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; + if(opt.Isreal) + switch opt.KM_method + case 'SDH' + case 'MDH' + robot.theta = zeros([1,ndof]); + robot.a = [0,1,1]; + robot.d = [0,0,0]; + robot.alpha = [0,0,0]; + otherwise + disp('Bad opt.KM_method!') + return; + end + else + % Create symbolic generilized coordiates, their first and second deriatives + 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'); + q_sym(ndof+1) = 0; + qd_sym(ndof+1) = 0; + q2d_sym(ndof+1) = 0; + robot.theta = q_sym; + robot.dtheta = qd_sym; + robot.ddtheta = q2d_sym; + switch opt.KM_method + case 'SDH' + case 'MDH' + robot.a = [0,1,1]; + robot.d = [0,0,0]; + robot.alpha = [0,0,0]; + otherwise + disp('Bad opt.KM_method!') + return; + end end % Dynamics parameters link_mass = [1,1]; @@ -47,4 +71,7 @@ switch opt.robot_def robot = []; disp('Bad robot define options!') return -end \ No newline at end of file +end +%Gravity +gravity = [0;0;9.8]; +robot.gravity = gravity; \ No newline at end of file diff --git a/get_velocity.asv b/get_velocity.asv new file mode 100644 index 0000000..bb70b5f --- /dev/null +++ b/get_velocity.asv @@ -0,0 +1,31 @@ +function robot = get_velocity(robot, opt) +switch opt.Vel_method + case 'Direct' + Z = [0,0,1]'; + w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity; + % w = zeros(3,number); + % dw = zeros(3,number); + % dv = zeros(3,number); + theta = robot.theta; + dtheta = robot.dtheta; + ddtheta = robot.ddtheta; + R = robot.R; + P = robot.t; + % 1-n外推公式 + %第一关节 + w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z; + dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z; + dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0); + %后面n-1关节 + for i = 1:robot.ndof + w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ; + dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z; + dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i)); + end + robot.vel.w = w; + robot.vel.dw = dw; + robot.vdv = dv; + otherwise + disp('Bad opt.KM_method!') + return; +end \ No newline at end of file diff --git a/get_velocity.m b/get_velocity.m new file mode 100644 index 0000000..3843ab9 --- /dev/null +++ b/get_velocity.m @@ -0,0 +1,38 @@ +function robot = get_velocity(robot, opt) +switch opt.KM_method + case 'SDH' + case 'MDH' + switch opt.Vel_method + case 'Direct' + Z = [0,0,1]'; + w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity; + % w = zeros(3,number); + % dw = zeros(3,number); + % dv = zeros(3,number); + theta = robot.theta; + dtheta = robot.dtheta; + ddtheta = robot.ddtheta; + R = robot.R; + P = robot.t; + % 1-n外推公式 + %第一关节 + w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z; + dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z; + dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0); + %后面n-1关节 + for i = 1:robot.ndof + w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ; + dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z; + dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i)); + end + robot.vel.w = w; + robot.vel.dw = dw; + robot.vel.dv = dv; + otherwise + disp('Bad opt.Vel_method!') + return; + end + otherwise + disp('Bad opt.KM_method!') + return; +end diff --git a/vec2linearSymMat.m b/vec2linearSymMat.m new file mode 100644 index 0000000..9c82831 --- /dev/null +++ b/vec2linearSymMat.m @@ -0,0 +1,5 @@ +function matrix = vec2linearSymMat(w) +matrix = [w(1),w(2),w(3),zeros(1,3);... + 0,w(1),0,w(2),w(3),0;... + 0,0,w(1),0,w(2),w(3)]; +end \ No newline at end of file