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..e5415ff 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -1,19 +1,32 @@ +close all;clc;clear file = []; opt.robot_def = 'direct'; -opt.KM_method = 'MDH'; +opt.KM_method = 'SCREW'; opt.Vel_method = 'Direct'; opt.LD_method = 'Direct'; -opt.debug = true; -opt.robotName = 'Two_bar'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = true; +opt.isSixAxisFTSensor = false; -opt.Isreal = false; -robot = get_robot(file,opt); -% robot.theta = [1,1,0]; -robot = get_Kinematics(robot, opt); +theta = zeros(9,1); +dtheta = zeros(9,1); +ddtheta = zeros(9,1); -opt.Isreal = false; +get_robot_func = sprintf('get_robot_%s',opt.robotName); +robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt); +get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName); +robot = feval(get_Kinematics_func,robot,opt); + +% R1000_Dynamics_num; +% R1000_Dynamics; robot = get_velocity(robot, opt); robot = get_regressor(robot,opt); % symbol matched -verify_regressor -robot = get_baseParams(robot, opt); \ No newline at end of file +% verify_regressor_R1000; +robot = get_baseParams(robot, opt); +% robot = estimate_dyn(robot,opt); +% robot = estimate_dyn_form_data(robot,opt); +robot = estimate_dyn_MLS(robot,opt); \ No newline at end of file diff --git a/R1000_Dynamics.m b/R1000_Dynamics.m new file mode 100644 index 0000000..38c98ec --- /dev/null +++ b/R1000_Dynamics.m @@ -0,0 +1,60 @@ +%% R1000 +N=length(robot.theta); +% Dynamics parameters +link_mass = robot.m; +com_pos = robot.com; +link_inertia = robot.I; + +q_sym = sym('q%d',[N,1],'real'); +qd_sym = zeros(N,1);%sym('qd%d',[N,1],'real'); +q2d_sym = zeros(N,1);%sym('qdd%d',[N,1],'real'); +thetalist = q_sym(1:N); +dthetalist = qd_sym(1:N); +ddthetalist = q2d_sym(1:N); + +% Get general mass matrix +Glist=[]; +for i = 1:N + Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])]; + Glist = cat(3, Glist, Gb); +end + + +Mlist_CG = robot.kine.Mlist_CG; +Mlist_ED = robot.kine.Mlist_ED; + +%TODO: Get Slist form DH table method +% RRRRRRRRP +Slist=robot.slist; + +Vlinear=sym(zeros(3,3)); +J=sym(zeros(6,N)); +exf=[0;0;0;0;0;0]; + +[V1,Vd1,Adgab_mat,Fmat,tau_mat] ... + = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ... + [0;0;-9.806], exf, Mlist_CG, Glist, Slist); +% G = FKinSpaceExpand_Sym(Mlist_CG, Slist, thetalist); +% T=FKinSpaceExpand_Sym(Mlist_ED, Slist, thetalist); +% F_Simpack = getSimpackF_Sym(G,T,Mlist_ED,Fmat); + +%Gen Files +matlabFunction(tau_mat,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),... + 'Vars',{q_sym}); +standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName); +% traj +% time = 0:1:2; +% f=1; +% q_J = sin(2*pi*f*time); +% qd_J = (2*pi*f)*cos(2*pi*f*time); +% qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +% F_Simpack_list = feval(standard_dynamics_func, q_J',qd_J',qdd_J'); + + +% Use Body Twist cal linear vel, but can't cal the end frame vel +% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ... +% [0;0;0], exf, Mlist, Glist, Slist); +% j=1; +% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12); +% j=2; +% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23); \ No newline at end of file diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m new file mode 100644 index 0000000..ea24387 --- /dev/null +++ b/R1000_Dynamics_num.m @@ -0,0 +1,188 @@ +%% R1000 +N=9; +% traj +time = 0:0.01:1-0.01; +f=1; +q_J = sin(2*pi*f*time); +qd_J = (2*pi*f)*cos(2*pi*f*time); +qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +zero_ = zeros(1,length(q_J)); +q_J = -0.3*ones(1,length(q_J)); +ones_ = ones(1,length(q_J)); + +% Dynamics parameters +link_mass = robot.m; +com_pos = robot.com; +link_inertia = robot.I; + +% thetalist = [zero_;0.4807*ones_;-0.8717*ones_;0.3466*ones_;zero_;0.6965*ones_;-0.5224*ones_;0.3473*ones_;0.05*ones_]'; +% dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +% ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; + +%real traj +get_GCTraj_R1000_DVT; +thetalist = idntfcnTrjctry(6).q'; +dthetalist = idntfcnTrjctry(6).qd'; +ddthetalist = idntfcnTrjctry(6).qdd'; + +% Get general mass matrix +Glist=[]; +for i = 1:N + link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i); + Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])]; + Glist = cat(3, Glist, Gb); +end + +% Get the com pos transformation in each joint reference frame +% Mlist_CG = []; +% for i = 0:N-1 +% if i == 0 +% M = robot.T(:,:,i+1)*transl(com_pos(:,i+1)); +% else +% M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1)); +% end +% Mlist_CG = cat(3, Mlist_CG, M); +% end +% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; +% Mlist_CG = cat(3, Mlist_CG, M); + +% Get the com pos transformation in each joint reference frame +% FIXME: BUG here +% Mlist_CG=[]; +% for i = 0:N-1 +% if i == 0 +% M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); +% else +% rotation_i = diag([1,1,1]); +% for j = 1:i +% rotation_i = rotation_i*TransToRp(robot.T(:,:,i)); +% rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1)); +% end +% M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1)); +% end +% Mlist_CG = cat(3, Mlist_CG, M); +% end +% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; +% Mlist_CG = cat(3, Mlist_CG, M); + +% ct=[]; +% Mlist_CG=[]; +% for i = 1:N +% if i == 1 +% ct(:,i) = com_pos_R1(:,i); +% elseif i< 9 +% ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i); +% else +% ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i); +% end +% robot.Home.com(:,i) = ct(:,i); +% M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i)); +% Mlist_CG = cat(3, Mlist_CG, M); +% end +% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; +% Mlist_CG = cat(3, Mlist_CG, M); + +% Mlist_CG=[]; +% for i = 1:N +% if i == 1 +% M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1]; +% elseif i<=8 +% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i))); +% elseif i==9 +% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i))); +% end +% Mlist_CG = cat(3, Mlist_CG, M); +% end +% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; +% Mlist_CG = cat(3, Mlist_CG, M); + +% get the CG at the world base frame +com_pos_R1 = robot.com_pos_R1; +com_pos_R2 = robot.com_pos_R2; +ct=[]; +Mlist_CG_Base=[]; +for i = 1:N + if i == 1 + ct(:,i) = com_pos_R1(:,i); + elseif i< 9 + ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i); + else + ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i); + end + robot.Home.com(:,i) = ct(:,i); + M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i)); + Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base); +end +% get the CG at the last GC frame +Mlist_CG=[]; +for i = 1:N + if i == 1 + Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i); + else + Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i); + end +end +M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; +Mlist_CG = cat(3, Mlist_CG, M); + + +% Get the end efforce transformation in each joint reference frame +Mlist_ED = []; +for i = 1:N + M = robot.T(:,:,i); + Mlist_ED = cat(3, Mlist_ED, M); +end +M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; +Mlist_ED = cat(3, Mlist_ED, M); + +%TODO: Get Slist form DH table method +% RRRRRRRRP +Slist=robot.slist; + +Vlinear=sym(zeros(3,3)); +J=sym(zeros(6,N)); +exf=[0;0;0;0;0;0]; + +for i = 1:length(q_J) + [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ... + = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ... + [0;0;-9.806], exf, Mlist_CG, Glist, Slist); + G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)'); + T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)'); + %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin + %why we need Mlist_ED + %please explain this more + F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i)); +end +% plot Torque +% above 2020b +% F_Simpack = pagetranspose(F_Simpack); +% below 2020b + +for i = 1:3 + subplot(3,1,i); + hold on; + %added minus, so should be the same as simpack + plot(time,taumat(i+6,:)) + xlabel('time(s)') + ylabel('Torque(Nm)') +% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y) +end + +F_Simpack = permute(F_Simpack,[2 1 3]); +figure(2) +for i = 1:3 + subplot(3,1,i); + hold on; + %added minus, so should be the same as simpack + plot(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)])) +% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y) +end + +% Use Body Twist cal linear vel, but can't cal the end frame vel +% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ... +% [0;0;0], exf, Mlist, Glist, Slist); +% j=1; +% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12); +% j=2; +% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23); \ No newline at end of file diff --git a/R1000_Dynamics_num_regressor.m b/R1000_Dynamics_num_regressor.m new file mode 100644 index 0000000..54470d6 --- /dev/null +++ b/R1000_Dynamics_num_regressor.m @@ -0,0 +1,96 @@ +%% R1000 +N=9; +ndof = N; +% traj +time = 0:0.01:1; +f=1; +q_J = sin(2*pi*f*time); +qd_J = (2*pi*f)*cos(2*pi*f*time); +qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +zero_ = zeros(1,length(q_J)); + +thetalist = [q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +dthetalist = [qd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +% Dynamics parameters +link_mass = robot.m; +com_pos = robot.com; +link_inertia = robot.I; + + +% init regressor +robot.regressor.m = link_mass; +robot.regressor.mc_x = com_pos(1,:); +robot.regressor.mc_y = com_pos(2,:); +robot.regressor.mc_z = com_pos(3,:); +robot.regressor.ixx = link_inertia(1,1,:); +robot.regressor.ixy = link_inertia(1,2,:); +robot.regressor.ixz = link_inertia(1,3,:); +robot.regressor.iyy = link_inertia(2,2,:); +robot.regressor.iyz = link_inertia(2,3,:); +robot.regressor.izz = link_inertia(3,3,:); +robot.regressor.im = sym('im%d',[ndof,1],'real'); +for i = 1:ndof + robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),... + robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),... + robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]'; +end + +[nLnkPrms, nLnks] = size(robot.regressor.pi); +robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]); +% 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 = 1:ndof + 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) = [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 + delta_(i,6*i) = 1; + 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 + + 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/Untitled.m b/Untitled.m new file mode 100644 index 0000000..6f7f55d --- /dev/null +++ b/Untitled.m @@ -0,0 +1,17 @@ +com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'; +com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'; +com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'; +com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'; +com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'; +com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'; +com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'; +com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'; +com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'; +com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'; +com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'; +com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'; +com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'; +com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'; +com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'; +com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'; +com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'; \ No newline at end of file diff --git a/autogen/standard_dynamics_R1000.m b/autogen/standard_dynamics_R1000.m new file mode 100644 index 0000000..ec3ef07 --- /dev/null +++ b/autogen/standard_dynamics_R1000.m @@ -0,0 +1,341 @@ +function tau_mat = standard_dynamics_R1000(in1) +%standard_dynamics_R1000 +% TAU_MAT = standard_dynamics_R1000(IN1) + +% This function was generated by the Symbolic Math Toolbox version 9.1. +% 07-Oct-2024 20:05:17 + +q2 = in1(2,:); +q3 = in1(3,:); +q4 = in1(4,:); +q5 = in1(5,:); +q6 = in1(6,:); +q7 = in1(7,:); +q8 = in1(8,:); +q9 = in1(9,:); +t2 = cos(q2); +t3 = cos(q3); +t4 = cos(q4); +t5 = cos(q5); +t6 = cos(q6); +t7 = cos(q7); +t8 = cos(q8); +t9 = sin(q2); +t10 = sin(q3); +t11 = sin(q4); +t12 = sin(q5); +t13 = sin(q6); +t14 = sin(q7); +t15 = sin(q8); +t38 = q9+3.647515815399993e-2; +t16 = t2.*t3.*9.806; +t17 = t2.*t10.*9.806; +t18 = t3.*t9.*9.806; +t19 = t9.*t10.*9.806; +t21 = t2.*t3.*2.373052e+1; +t22 = t2.*t10.*2.373052e+1; +t23 = t3.*t9.*2.373052e+1; +t24 = t9.*t10.*2.373052e+1; +t32 = t4.*4.922552299946314e-7; +t33 = t11.*4.922552299946314e-7; +t36 = t3.*4.409419000001025e-5; +t37 = t10.*4.409419000001025e-5; +t39 = t8.*2.32103180000387e-5; +t40 = t15.*2.32103180000387e-5; +t43 = t7.*4.740360800000037e-3; +t44 = t14.*4.740360800000037e-3; +t45 = t6.*1.230157900000001e-2; +t46 = t13.*1.230157900000001e-2; +t48 = t8.*2.0257876e-1; +t49 = t15.*2.0257876e-1; +t51 = t3.*1.2654643e-1; +t52 = t10.*1.2654643e-1; +t55 = t5.*6.595131499999993e-2; +t56 = t12.*6.595131499999993e-2; +t57 = t4.*2.618828899999998e-1; +t58 = t11.*2.618828899999998e-1; +t62 = t6.*4.895353899999624e-5; +t63 = t13.*4.895353899999624e-5; +t68 = t7.*5.992854699999624e-5; +t69 = t14.*5.992854699999624e-5; +t70 = t5.*9.036099100000004e-3; +t71 = t12.*9.036099100000004e-3; +t20 = -t19; +t25 = -t24; +t26 = t17+t18; +t35 = -t32; +t41 = -t36; +t42 = -t40; +t50 = -t44; +t61 = -t56; +t67 = -t62; +t99 = t33+t57+5.404868499999996e-2; +t100 = t37+t51+1.8811711e-1; +t101 = t39+t49+3.657193499999997e-2; +t107 = t43+t69+2.32103180000387e-5; +t113 = t45+t63-2.8858572e-2; +t119 = t55+t71+1.302852800000001e-1; +t27 = t16+t20; +t28 = t4.*t26; +t29 = t11.*t26; +t97 = t41+t52+4.922552299946314e-7; +t98 = t35+t58+5.091563100000002e-3; +t102 = t42+t48+4.712426700000003e-2; +t105 = t4.*t99; +t106 = t11.*t99; +t108 = t8.*t101; +t109 = t15.*t101; +t114 = t7.*t107; +t115 = t14.*t107; +t118 = t46+t67+5.992854699999624e-5; +t120 = t6.*t113; +t121 = t13.*t113; +t123 = t12.*t119; +t124 = t61+t70+4.895353899999624e-5; +t125 = t50+t68+9.503114400000004e-3; +t128 = t5.*t119; +t30 = t4.*t27; +t31 = t11.*t27; +t34 = -t29; +t47 = t29.*(1.11e+2./5.0e+1); +t59 = t28.*(5.19e+2./1.0e+2); +t60 = t29.*(7.41e+2./1.0e+2); +t103 = t4.*t98; +t104 = t11.*t98; +t110 = -t106; +t111 = t8.*t102; +t112 = t15.*t102; +t117 = -t114; +t122 = -t121; +t126 = t6.*t118; +t127 = t13.*t118; +t129 = -t128; +t130 = t5.*t124; +t131 = t7.*t125; +t132 = t12.*t124; +t133 = t14.*t125; +t53 = t30.*(1.11e+2./5.0e+1); +t54 = -t47; +t64 = t30.*(7.41e+2./1.0e+2); +t65 = t31.*(5.19e+2./1.0e+2); +t66 = -t60; +t72 = t28+t31; +t73 = t30+t34; +t76 = -t6.*(t29-t30); +t77 = -t13.*(t29-t30); +t85 = t13.*(t29-t30).*(-9.0./5.0); +t92 = t6.*(t29-t30).*(-2.31e+2./1.0e+2); +t93 = t6.*(t29-t30).*(-4.11e+2./1.0e+2); +t116 = -t112; +t154 = t104+t105; +t155 = t103+t110; +t156 = t109+t111; +t164 = t115+t131; +t165 = t120+t127; +t166 = t117+t133; +t167 = t122+t126; +t168 = t123+t130; +t169 = t129+t132; +t74 = t6.*t12.*t72; +t75 = t12.*t13.*t72; +t78 = t5.*t7.*t72; +t80 = t5.*t72.*(9.0./5.0); +t82 = t5.*t14.*t72.*(6.0./5.0); +t84 = t5.*t72.*(2.01e+2./5.0e+1); +t86 = t12.*t72.*(1.11e+2./5.0e+1); +t88 = t5.*t14.*t72.*(1.31e+2./2.5e+1); +t89 = t5.*t14.*t72.*(2.93e+2./1.0e+2); +t96 = t5.*t14.*t72.*5.529408000000002e-2; +t160 = t108+t116; +t79 = -t74; +t81 = -t78; +t83 = t74.*(9.0./5.0); +t90 = t75.*(2.31e+2./1.0e+2); +t91 = t75.*(4.11e+2./1.0e+2); +t94 = t78.*(2.31e+2./1.0e+2); +t134 = t75+t76; +t136 = -t14.*(t74+t13.*(t29-t30)); +t138 = t7.*(t74+t13.*(t29-t30)).*(-6.0./5.0); +t141 = t7.*(t74+t13.*(t29-t30)).*(-1.31e+2./2.5e+1); +t143 = t7.*(t74+t13.*(t29-t30)).*(-2.93e+2./1.0e+2); +t144 = t14.*(t74+t13.*(t29-t30)).*(-2.31e+2./1.0e+2); +t147 = t7.*(t74+t13.*(t29-t30)).*(-5.529408000000002e-2); +t153 = t15.*(t78+t14.*(t74+t13.*(t29-t30))).*(-6.0./5.0); +t158 = t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(-2.93e+2./1.0e+2); +t159 = t15.*(t78+t14.*(t74+t13.*(t29-t30))).*(-2.93e+2./1.0e+2); +t170 = t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(-5.529408000000002e-2); +t171 = t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(-6.05542308e-4); +t172 = t15.*(t78+t14.*(t74+t13.*(t29-t30))).*(-6.05542308e-4); +t87 = -t83; +t95 = -t94; +t135 = t77+t79; +t137 = t8.*t134.*(6.0./5.0); +t139 = t8.*t134.*(2.93e+2./1.0e+2); +t140 = t15.*t134.*(2.93e+2./1.0e+2); +t145 = t15.*t134.*5.529408000000002e-2; +t148 = t15.*t134.*6.05542308e-4; +t149 = t8.*t134.*6.05542308e-4; +t151 = t81+t136; +t152 = t82+t138; +t157 = t88+t141; +t161 = t89+t143; +t142 = -t140; +t146 = -t145; +t150 = -t148; +t162 = t7.*t157; +t163 = t14.*t157; +t173 = t38.*t152; +t177 = t107.*t157; +t178 = t137+t153; +t179 = t139+t159; +t183 = -t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)); +t184 = -t15.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)); +t185 = t15.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)); +t190 = t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)).*(-9.140287820000001e-3); +t193 = t156.*t161; +t194 = t160.*t161; +t197 = -t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)); +t202 = t96+t147+t149+t172; +t174 = t162.*1.65162668e-1; +t175 = t163.*1.65162668e-1; +t180 = t142+t158; +t181 = t8.*t179; +t182 = t15.*t179; +t186 = t38.*t178; +t191 = t185.*(-9.140287820000001e-3); +t192 = t185.*9.140287820000001e-3; +t195 = -t194; +t196 = t101.*t179; +t198 = t150+t171+t173; +t199 = -t8.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4); +t200 = -t15.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4); +t201 = t15.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4); +t203 = t8.*t202; +t204 = t15.*t202; +t176 = -t175; +t187 = -t186; +t188 = t181.*9.140287820000001e-3; +t189 = t182.*9.140287820000001e-3; +t205 = t90+t92+t181+t185; +t206 = t91+t93+t181+t185; +t211 = t95+t144+t182+t183; +t212 = -t7.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))); +t213 = -t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))); +t214 = t7.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))); +t216 = t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))).*(-1.65162668e-1); +t218 = -t125.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))); +t219 = t125.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))); +t231 = -t6.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)); +t232 = -t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)); +t233 = t6.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)); +t235 = t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)).*(-1.448743539999999e-1); +t238 = -t113.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)); +t239 = t113.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)); +t244 = -t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2); +t245 = -t14.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2); +t207 = t13.*t206; +t208 = t6.*t206; +t215 = t214.*(-1.65162668e-1); +t217 = t118.*t206; +t220 = t164.*t205; +t221 = -t205.*(t114-t133); +t223 = t80+t163+t214; +t224 = t84+t163+t214; +t230 = t85+t87+t162+t213; +t234 = t233.*(-1.448743539999999e-1); +t236 = t233.*1.448743539999999e-1; +t243 = t146+t170+t187+t196+t197; +t265 = t188+t192+t193+t201+t203; +t279 = t177+t189+t190+t195+t199+t204+t219; +t209 = t208.*1.448743539999999e-1; +t210 = t207.*1.448743539999999e-1; +t222 = -t220; +t225 = t5.*t224; +t226 = t12.*t224; +t237 = t124.*t224; +t240 = t165.*t223; +t241 = -t223.*(t121-t126); +t246 = t86+t207+t233; +t247 = t53+t54+t208+t232; +t250 = t64+t66+t208+t232; +t251 = -t4.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +t252 = -t11.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +t253 = t11.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +t257 = t4.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))).*(-5.562904389999999e-2); +t260 = -t99.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +t262 = -t168.*(t47-t53-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +t263 = (t128-t132).*(t47-t53-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +t266 = t7.*t265; +t267 = t14.*t265; +t280 = t6.*t279; +t281 = t13.*t279; +t227 = -t226; +t228 = t225.*6.710016000000013e-3; +t229 = t226.*6.710016000000013e-3; +t242 = -t240; +t248 = t5.*t246; +t249 = t12.*t246; +t258 = t253.*(-5.562904389999999e-2); +t259 = t253.*5.562904389999999e-2; +t261 = t119.*t246; +t264 = -t263; +t268 = -t267; +t282 = -t280; +t286 = -t13.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133)); +t287 = -t6.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133)); +t288 = t13.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133)); +t289 = t6.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133)); +t290 = t174+t216+t217+t222+t239+t245+t266; +t254 = t248.*6.710016000000013e-3; +t255 = t249.*6.710016000000013e-3; +t269 = t227+t248; +t270 = t59+t65+t225+t249; +t276 = -t154.*(t226-t248); +t277 = -t155.*(t226-t248); +t278 = t155.*(t226-t248); +t285 = t176+t215+t221+t244+t268; +t291 = t5.*t290; +t292 = t12.*t290; +t294 = t209+t235+t242+t281+t289; +t297 = t210+t236+t237+t241+t261+t282+t288; +t256 = -t254; +t271 = t4.*t270; +t272 = t11.*t270; +t275 = t98.*t270; +t293 = -t292; +t295 = t5.*t294; +t296 = t12.*t294; +t298 = t4.*t297; +t299 = t11.*t297; +t273 = t271.*5.562904389999999e-2; +t274 = t272.*5.562904389999999e-2; +t283 = t21+t25+t251+t272; +t284 = t22+t23+t253+t271; +t300 = t229+t256+t262+t291+t296; +t301 = t4.*t300; +t302 = t11.*t300; +t303 = -t302; +t305 = t257+t274+t278+t299+t301; +t304 = t259+t273+t276+t298+t303; +et1 = t2.*(t2.*1.0040117e-1+t9.*3.452226e-2+3.734539500000001e-1); +et2 = t9.*(t2.*(-3.452226e-2)+t9.*1.0040117e-1+4.431314100000749e-5); +et3 = t226.*9.743525e-2-t248.*9.743525e-2+t2.*(t3.*t284.*1.043363139e-1-t10.*t283.*1.043363139e-1-t3.*t304+t10.*t305+(t226-t248).*(t3.*t100+t10.*t97)); +et4 = t9.*(t3.*t283.*(-1.043363139e-1)-t10.*t284.*1.043363139e-1+t3.*t305+t10.*t304+(t226-t248).*(t3.*t97-t10.*t100))+(et1+et2).*(t226-t248)+t9.*(t2.*7.55062e+1+t3.*t283+t10.*t284).*3.967117054999999e-2; +et5 = t2.*(t9.*7.55062e+1+t3.*t284-t10.*t283).*(-3.967117054999999e-2); +et6 = t2.*2.819808863949e+1+t9.*3.345916886974766e-3+t228+t255+t260+t264+t275+t293+t295+t3.*t283.*3.734539500000001e-1+t3.*t284.*4.431314100000749e-5; +et7 = t10.*t283.*(-4.431314100000749e-5)+t10.*t284.*3.734539500000001e-1+t97.*t284+t100.*t283; +et8 = t228+t255+t260+t264+t272.*1.8811711e-1+t275+t293+t295+t2.*t3.*4.4641168411972+t2.*t10.*1.16814725804922e-5+t3.*t9.*1.16814725804922e-5; +et9 = t9.*t10.*(-4.4641168411972)+t32.*t270-t4.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))).*1.8811711e-1; +et10 = t33.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))); +et11 = t28.*2.642521248900001e-2-t29.*4.005007558499997e-1+t30.*4.005007558499997e-1+t31.*2.642521248900001e-2+t208.*5.404868499999996e-2; +et12 = t225.*1.180157910000001e-2+t249.*1.180157910000001e-2+t264+t293+t295-t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)).*5.404868499999996e-2; +et13 = t163.*(-4.895353899999624e-5)-t207.*2.75159634e-1-t214.*4.895353899999624e-5-t233.*2.75159634e-1+t280+t286-t5.*t72.*1.967932267799849e-4; +et14 = t12.*t72.*(-2.892333216000002e-1)+t223.*(t121-t126); +et15 = t74.*(-5.19454296e-2)+t75.*2.463063281699846e-4+t162.*1.9402124e-1+t181.*5.992854699999624e-5+t185.*5.992854699999624e-5+t222+t245+t266; +et16 = t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))).*(-1.9402124e-1)-t6.*(t29-t30).*2.463063281699846e-4-t13.*(t29-t30).*5.19454296e-2; +et17 = t78.*(-2.195219426400001e-2)+t182.*3.628265800000032e-4+t194-t204+t7.*(t74+t13.*(t29-t30)).*1.216220663202028e-4-t14.*(t74+t13.*(t29-t30)).*2.195219426400001e-2; +et18 = t8.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4)-t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)).*3.628265800000032e-4-t5.*t14.*t72.*1.216220663202028e-4; +mt1 = [et3+et4+et5;et6+et7;et8+et9+et10;et11+et12;et13+et14;et15+et16;et17+et18]; +mt2 = [t186-t8.*t134.*1.071557695499999e-1+t15.*t134.*1.933681823100001e-1+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*1.933681823100001e-1+t15.*(t78+t14.*(t74+t13.*(t29-t30))).*1.071557695499999e-1]; +mt3 = [t15.*t134.*(6.0./5.0)+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(6.0./5.0)]; +tau_mat = [mt1;mt2;mt3]; diff --git a/codegen/base_6AxisFT_regressor_R1000_DVT.m b/codegen/base_6AxisFT_regressor_R1000_DVT.m new file mode 100644 index 0000000..f8b5db5 --- /dev/null +++ b/codegen/base_6AxisFT_regressor_R1000_DVT.m @@ -0,0 +1,21 @@ +function base_regrssor = base_6AxisFT_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_DVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_R1000_DVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +% get base params +robot.baseQR = baseQR; +robot.baseQR.regressor = robot.regressor.U*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters); +base_regrssor = robot.baseQR.regressor; \ No newline at end of file diff --git a/codegen/base_6AxisFT_regressor_R1000_EVT.m b/codegen/base_6AxisFT_regressor_R1000_EVT.m new file mode 100644 index 0000000..ba452b7 --- /dev/null +++ b/codegen/base_6AxisFT_regressor_R1000_EVT.m @@ -0,0 +1,21 @@ +function base_regrssor = base_6AxisFT_regressor_R1000_EVT(theta,dtheta,ddtheta,baseQR) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_R1000_EVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +% get base params +robot.baseQR = baseQR; +robot.baseQR.regressor = robot.regressor.U*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters); +base_regrssor = robot.baseQR.regressor; \ No newline at end of file diff --git a/codegen/base_regressor_R1000_DVT.m b/codegen/base_regressor_R1000_DVT.m new file mode 100644 index 0000000..4f76b51 --- /dev/null +++ b/codegen/base_regressor_R1000_DVT.m @@ -0,0 +1,21 @@ +function base_regrssor = base_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = true; +opt.isSixAxisFTSensor = false; +file=[]; + +robot = get_robot_R1000_DVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_R1000_DVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +% get base params +robot.baseQR = baseQR; +robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters); +base_regrssor = robot.baseQR.regressor; \ No newline at end of file diff --git a/codegen/base_regressor_R1000_EVT.m b/codegen/base_regressor_R1000_EVT.m new file mode 100644 index 0000000..d77a2cc --- /dev/null +++ b/codegen/base_regressor_R1000_EVT.m @@ -0,0 +1,21 @@ +function base_regrssor = base_regressor_R1000_EVT(theta,dtheta,ddtheta,baseQR) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_R1000_EVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +% get base params +robot.baseQR = baseQR; +robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters); +base_regrssor = robot.baseQR.regressor; \ No newline at end of file diff --git a/codegen/regressor_R1000_DVT.m b/codegen/regressor_R1000_DVT.m new file mode 100644 index 0000000..ac6de0a --- /dev/null +++ b/codegen/regressor_R1000_DVT.m @@ -0,0 +1,18 @@ +function regrssor = regressor_R1000_DVT(theta,dtheta,ddtheta) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_EVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +regrssor = robot.regressor.U; \ No newline at end of file diff --git a/codegen/regressor_R1000_EVT.m b/codegen/regressor_R1000_EVT.m new file mode 100644 index 0000000..ac6de0a --- /dev/null +++ b/codegen/regressor_R1000_EVT.m @@ -0,0 +1,18 @@ +function regrssor = regressor_R1000_DVT(theta,dtheta,ddtheta) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_EVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +regrssor = robot.regressor.U; \ No newline at end of file diff --git a/codegen/standard_regressor_R1000_DVT.m b/codegen/standard_regressor_R1000_DVT.m new file mode 100644 index 0000000..4e76399 --- /dev/null +++ b/codegen/standard_regressor_R1000_DVT.m @@ -0,0 +1,18 @@ +function standard_regrssor = standard_regressor_R1000_DVT(theta,dtheta,ddtheta) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_DVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_DVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_R1000_DVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +standard_regrssor = robot.regressor.K; \ No newline at end of file diff --git a/codegen/standard_regressor_R1000_EVT.m b/codegen/standard_regressor_R1000_EVT.m new file mode 100644 index 0000000..e51ca91 --- /dev/null +++ b/codegen/standard_regressor_R1000_EVT.m @@ -0,0 +1,18 @@ +function standard_regrssor = standard_regressor_R1000_EVT(theta,dtheta,ddtheta) +opt.robot_def = 'direct'; +opt.KM_method = 'SCREW'; +opt.Vel_method = 'Direct'; +opt.LD_method = 'Direct'; +opt.debug = false; +opt.robotName = 'R1000_EVT'; +opt.reGenerate = false; +opt.Isreal = true; +opt.isJointTorqueSensor = false; +opt.isSixAxisFTSensor = true; +file=[]; + +robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); +robot = get_Kinematics_R1000_EVT(robot, opt); +robot = get_velocity(robot, opt); +robot = get_regressor(robot, opt); +standard_regrssor = robot.regressor.K; \ No newline at end of file diff --git a/estimate_dyn.m b/estimate_dyn.m index a69d1b9..0c3d9a5 100644 --- a/estimate_dyn.m +++ b/estimate_dyn.m @@ -2,23 +2,42 @@ function robot = estimate_dyn(robot,opt) % ------------------------------------------------------------------- % Get datas % ------------------------------------------------------------------------ -time = 0:0.01:2; +time = 0:0.01:0.5; f=1; q_J = sin(2*pi*f*time); qd_J = (2*pi*f)*cos(2*pi*f*time); qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); -q=[q_J;-q_J]; -qd=[qd_J; -qd_J]; -qdd=[qdd_J; -qdd_J]; +q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]; +qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J]; +qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J]; g = [0; 0; -9.8]; -tau = zeros([2,101]); +toruqeSensorRange = [500;500;230;90;60;60;60;20;20]; % pi -> [m;mc;I] 10 element -robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4]; -robot_pi2=[2;1;0;0;1+1/4;0;0;1+1/4;0;1+1/4]; -robot_pi=[robot_pi1;robot_pi2]; -for i = 1:length(q_J) - regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i)); - tau(:,i)=regressor*robot_pi; +[nLnkPrms, nLnks] = size(robot.pi); +robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); +if opt.isJointTorqueSensor + %hack + tau = zeros([robot.ndof,length(q_J)]); +% tau = zeros([1,length(q_J)]); + for i = 1:length(q_J) + % regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i)); + standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); + regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i)); + tau_Full=regressor*robot_pi; + tau(:,i) = tau_Full; +% joint_idex = robot.ndof-2; +% tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex)); +% tau(:,i) = tau(:,i) + 5*10^-3*toruqeSensorRange.*rand(size(tau(:,i))); + end +elseif opt.isSixAxisFTSensor + tau = zeros([6,length(q_J)]); + for i = 1:length(q_J) + regressor_func = sprintf('regressor_%s',opt.robotName); + regressor = feval(regressor_func,q(:,i),qd(:,i),qdd(:,i)); + FT = regressor*robot_pi; + joint_idex = robot.ndof-2; + tau(:,i) = FT(6*(joint_idex-1)+1:6*(joint_idex)); + end end idntfcnTrjctry.t = time; idntfcnTrjctry.q = q; @@ -30,7 +49,7 @@ idntfcnTrjctry.tau = tau; % ------------------------------------------------------------------------ drvGains = []; baseQR = robot.baseQR; -[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains); +[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains, opt); % --------------------------------------------------------------------- % Estimate parameters @@ -48,7 +67,7 @@ else end robot.sol = sol; % Local unctions - function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains) + function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt) % The function builds observation matrix for UR10E E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters); @@ -59,12 +78,26 @@ robot.sol = sol; % idntfcnTrjctry.q2d(i,:)'); % Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)'); % Ybi = [Yi*E1, Yfrctni]; - base_regressor_func = sprintf('base_regressor_%s',opt.robotName); - Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ... - idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i)); - Wb = vertcat(Wb, Yb); - % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); - Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + if opt.isJointTorqueSensor + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ... + idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); + %hack +% joint_idex = robot.ndof-2; +% Yb = Yb((joint_idex-1)+1:(joint_idex),:); + Wb = vertcat(Wb, Yb); + % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); + Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + elseif opt.isSixAxisFTSensor + base_6AxisFT_regressor_func = sprintf('base_6AxisFT_regressor_%s',opt.robotName); + Yb = feval(base_6AxisFT_regressor_func, idntfcnTrjctry.q(:,i), ... + idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); + joint_idex = robot.ndof-2; + Yb = Yb(6*(joint_idex-1)+1:6*(joint_idex),:); + Wb = vertcat(Wb, Yb); + % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); + Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + end end end diff --git a/estimate_dyn_MLS.m b/estimate_dyn_MLS.m new file mode 100644 index 0000000..a1c6373 --- /dev/null +++ b/estimate_dyn_MLS.m @@ -0,0 +1,114 @@ +function robot = estimate_dyn_MLS(robot,opt) +% ------------------------------------------------------------------- +% Get datas +% ------------------------------------------------------------------------ +get_GCTraj_func = sprintf('get_GCTraj_%s',opt.robotName); +feval(get_GCTraj_func); +% ------------------------------------------------------------------- +% Generate Regressors based on data +% ------------------------------------------------------------------------ +drvGains = []; +baseQR = robot.baseQR; + +for i = 2:1:robot.ndof +% for i = robot.ndof + q = idntfcnTrjctry(i).q; + qd = idntfcnTrjctry(i).qd; + qdd = idntfcnTrjctry(i).qdd; + [nRow,nCol] = size(idntfcnTrjctry(i).qd); + Wb = []; Tau = []; + for j = 1:nRow/robot.ndof + for k = 1:nCol + if opt.isJointTorqueSensor + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),... + qd(robot.ndof*(j-1)+1:robot.ndof*j,k),... + qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR); + Yb = Yb(i,:); + Wb = vertcat(Wb, Yb); + Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k)); + end + end + end + observationMatrix(i).Wb = Wb; + observationMatrix(i).Tau = Tau; + observationMatrix(i).rank = robot.baseQR.rank(i); +end + +% --------------------------------------------------------------------- +% Estimate parameters +% --------------------------------------------------------------------- +sol = struct; +for i = robot.ndof:-1:2 +% for i = robot.ndof + Wb = observationMatrix(i).Wb; + Tau = observationMatrix(i).Tau; + if i == robot.ndof + pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:end))*Tau; + pib_MLS = []; + elseif i > 1 + pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:... + baseQR.numberOfBaseParameters-observationMatrix(i+1).rank))*... + (-Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau); + else + break; + end + pifrctn_OLS = 0; + pib_MLS = [pib_OLS;pib_MLS]; +end +sol.pib_MLS = pib_MLS; +robot.sol = sol; +% method = 'OLS'; +% if strcmp(method, 'OLS') +% % Usual least squares +% [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(observationMatrix); +% elseif strcmp(method, 'PC-OLS') +% % Physically consistent OLS using SDP optimization +% [sol.pi_b, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR); +% else +% error("Chosen method for dynamic parameter estimation does not exist"); +% end +% robot.sol = sol; +% % Local unctions +% function observationMatrix = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt) +% for i = 1:1:robot.ndof +% [nRow,nCol] = size(idntfcnTrjctry(i).qd); +% Wb = []; Tau = []; +% for j = 1:nRow/robot.ndof +% for k = 1:nCol +% if opt.isJointTorqueSensor +% base_regressor_func = sprintf('base_regressor_%s',opt.robotName); +% Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),... +% qd(robot.ndof*(j-1)+1:robot.ndof*j,k),... +% qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR); +% Yb = Yb(i,:); +% Wb = vertcat(Wb, Yb); +% Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k)); +% end +% end +% end +% observationMatrix(i).Wb = Wb; +% observationMatrix(i).Tau = Tau; +% end +% end +% +% +% function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(observationMatrix) +% % Function perfroms ordinary least squares estimation of parameters +% % pi_OLS = (Wb'*Wb)\(Wb'*Tau); +% % pib_OLS = pi_OLS(1:40); % variables for base paramters +% % pifrctn_OLS = pi_OLS(41:end); +% for i = 9:-1:1 +% Wb = observationMatrix(i).Wb; +% Tau = observationMatrix(i).Tau; +% pib_OLS(i)=pinv(Wb)*Tau; +% pifrctn_OLS = 0; +% end +% end +% function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb) +% % Function perfroms Multi step ordinary least squares estimation of parameters +% +% pib_OLS=pinv(Wb)*Tau; +% pifrctn_OLS = 0; +% end +end \ No newline at end of file diff --git a/estimate_dyn_form_data.m b/estimate_dyn_form_data.m new file mode 100644 index 0000000..ac004d0 --- /dev/null +++ b/estimate_dyn_form_data.m @@ -0,0 +1,111 @@ +function robot = estimate_dyn_form_data(robot,opt) +% ------------------------------------------------------------------- +% Get datas +% ------------------------------------------------------------------------ +posData = robot.posData; +currentData = robot.currentData; +[nRow,nCol] = size(posData); +index_p = find(posData(:,nCol-1)==7); +jointPos_p = posData(index_p,1:9); +jointPos_p = jointPos_p(1:100:end,:); +index_n = find(posData(:,nCol-1)==-7); +jointPos_n = posData(index_n,1:9); +jointPos_n = jointPos_n(1:100:end,:); +q = [jointPos_p]'; +qd = q; qdd = q; +motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185]; +gearRatio = [101,101,100,100,100,100,100,100,100]; +% pi -> [m;mc;I] 10 element +index_p = find(currentData(:,nCol-1)==7); +jointCur_p = currentData(index_p,1:9); +jointCur_p = jointCur_p(1:100:end,:); +index_n = find(currentData(:,nCol-1)==-7); +jointCur_n = currentData(index_n,1:9); +jointCur_n = jointCur_n(1:100:end,:); +current = (jointCur_p+jointCur_n)/2; +torque_cur = gearRatio.*motorConst.*current; +tau = torque_cur(:,7)'; + +idntfcnTrjctry.t = 1:length(q); +idntfcnTrjctry.q = q; +idntfcnTrjctry.qd = qd; +idntfcnTrjctry.qdd = qdd; +idntfcnTrjctry.tau = tau; +% ------------------------------------------------------------------- +% Generate Regressors based on data +% ------------------------------------------------------------------------ +drvGains = []; +baseQR = robot.baseQR; +[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains, opt); + +% --------------------------------------------------------------------- +% Estimate parameters +% --------------------------------------------------------------------- +sol = struct; +method = 'PC-OLS'; +if strcmp(method, 'OLS') + % Usual least squares + [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(Tau, Wb); +elseif strcmp(method, 'PC-OLS') + % Physically consistent OLS using SDP optimization + [sol.pi_b] = physicallyConsistentEstimation(Tau, Wb); +else + error("Chosen method for dynamic parameter estimation does not exist"); +end +robot.sol = sol; +% Local unctions + function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt) + % The function builds observation matrix for UR10E + E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters); + + Wb = []; Tau = []; + for i = 1:1:length(idntfcnTrjctry.t) + % Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',... + % idntfcnTrjctry.qd(i,:)',... + % idntfcnTrjctry.q2d(i,:)'); + % Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)'); + % Ybi = [Yi*E1, Yfrctni]; + if opt.isJointTorqueSensor + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ... + idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); + %hack + joint_idex = robot.ndof-2; + Yb = Yb((joint_idex-1)+1:(joint_idex),:); + Wb = vertcat(Wb, Yb); + % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); + Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + elseif opt.isSixAxisFTSensor + base_6AxisFT_regressor_func = sprintf('base_6AxisFT_regressor_%s',opt.robotName); + Yb = feval(base_6AxisFT_regressor_func, idntfcnTrjctry.q(:,i), ... + idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); + joint_idex = robot.ndof-2; + Yb = Yb(6*(joint_idex-1)+1:6*(joint_idex),:); + Wb = vertcat(Wb, Yb); + % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); + Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + end + end + end + + + function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb) + % Function perfroms ordinary least squares estimation of parameters +% pi_OLS = (Wb'*Wb)\(Wb'*Tau); +% pib_OLS = pi_OLS(1:40); % variables for base paramters +% pifrctn_OLS = pi_OLS(41:end); + pib_OLS=pinv(Wb)*Tau; + pifrctn_OLS = 0; + end + function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb) + % Function perfroms Multi step ordinary least squares estimation of parameters + + pib_OLS=pinv(Wb)*Tau; + pifrctn_OLS = 0; + end + function pib = physicallyConsistentEstimation(Tau, Wb) + % 求解线性规划问题 + options = optimoptions('lsqlin','Algorithm','trust-region-reflective','Display','iter'); + pib = lsqlin(Wb,Tau,[],[],[],[],-2*ones(5,1),2*ones(5,1),[0.9151,0.0092,0.0002,-0.0644,0.1067]',options) + end +end \ No newline at end of file diff --git a/estimate_dyn_form_data_MLS.m b/estimate_dyn_form_data_MLS.m new file mode 100644 index 0000000..a8f64e9 --- /dev/null +++ b/estimate_dyn_form_data_MLS.m @@ -0,0 +1,117 @@ +function robot = estimate_dyn_form_data(robot,opt) +% ------------------------------------------------------------------- +% Get datas +% ------------------------------------------------------------------------ + +% ------------------------------------------------------------------- +% Generate Regressors based on data +% ------------------------------------------------------------------------ +drvGains = []; +baseQR = robot.baseQR; +pi_b = buildObservationMatrices_MLS(baseQR, drvGains, opt); + +% --------------------------------------------------------------------- +% Estimate parameters +% --------------------------------------------------------------------- +sol = struct; +% method = 'OLS'; +% if strcmp(method, 'OLS') +% % Usual least squares +% [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(Tau, Wb); +% elseif strcmp(method, 'PC-OLS') +% % Physically consistent OLS using SDP optimization +% [sol.pi_b] = physicallyConsistentEstimation(Tau, Wb); +% else +% error("Chosen method for dynamic parameter estimation does not exist"); +% end +sol.pi_b = pi_b; +robot.sol = sol; +% Local unctions + function pi_b = buildObservationMatrices_MLS(baseQR, drvGains,opt) + % The function builds observation matrix for UR10E +% E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters); + motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185]; + gearRatio = [101,101,100,100,100,100,100,100,100]; + posData = robot.posData; + currentData = robot.currentData; + [nRow,nCol] = size(posData); pi_b =[]; + %get data for MLS + for j = 9:-1:7 + index_p = find(posData(:,nCol-1)==j); + jointPos_p = posData(index_p,1:9); + jointPos_p = jointPos_p(1:500:end,:); + index_n = find(posData(:,nCol-1)==-j); + jointPos_n = posData(index_n,1:9); + jointPos_n = jointPos_n(1:500:end,:); + q = [jointPos_p]'; + qd = q; qdd = q; + + % pi -> [m;mc;I] 10 element + index_p = find(currentData(:,nCol-1)==j); + jointCur_p = currentData(index_p,1:9); + jointCur_p = jointCur_p(1:500:end,:); + index_n = find(currentData(:,nCol-1)==-j); + jointCur_n = currentData(index_n,1:9); + jointCur_n = jointCur_n(1:500:end,:); + current = (jointCur_p+jointCur_n)/2; + torque_cur = gearRatio.*motorConst.*current; + tau = torque_cur(:,j)'; + + idntfcnTrjctry.t = 1:length(q); + idntfcnTrjctry.q = q; + idntfcnTrjctry.qd = qd; + idntfcnTrjctry.qdd = qdd; + idntfcnTrjctry.tau = tau; + Wb = []; Tau = []; + for i = 1:1:length(idntfcnTrjctry.t) + % Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',... + % idntfcnTrjctry.qd(i,:)',... + % idntfcnTrjctry.q2d(i,:)'); + % Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)'); + % Ybi = [Yi*E1, Yfrctni]; + if opt.isJointTorqueSensor + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ... + idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); + %hack + joint_idex = j; + Yb = Yb(j,:); + Wb = vertcat(Wb, Yb); + Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + elseif opt.isSixAxisFTSensor + base_6AxisFT_regressor_func = sprintf('base_6AxisFT_regressor_%s',opt.robotName); + Yb = feval(base_6AxisFT_regressor_func, idntfcnTrjctry.q(:,i), ... + idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); + joint_idex = robot.ndof-2; + Yb = Yb(6*(joint_idex-1)+1:6*(joint_idex),:); + Wb = vertcat(Wb, Yb); + % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); + Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); + end + end + pi_b_joint = ordinaryLeastSquareEstimation(Tau, Wb); + pi_b = vertcat(pi_b,pi_b_joint); + end + end + + + function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb) + % Function perfroms ordinary least squares estimation of parameters +% pi_OLS = (Wb'*Wb)\(Wb'*Tau); +% pib_OLS = pi_OLS(1:40); % variables for base paramters +% pifrctn_OLS = pi_OLS(41:end); + pib_OLS=pinv(Wb)*Tau; + pifrctn_OLS = 0; + end + function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb) + % Function perfroms Multi step ordinary least squares estimation of parameters + + pib_OLS=pinv(Wb)*Tau; + pifrctn_OLS = 0; + end + function pib = physicallyConsistentEstimation(Tau, Wb) + % 求解线性规划问题 + options = optimoptions('lsqlin','Algorithm','trust-region-reflective','Display','iter'); + pib = lsqlin(Wb,Tau,[],[],[],[],-2*ones(5,1),2*ones(5,1),[0.9151,0.0092,0.0002,-0.0644,0.1067]',options) + end +end \ No newline at end of file diff --git a/getGravityForce.m b/getGravityForce.m new file mode 100644 index 0000000..7ceb6db --- /dev/null +++ b/getGravityForce.m @@ -0,0 +1,41 @@ +%% R1000 +N=9; +time = 0:0.01:1; +f=1; +q_J = sin(2*pi*f*time); +qd_J = (2*pi*f)*cos(2*pi*f*time); +qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +zero_ = zeros(1,length(q_J)); + + +% thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +thetalist = zeros(N,1); + +Mlist_CG = robot.kine.Mlist_CG; +Slist=robot.slist; + +% Dynamics parameters +link_mass = robot.m; +com_pos = robot.com; +link_inertia = robot.I; +% Get general mass matrix +Glist=[]; +for i = 1:N + Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])]; + Glist = cat(3, Glist, Gb); +end + +gravity = [0;0;-9.806]; +gravityForcelist = zeros(6,N); +for i = 1:N +gravityForcelist(:,i) = Glist(:,:,i)*[zeros(3,1);gravity]; +end + +% JacobianMatrix = zeros(6*N,6*N); +% for i = 1:N +% for j = 1:N +% JacobianMatrix(1+6*(i-1):6*i,1+6*(j-1):6*j) = JacobianSpace(Slist(:,i:j),thetalist(i:j)); +% end +% end +JacobianMatrix = JacobianSpace(Slist,thetalist); +gravityTorques = transpose(JacobianMatrix)*gravityForcelist; diff --git a/get_GCTraj_R1000_DVT.m b/get_GCTraj_R1000_DVT.m new file mode 100644 index 0000000..49106c3 --- /dev/null +++ b/get_GCTraj_R1000_DVT.m @@ -0,0 +1,100 @@ +%temp get DVT traj +gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))]; +motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031]; +sensorDir = [-1,1,-1,-1,-1,1,-1,1,1]; +load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\lab12.mat"); +posDir = [1,1,1,1,1,1,1,-1,1]; +isCurrentSensor = true; +% J9 traj +i = 9; +fileData=eval(strcat('fileData', num2str(i))); +data = fileData.data; +dataLength = length(data); +d1Length = floor(length(data) / 2); +% idntfcnTrjctry(i).tau = (data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i)+... +% data(d1Length+1:end,i+1+11*2)*gearRatio(i)*motorConstant(i))/2; +idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i); +idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau'; + +q=zeros(robot.ndof,dataLength);qd=q;qdd=qd; +for j=1:robot.ndof + if(j==i||j==i-1) + q(j,:)=data(:,j+1+11); + continue; + end + q(j,:) = mean(data(:,j+1+11),1); +end +idntfcnTrjctry(i).q = q; +idntfcnTrjctry(i).qd = qd; +idntfcnTrjctry(i).qdd = qdd; + +% J8 to J2 traj +for i=8:-1:2 + fileData=eval(strcat('fileData', num2str(i))); + data = fileData.data; + dataLength = length(data); + d1Length = floor(length(data) / 2); + if(isCurrentSensor) + idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i); + else + idntfcnTrjctry(i).tau = data(:,6*(i+1)+11*3)*sensorDir(i); + end + idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau'; + q=zeros(robot.ndof,dataLength);qd=q;qdd=qd; + for j=1:robot.ndof + if((j==i||j==i-1)&&i~=2) %hack i=2 + q(j,:)=data(:,j+1+11); + continue; + end + if((i==2&&j==6)||(i==2&&j==2)) + q(j,:)=data(:,j+1+11); + continue; + end + q(j,:) = mean(data(:,j+1+11),1); + end + idntfcnTrjctry(i).q = q; + idntfcnTrjctry(i).qd = qd; + idntfcnTrjctry(i).qdd = qdd; +end + +% Reduce data size +for i=robot.ndof:-1:2 + if i == 9 || i == 8 || i==2 + dataLength = length(idntfcnTrjctry(i).tau); + idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau(mod(1:dataLength, 400) == 0); + idntfcnTrjctry(i).q = idntfcnTrjctry(i).q(:,mod(1:dataLength, 400) == 0); + idntfcnTrjctry(i).qd = idntfcnTrjctry(i).qd(:,mod(1:dataLength, 400) == 0); + idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0); + continue; + end + dataLength = length(idntfcnTrjctry(i).tau); + idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau(mod(1:dataLength, 400) == 0); + idntfcnTrjctry(i).q = idntfcnTrjctry(i).q(:,mod(1:dataLength, 400) == 0); + idntfcnTrjctry(i).qd = idntfcnTrjctry(i).qd(:,mod(1:dataLength, 400) == 0); + idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0); +end + +%hack +% for i=robot.ndof:-1:2 +% idntfcnTrjctry(i).q(8,:) = - idntfcnTrjctry(i).q(8,:); +% end + +% pi -> [m;mc;I] 10 element +% [nLnkPrms, nLnks] = size(robot.pi); +% robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); +% for i=2:robot.ndof +% q = idntfcnTrjctry(i).q; +% qd = idntfcnTrjctry(i).qd; +% qdd = idntfcnTrjctry(i).qdd; +% [nRow,nCol] = size(idntfcnTrjctry(i).qd); +% for j = 1:nRow/robot.ndof +% for k = 1:nCol +% standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); +% regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),... +% qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k)); +% tau_Full=regressor*robot_pi; +% tau(j,k) = tau_Full(i); +% end +% end +% Trjctry(i).tau = tau; +% end \ No newline at end of file diff --git a/get_GCTraj_R1000_EVT.m b/get_GCTraj_R1000_EVT.m new file mode 100644 index 0000000..07cd401 --- /dev/null +++ b/get_GCTraj_R1000_EVT.m @@ -0,0 +1,219 @@ +% function robot = get_GCTraj_R1000_EVT(robot,opt) +% J9 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(9,:)=q_J; +q(8,:)=pi/3*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(9).t = time; +idntfcnTrjctry(9).q = q; +idntfcnTrjctry(9).qd = qd; +idntfcnTrjctry(9).qdd = qdd; + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(9,:)=q_J; +q(8,:)=-pi/3*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time); +idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q); +idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd); +idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd); + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(9,:)=q_J; +q(6,:)=pi/2*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time); +idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q); +idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd); +idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd); + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(9,:)=q_J; +q(6,:)=-pi/2*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time); +idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q); +idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd); +idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd); + +% J8 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(8,:)=pi/3*q_J; +q(6,:)=pi/2*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(8).t = time; +idntfcnTrjctry(8).q = q; +idntfcnTrjctry(8).qd = qd; +idntfcnTrjctry(8).qdd = qdd; + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(8,:)=pi/3*q_J; +q(6,:)=-pi/2*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(8).t = vertcat(idntfcnTrjctry(8).t,time); +idntfcnTrjctry(8).q = vertcat(idntfcnTrjctry(8).q,q); +idntfcnTrjctry(8).qd = vertcat(idntfcnTrjctry(8).qd,qd); +idntfcnTrjctry(8).qdd = vertcat(idntfcnTrjctry(8).qdd,qdd); + +% J7 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +% q(7,:)=q_J; +% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J; +% q(4,:)=q_J;q(9,:)=q_J; +qd=zeros(robot.ndof,length(q_J));qdd=qd; +q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]; +idntfcnTrjctry(7).t = time; +idntfcnTrjctry(7).q = q; +idntfcnTrjctry(7).qd = qd; +idntfcnTrjctry(7).qdd = qdd; + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +% q(7,:)=q_J; +% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J; +% q(4,:)=q_J;q(9,:)=q_J; +qd=zeros(robot.ndof,length(q_J));qdd=qd; +q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]; +idntfcnTrjctry(7).t = vertcat(idntfcnTrjctry(7).t,time); +idntfcnTrjctry(7).q = vertcat(idntfcnTrjctry(7).q,q); +idntfcnTrjctry(7).qd = vertcat(idntfcnTrjctry(7).qd,qd); +idntfcnTrjctry(7).qdd = vertcat(idntfcnTrjctry(7).qdd,qdd); + +% J6 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(6,:)=pi/2*q_J; +q(4,:)=pi/4*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(6).t= time; +idntfcnTrjctry(6).q= q; +idntfcnTrjctry(6).qd= qd; +idntfcnTrjctry(6).qdd= qdd; + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(6,:)=pi/2*q_J; +q(4,:)=-pi/4*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(6).t= vertcat(idntfcnTrjctry(6).t,time); +idntfcnTrjctry(6).q= vertcat(idntfcnTrjctry(6).q,q); +idntfcnTrjctry(6).qd= vertcat(idntfcnTrjctry(6).qd,qd); +idntfcnTrjctry(6).qdd= vertcat(idntfcnTrjctry(6).qdd,qdd); + +% J5 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(5,:)=pi/2*q_J; +q(4,:)=pi/4*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(5).t = time; +idntfcnTrjctry(5).q = q; +idntfcnTrjctry(5).qd = qd; +idntfcnTrjctry(5).qdd = qdd; + +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(5,:)=pi/2*q_J; +q(4,:)=-pi/4*ones(size(q_J)); +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(5).t = vertcat(idntfcnTrjctry(5).t,time); +idntfcnTrjctry(5).q = vertcat(idntfcnTrjctry(5).q,q); +idntfcnTrjctry(5).qd = vertcat(idntfcnTrjctry(5).qd,qd); +idntfcnTrjctry(5).qdd = vertcat(idntfcnTrjctry(5).qdd,qdd); + +% J4 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(4,:)=pi/4*q_J; +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(4).t = time; +idntfcnTrjctry(4).q = q; +idntfcnTrjctry(4).qd = qd; +idntfcnTrjctry(4).qdd = qdd; + +% J3 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(3,:)=pi/4*q_J; +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(3).t = time; +idntfcnTrjctry(3).q = q; +idntfcnTrjctry(3).qd = qd; +idntfcnTrjctry(3).qdd = qdd; + +% J2 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(2,:)=pi/4*q_J; +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(2).t = time; +idntfcnTrjctry(2).q = q; +idntfcnTrjctry(2).qd = qd; +idntfcnTrjctry(2).qdd = qdd; + +% J1 traj +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +q=zeros(robot.ndof,length(q_J)); +q(1,:)=pi/4*q_J; +qd=zeros(robot.ndof,length(q_J));qdd=qd; +idntfcnTrjctry(1).t = time; +idntfcnTrjctry(1).q = q; +idntfcnTrjctry(1).qd = qd; +idntfcnTrjctry(1).qdd = qdd; + +% pi -> [m;mc;I] 10 element +[nLnkPrms, nLnks] = size(robot.pi); +robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); +for i=1:robot.ndof + q = idntfcnTrjctry(i).q; + qd = idntfcnTrjctry(i).qd; + qdd = idntfcnTrjctry(i).qdd; + [nRow,nCol] = size(idntfcnTrjctry(i).qd); + for j = 1:nRow/robot.ndof + for k = 1:nCol + standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); + regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),... + qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k)); + tau_Full=regressor*robot_pi; + tau(j,k) = tau_Full(i); + end + end + idntfcnTrjctry(i).tau = tau; +end \ No newline at end of file diff --git a/get_Kinematics.m b/get_Kinematics.m deleted file mode 100644 index c9e377d..0000000 --- a/get_Kinematics.m +++ /dev/null @@ -1,81 +0,0 @@ -function robot = get_Kinematics(robot, opt) -if(opt.Isreal) - switch opt.KM_method - case 'SDH' - 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))*cos(alpha(i)) sin(theta(i))*sin(alpha(i));... - sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i));... - 0 sin(alpha(i)) cos(alpha(i))]; - robot.t(:,:,i) = [a(i)*cos(theta(i));a(i)*sin(theta(i));d(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 - 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_Kinematics_R1000_DVT.m b/get_Kinematics_R1000_DVT.m new file mode 100644 index 0000000..aeb369b --- /dev/null +++ b/get_Kinematics_R1000_DVT.m @@ -0,0 +1,219 @@ +function robot = get_Kinematics_R1000_DVT(robot, opt) +if(opt.Isreal) + switch opt.KM_method + case 'SCREW' + % Get transformation for each joint + thetalist = robot.theta; + Slist = robot.slist; + % TODO:move to lib + for j = 1:length(thetalist) + T=robot.Home.M(:,:,j); + for i = j: -1: 1 + T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T; + end + robot.TW(:,:,j) = T; + end + for i = 1:length(thetalist) + if i == 1 + robot.T(:,:,i) = robot.TW(:,:,i); + else + robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i); + end + end + robot.Fkine = robot.TW(:,:,end); + + %stack result into kine structure + robot.kine.TW = robot.TW; + robot.kine.T = robot.T; + robot.kine.R = robot.kine.T(1:3,1:3,:); + robot.kine.t = robot.kine.T(1:3,4,:); + robot.kine.Fkine = robot.Fkine; + % get the CG at the world base frame + % FIXME: Fix this hack + ct=[]; + Mlist_CG_Base=[]; + com_pos_R1 = robot.com_pos_R1; + com_pos_R2 = robot.com_pos_R2; + for i = 1:length(thetalist) + if i == 1 + ct(:,i) = com_pos_R1(:,i); + elseif i< 9 + ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i); + else + % HACK + ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i); + end + robot.Home.com(:,i) = ct(:,i); + M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i)); + Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base); + end + robot.kine.Mlist_CG_Base = Mlist_CG_Base; + % get the CG at the last GC frame + Mlist_CG=[]; + for i = 1:length(thetalist) + if i == 1 + Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i); + else + Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i); + end + end + M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; + Mlist_CG = cat(3, Mlist_CG, M); + robot.kine.Mlist_CG = Mlist_CG; + + % Get the end efforce transformation in each joint reference frame + Mlist_ED = []; + for i = 1:length(thetalist) + M = robot.T(:,:,i); + Mlist_ED = cat(3, Mlist_ED, M); + end + M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; + Mlist_ED = cat(3, Mlist_ED, M); + robot.kine.Mlist_ED = Mlist_ED; + case 'SDH' + 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))*cos(alpha(i)) sin(theta(i))*sin(alpha(i));... + sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i));... + 0 sin(alpha(i)) cos(alpha(i))]; + robot.t(:,:,i) = [a(i)*cos(theta(i));a(i)*sin(theta(i));d(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 + 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 'SCREW' + % Get transformation for each joint + thetalist = robot.theta; + Slist = robot.slist; + % TODO:move to lib + for j = 1:length(thetalist) + T=robot.Home.M(:,:,j); + for i = j: -1: 1 + T = MatrixExp6_Sym(VecTose3(Slist(:, i) * thetalist(i))) * T; + end + robot.TW(:,:,j) = T; + end + for i = 1:length(thetalist) + if i == 1 + robot.T(:,:,i) = robot.TW(:,:,i); + else + robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i); + end + robot.kine.R(:,:,i) = robot.T(1:3,1:3,i); + robot.kine.t(:,:,i) = robot.T(1:3,4,i); + end + robot.Fkine = robot.TW(:,:,end); + + %stack result into kine structure + robot.kine.TW = robot.TW; + robot.kine.T = robot.T; + robot.kine.Fkine = robot.Fkine; + % get the CG at the world base frame + % FIXME: Fix this hack + ct=[]; + Mlist_CG_Base=[]; + com_pos_R1 = robot.com_pos_R1; + com_pos_R2 = robot.com_pos_R2; + for i = 1:length(thetalist) + if i == 1 + ct(:,i) = com_pos_R1(:,i); + elseif i< 9 + ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i); + else + % HACK + ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i); + end + robot.Home.com(:,i) = ct(:,i); + M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i)); + Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base); + end + % get the CG at the last GC frame + Mlist_CG=[]; + for i = 1:length(thetalist) + if i == 1 + Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i); + else + Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i); + end + end + M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; + Mlist_CG = cat(3, Mlist_CG, M); + % Get the end efforce transformation in each joint reference frame + Mlist_ED = []; + for i = 1:length(thetalist) + M = robot.T(:,:,i); + Mlist_ED = cat(3, Mlist_ED, M); + end + M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; + Mlist_ED = cat(3, Mlist_ED, M); + % stack result + robot.kine.Mlist_CG_Base = Mlist_CG_Base; + robot.kine.Mlist_CG = Mlist_CG; + robot.kine.Mlist_ED = Mlist_ED; + 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_Kinematics_R1000_EVT.m b/get_Kinematics_R1000_EVT.m new file mode 100644 index 0000000..d7e37a2 --- /dev/null +++ b/get_Kinematics_R1000_EVT.m @@ -0,0 +1,73 @@ +function robot = get_Kinematics_R1000_EVT(robot, opt) +if(opt.Isreal) + switch opt.KM_method + case 'SCREW' + % Get transformation for each joint + thetalist = robot.theta; + Slist = robot.slist; + % TODO:move to lib + for j = 1:length(thetalist) + T=robot.Home.M(:,:,j); + for i = j: -1: 1 + T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T; + end + robot.kine.TW(:,:,j) = T; + end + for i = 1:length(thetalist) + if i == 1 + robot.kine.T(:,:,i) = robot.kine.TW(:,:,i); + else + robot.kine.T(:,:,i) = TransInv(robot.kine.TW(:,:,i-1))*robot.kine.TW(:,:,i); + end + end + %stack result into kine structure + robot.kine.R = robot.kine.T(1:3,1:3,:); + robot.kine.t = robot.kine.T(1:3,4,:); + robot.kine.Fkine = robot.kine.TW(:,:,end); + % get the CG at the world base frame + % FIXME: Fix this hack + ct=zeros(3,robot.ndof); + Mlist_CG_Base=zeros(4,4,10); + com_pos_R1 = robot.com_pos_R1; + com_pos_R2 = robot.com_pos_R2; + for i = 1:length(thetalist) + if i == 1 + ct(:,i) = com_pos_R1(:,i); +% elseif i< 9 + else + ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i); +% else +% % HACK +% ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i); + end + robot.Home.com(:,i) = ct(:,i); + M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i)); + Mlist_CG_Base(:,:,i) = M_CG_Base; + end + robot.kine.Mlist_CG_Base = Mlist_CG_Base; + % get the CG at the last GC frame + Mlist_CG = zeros(4,4,9); + for i = 1:length(thetalist) + if i == 1 + Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i); + else + Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i); + end + end + M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; + Mlist_CG = cat(3, Mlist_CG, M); + robot.kine.Mlist_CG = Mlist_CG; + + % Get the end efforce transformation in each joint reference frame + Mlist_ED = zeros(4,4,9); + for i = 1:length(thetalist) + Mlist_ED(:,:,i) = robot.kine.T(:,:,i); + end + M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]]; + Mlist_ED = cat(3, Mlist_ED, M); + robot.kine.Mlist_ED = Mlist_ED; + otherwise + disp('Bad opt.KM_method!') + return; + end +end \ No newline at end of file diff --git a/get_baseParams.m b/get_baseParams.m index 6ee47d4..690281b 100644 --- a/get_baseParams.m +++ b/get_baseParams.m @@ -1,6 +1,6 @@ function robot = get_baseParams(robot,opt) % Seed the random number generator based on the current time -rng('shuffle'); +rng("default"); ndof = robot.ndof; includeMotorDynamics = false; % ------------------------------------------------------------------------ @@ -14,7 +14,7 @@ q2d_max = 6*pi*ones(ndof,1); % Find relation between independent columns and dependent columns % ----------------------------------------------------------------------- % Get observation matrix of identifiable paramters -W = []; +W = []; for i = 1:25 q_rnd = q_min + (q_max - q_min).*rand(ndof,1); qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1); @@ -22,9 +22,22 @@ for i = 1:25 if includeMotorDynamics % Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd); - else + elseif opt.isJointTorqueSensor standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd); +% joint_idex = ndof-2; +% Y = Y(joint_idex:end,:); + elseif opt.isSixAxisFTSensor + regressor_func = sprintf('regressor_%s',opt.robotName); + Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd); + joint_idex = ndof-2; + Y = Y(6*(joint_idex-1)+1:6*(joint_idex),:); + % FIXME hack here +% standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); +% Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd); +% Zeros_ = zeros(size(Y)); +% Zeros_(ndof-2,:) = Y(ndof-2,:); +% Y = Zeros_; end W = vertcat(W,Y); end @@ -33,39 +46,70 @@ end % R is upper triangular matrix % Q is unitary matrix % E is permutation matrix -[Q, R, E] = qr(W); +[~, R, E] = qr(W); % matrix W has rank qr_rank which is number number of base parameters qr_rank = rank(W); % R = [R1 R2; % 0 0] -% R1 is bbxbb upper triangular and reguar matrix +% R1 is bbxbb upper triangular and regu ar matrix % R2 is bbx(c-qr_rank) matrix where c is number of standard parameters R1 = R(1:qr_rank,1:qr_rank); R2 = R(1:qr_rank,qr_rank+1:end); beta = R1\R2; % the zero rows of K correspond to independent columns of WP -beta(abs(beta) + diff --git a/r1000dynamics/code_generation/design_data.sldd b/r1000dynamics/code_generation/design_data.sldd new file mode 100644 index 0000000..8a57d52 Binary files /dev/null and b/r1000dynamics/code_generation/design_data.sldd differ diff --git a/r1000dynamics/code_generation/r1000Dynamics.slx b/r1000dynamics/code_generation/r1000Dynamics.slx new file mode 100644 index 0000000..8d8ce91 Binary files /dev/null and b/r1000dynamics/code_generation/r1000Dynamics.slx differ diff --git a/r1000dynamics/code_generation/r1000DynamicsSub.slx b/r1000dynamics/code_generation/r1000DynamicsSub.slx new file mode 100644 index 0000000..535aea7 Binary files /dev/null and b/r1000dynamics/code_generation/r1000DynamicsSub.slx differ diff --git a/r1000dynamics/r1000_dynamic_analysis/calculateArmRot.m b/r1000dynamics/r1000_dynamic_analysis/calculateArmRot.m new file mode 100644 index 0000000..d0ba22b --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/calculateArmRot.m @@ -0,0 +1,29 @@ +function [rotInfo] = calculateArmRot(dynamicBasicInfo,theta) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 计算机器人各个轴臂的姿态，四元素/旋转矩阵... +% dynamicBasicInfo: PSA机械臂的基本信息 +% setupInfo: SUA(setup arm)状态，各个轴的角度等 [] +% state:系统动力学的状态变量 +% 假设：关节变形和柔性体变形都是小量，忽略其对姿态的影响 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +rotInfo = struct; +temp = struct; +temp.rot = eye(3); +temp.rotTran = eye(3); +rotInfo.link = ([temp temp temp temp temp temp temp temp temp]); +rotInfo.motor = ([temp temp temp temp temp temp temp temp temp]); +rotInfo.link(1).rot = rot(dynamicBasicInfo.link(1).jointAxis,theta(1)); +rotInfo.link(1).rotTran = rotInfo.link(1).rot'; + +rotInfo.motor(1).rot = rotInfo.link(1).rot; +rotInfo.motor(1).rotTran = rotInfo.motor(1).rot'; +for i=2:9 + if dynamicBasicInfo.link(i).jointAxis>3 + rotInfo.link(i).rot = rotInfo.link(i-1).rot*rot(dynamicBasicInfo.link(i).jointAxis-3,theta(i)); + else + rotInfo.link(i).rot = rotInfo.link(i-1).rot; + end + rotInfo.link(i).rotTran = rotInfo.link(i).rot'; + rotInfo.motor(i).rot = rotInfo.link(i).rot; + rotInfo.motor(i).rotTran = rotInfo.motor(i).rot'; +end \ No newline at end of file diff --git a/r1000dynamics/r1000_dynamic_analysis/calculateJacobian.asv b/r1000dynamics/r1000_dynamic_analysis/calculateJacobian.asv new file mode 100644 index 0000000..509b9be --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/calculateJacobian.asv @@ -0,0 +1,49 @@ +function [jacoInfo] = calculateJacobian(robotRigidInfo,... + rotInfo,... + theta) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 计算机器人各个轴臂的雅可比矩阵， +% dynamicBasicInfo: 机械臂的基本信息 +% rotInfo: 各个轴臂的姿态信息 +% theta: 转角 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +temp = struct; +temp.rot = eye(3); +temp.rotTran = eye(3); +rotInfo.link = ([temp temp temp temp temp temp temp temp temp]); +rotInfo.motor = ([temp temp temp temp temp temp temp temp temp]); +for i=1:9 + jacoInfo.link(i).jaco = zeros(6,9); + jacoInfo.motor(i).jaco = zeros(6,robotRigidInfo.dof); + comToRotCenter = -rotInfo.link(i).rot*robotRigidInfo.link(i).R1; + for j=i:-1:1 + if robotRigidInfo.link(j).jointAxis>3 + rotAxis=zeros(3,1); + rotAxis(robotRigidInfo.link(j).jointAxis-3) = 1; + % movement + jacoInfo.link(i).jaco(1:3,j) = -vector2so3(comToRotCenter)*rotInfo.link(j).rot*rotAxis; + if j>1 + comToRotCenter = comToRotCenter + ... + rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ... + - rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1; + end + % rotation + jacoInfo.link(i).jaco(4:6,j) = rotInfo.link(i).rotTran*rotInfo.link(j).rot*rotAxis; + else + tranAxis=zeros(3,1); + tranAxis(robotRigidInfo.link(j).jointAxis) = 1; + % movement + jacoInfo.link(i).jaco(1:3,j) = rotInfo.link(j).rot*tranAxis; + if j>1 + comToRotCenter = comToRotCenter + ... + rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ... + - rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1 + ... + rotInfo.link(j).rot*tranAxis*theta(j); + end + end + motorRotAxis = zeros(3,1); + motorRotAxis(robotRigidInfo.motor(i).jointAxis-3) = 1; + jacoInfo.motor(i).jaco = jacoInfo.link(i).jaco; + jacoInfo.motor(i).jaco(4:6,i) = robotRigidInfo.motor(i).gearRatio*motorRotAxis; + end +end \ No newline at end of file diff --git a/r1000dynamics/r1000_dynamic_analysis/calculateJacobian.m b/r1000dynamics/r1000_dynamic_analysis/calculateJacobian.m new file mode 100644 index 0000000..fbed90a --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/calculateJacobian.m @@ -0,0 +1,50 @@ +function [jacoInfo] = calculateJacobian(robotRigidInfo,... + rotInfo,... + theta) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 计算机器人各个轴臂的雅可比矩阵， +% dynamicBasicInfo: 机械臂的基本信息 +% rotInfo: 各个轴臂的姿态信息 +% theta: 转角 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +temp = struct; +temp.jaco = zeros(6,9); +jacoInfo = struct; +jacoInfo.link = ([temp temp temp temp temp temp temp temp temp]); +jacoInfo.motor = ([temp temp temp temp temp temp temp temp temp]); +for i=1:9 + jacoInfo.link(i).jaco = zeros(6,9); + jacoInfo.motor(i).jaco = zeros(6,9); + comToRotCenter = -rotInfo.link(i).rot*robotRigidInfo.link(i).R1; + for j=i:-1:1 + if robotRigidInfo.link(j).jointAxis>3 + rotAxis=zeros(3,1); + rotAxis(robotRigidInfo.link(j).jointAxis-3) = 1; + % movement + jacoInfo.link(i).jaco(1:3,j) = -vector2so3(comToRotCenter)*rotInfo.link(j).rot*rotAxis; + if j>1 + comToRotCenter = comToRotCenter + ... + rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ... + - rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1; + end + % rotation + jacoInfo.link(i).jaco(4:6,j) = rotInfo.link(i).rotTran*rotInfo.link(j).rot*rotAxis; + else + tranAxis=zeros(3,1); + tranAxis(robotRigidInfo.link(j).jointAxis) = 1; + % movement + jacoInfo.link(i).jaco(1:3,j) = rotInfo.link(j).rot*tranAxis; + if j>1 + comToRotCenter = comToRotCenter + ... + rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ... + - rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1 + ... + rotInfo.link(j).rot*tranAxis*theta(j); + end + end + + end + motorRotAxis = zeros(3,1); + motorRotAxis(robotRigidInfo.motor(i).jointAxis-3) = 1; + jacoInfo.motor(i).jaco = jacoInfo.link(i).jaco; + jacoInfo.motor(i).jaco(4:6,i) = robotRigidInfo.motor(i).gearRatio*motorRotAxis; +end \ No newline at end of file diff --git a/r1000dynamics/r1000_dynamic_analysis/calculateMassMatrix.m b/r1000dynamics/r1000_dynamic_analysis/calculateMassMatrix.m new file mode 100644 index 0000000..3bc5656 --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/calculateMassMatrix.m @@ -0,0 +1,28 @@ +function [massMatrix,totalMass,gravityForce] = calculateMassMatrix(dynamicBasicInfo,... + toolMass,jacoInfo) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 计算机器人系统的质量矩阵， +% dynamicBasicInfo: 机械臂的基本信息 +% jacoInfo: 雅可比矩阵 +% toolMass: 器械的质量 +% +% author:li.jiang +% e-mail:li.jiang@csrbtx.com +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +g = -9.8; +massMatrix = zeros(9,9); % init massMatrix to be zeros +totalMass = 0; +gravityForce = zeros(9,1); +for i=1:9-1 + tempMatrix = [dynamicBasicInfo.link(i).mass*eye(3) zeros(3,3); + zeros(3,3) dynamicBasicInfo.link(i).inertialMatrix]; + massMatrix = massMatrix + jacoInfo.link(i).jaco'*tempMatrix*jacoInfo.link(i).jaco; + totalMass = totalMass+dynamicBasicInfo.link(i).mass; + gravityForce = gravityForce + jacoInfo.link(i).jaco'*[0;0;g*dynamicBasicInfo.link(i).mass;0;0;0]; +end +tempMatrix = [(dynamicBasicInfo.link(9).mass+toolMass)*eye(3) zeros(3,3); + zeros(3,3) dynamicBasicInfo.link(9).inertialMatrix]; +massMatrix = massMatrix + jacoInfo.link(9).jaco'*tempMatrix*jacoInfo.link(9).jaco; +totalMass = totalMass+dynamicBasicInfo.link(9).mass+toolMass; +gravityForce = gravityForce + jacoInfo.link(9).jaco'*[0;0;g*(dynamicBasicInfo.link(9).mass+toolMass);0;0;0]; \ No newline at end of file diff --git a/r1000dynamics/r1000_dynamic_analysis/get_robot_arm_dynamic_basic_info.m b/r1000dynamics/r1000_dynamic_analysis/get_robot_arm_dynamic_basic_info.m new file mode 100644 index 0000000..04d9902 --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/get_robot_arm_dynamic_basic_info.m @@ -0,0 +1,11 @@ +function [dynamicBasicInfo] = get_robot_arm_dynamic_basic_info() +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% get_PSA_robot_arm_dynamic_basic_info.m +% robot arm dynamic init file,load dynamic basic info into a struct +% author:Li Jiang +% e-mail:li.jiang@csrbtx.com +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% load data from files +dynamicBasicInfo = get_robot_arm_rigid_info(); +dynamicBasicInfo.dof = 9; \ No newline at end of file diff --git a/r1000dynamics/r1000_dynamic_analysis/get_robot_arm_rigid_info.m b/r1000dynamics/r1000_dynamic_analysis/get_robot_arm_rigid_info.m new file mode 100644 index 0000000..a5fe0d9 --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/get_robot_arm_rigid_info.m @@ -0,0 +1,155 @@ +function [rigidInfo] = get_robot_arm_rigid_info() +rigidInfo.link(1).mass = 2.1315260e+01; +rigidInfo.link(1).jointAxis = 6; +rigidInfo.link(1).R1 = -[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3; +rigidInfo.link(1).R2 = -[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3; +rigidInfo.link(1).inertialMatrix = [1.2998819e+08 1.2600723e+07 -3.5301328e+07; + 1.2600723e+07 2.8872462e+08 3.7145910e+06; + -3.5301328e+07 3.7145910e+06 2.4658035e+08]*1e-9; +rigidInfo.motor(1).mass = 0; +rigidInfo.motor(1).jointAxis = 6; +rigidInfo.motor(1).gearRatio = 50; +rigidInfo.motor(1).R1 = [0 0 0]; +rigidInfo.motor(1).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(1).inertialMatrix(rigidInfo.motor(1).jointAxis-3,... + rigidInfo.motor(1).jointAxis-3) = 101219.53*1e-9*0; + + +rigidInfo.link(2).mass = 1.9235354e+01; +rigidInfo.link(2).jointAxis = 5; +rigidInfo.link(2).R1 = -[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3; +rigidInfo.link(2).R2 = -[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3; +rigidInfo.link(2).inertialMatrix = [9.4072398e+07 1.1819105e+08 -8.6335749e+03; + 1.1819105e+08 1.2626927e+09 -2.3575826e+03; + -8.6335749e+03 -2.3575826e+03 1.2707334e+09]*1e-9; +rigidInfo.motor(2).mass = 0; +rigidInfo.motor(2).jointAxis = 5; +rigidInfo.motor(2).gearRatio = 50; +rigidInfo.motor(2).R1 = [0 0 0]; +rigidInfo.motor(2).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(2).inertialMatrix(rigidInfo.motor(2).jointAxis-3,... + rigidInfo.motor(2).jointAxis-3) = 101219.53*1e-9*0; + +rigidInfo.link(3).mass = 1.0463871e+01; +rigidInfo.link(3).jointAxis = 5; +rigidInfo.link(3).R1 = -[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3; +rigidInfo.link(3).R2 = -[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3; +rigidInfo.link(3).inertialMatrix = [2.7873904e+07 4.4214569e+06 1.5506243e+05; + 4.4214569e+06 5.4509917e+08 -2.2695805e+04; + 1.5506243e+05 -2.2695805e+04 5.3638465e+08]*1e-9; +rigidInfo.motor(3).mass = 0; +rigidInfo.motor(3).jointAxis = 5; +rigidInfo.motor(3).gearRatio = 50; +rigidInfo.motor(3).R1 = [0 0 0]; +rigidInfo.motor(3).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(3).inertialMatrix(rigidInfo.motor(3).jointAxis-3,... + rigidInfo.motor(3).jointAxis-3) = 101219.53*1e-9*0; + +rigidInfo.link(4).mass = 6.1538806; +rigidInfo.link(4).jointAxis = 5; +rigidInfo.link(4).R1 = -[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3; +rigidInfo.link(4).R2 = -[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3; +rigidInfo.link(4).inertialMatrix = [1.3401032e+07 -3.3450696e+06 -4.9938448e+06; + -3.3450696e+06 2.9613188e+07 -9.7003445e+05; + -4.9938448e+06 -9.7003445e+05 2.9945110e+07]*1e-9; +rigidInfo.motor(4).mass = 0; +rigidInfo.motor(4).jointAxis = 5; +rigidInfo.motor(4).gearRatio = 50; +rigidInfo.motor(4).R1 = [0 0 0]; +rigidInfo.motor(4).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(4).inertialMatrix(rigidInfo.motor(4).jointAxis-3,... + rigidInfo.motor(4).jointAxis-3) = 101219.53*1e-9*0; + + +rigidInfo.link(5).mass = 3.0882893; +rigidInfo.link(5).jointAxis = 6; +rigidInfo.link(5).R1 = -[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3; +rigidInfo.link(5).R2 = -[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3; +rigidInfo.link(5).inertialMatrix = [3.5376413e+06 -1.2573168e+02 -1.7221434e+06; + -1.2573168e+02 1.7872508e+07 7.0524077e+02; + -1.7221434e+06 7.0524077e+02 1.8213619e+07]*1e-9; +rigidInfo.motor(5).mass = 0; +rigidInfo.motor(5).jointAxis = 6; +rigidInfo.motor(5).gearRatio = 50; +rigidInfo.motor(5).R1 = [0 0 0]; +rigidInfo.motor(5).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(5).inertialMatrix(rigidInfo.motor(5).jointAxis-3,... + rigidInfo.motor(5).jointAxis-3) = 101219.53*1e-9*0; + +rigidInfo.link(6).mass = 2.4450941; +rigidInfo.link(6).jointAxis = 4; +rigidInfo.link(6).R1 = -[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3; +rigidInfo.link(6).R2 = -[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3; +rigidInfo.link(6).inertialMatrix = [3.7434848e+06 2.7091091e+03 -3.2187006e+05; + 2.7091091e+03 6.1710692e+06 4.3115007e+03; + -3.2187006e+05 4.3115007e+03 5.2814895e+06]*1e-9; +rigidInfo.motor(6).mass = 0; +rigidInfo.motor(6).jointAxis = 4; +rigidInfo.motor(6).gearRatio = 50; +rigidInfo.motor(6).R1 = [0 0 0]; +rigidInfo.motor(6).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(6).inertialMatrix(rigidInfo.motor(6).jointAxis-3,... + rigidInfo.motor(6).jointAxis-3) = 101219.53*1e-9*0; + +rigidInfo.link(7).mass = 3.6376589; +rigidInfo.link(7).jointAxis = 6; +rigidInfo.link(7).R1 = -[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3; +rigidInfo.link(7).R2 = -[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3; +rigidInfo.link(7).inertialMatrix = [3.6676807e+07 1.9814032e+03 6.3921710e+03; + 1.9814032e+03 3.5426529e+07 -1.1636151e+05; + 6.3921710e+03 -1.1636151e+05 3.9665088e+06]*1e-9; +rigidInfo.motor(7).mass = 0; +rigidInfo.motor(7).jointAxis = 6; +rigidInfo.motor(7).gearRatio = 50; +rigidInfo.motor(7).R1 = [0 0 0]; +rigidInfo.motor(7).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(7).inertialMatrix(rigidInfo.motor(7).jointAxis-3,... + rigidInfo.motor(7).jointAxis-3) = 101219.53*1e-9*0; + +rigidInfo.link(8).mass = 1.7005420; +rigidInfo.link(8).jointAxis = 5; +rigidInfo.link(8).R1 = -[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3; +rigidInfo.link(8).R2 = -[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; +rigidInfo.link(8).inertialMatrix = [1.3876431e+06 -2.9158636e+04 3.8674359e+06; + -2.9158636e+04 6.0220640e+07 1.7266885e+03; + 3.8674359e+06 1.7266885e+03 5.9261473e+07]*1e-9; +rigidInfo.motor(8).mass = 0; +rigidInfo.motor(8).jointAxis = 5; +rigidInfo.motor(8).gearRatio = 50; +rigidInfo.motor(8).R1 = [0 0 0]; +rigidInfo.motor(8).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(8).inertialMatrix(rigidInfo.motor(8).jointAxis-3,... + rigidInfo.motor(8).jointAxis-3) = 101219.53*1e-9*0; + +rigidInfo.link(9).mass = 9.1507819e-1; +rigidInfo.link(9).jointAxis = 1; +rigidInfo.link(9).R1 = -[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3; +rigidInfo.link(9).R2 = -[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3; +rigidInfo.link(9).inertialMatrix = [8.3261779e+05 2.0399642e+03 1.3660323e+05; + 2.0399642e+03 1.6688880e+06 6.0504274e+03; + 1.3660323e+05 6.0504274e+03 1.3114690e+06]*1e-9; +rigidInfo.motor(9).mass = 0; +rigidInfo.motor(9).jointAxis = 4; +rigidInfo.motor(9).gearRatio = 83.3; +rigidInfo.motor(9).R1 = [0 0 0]; +rigidInfo.motor(9).inertialMatrix = [0 0 0; + 0 0 0; + 0 0 0]*1e-9; +rigidInfo.motor(9).inertialMatrix(rigidInfo.motor(8).jointAxis-3,... + rigidInfo.motor(8).jointAxis-3) = 101219.53*1e-9*0; \ No newline at end of file diff --git a/r1000dynamics/r1000_dynamic_analysis/test_mass_calculation.m b/r1000dynamics/r1000_dynamic_analysis/test_mass_calculation.m new file mode 100644 index 0000000..8664c12 --- /dev/null +++ b/r1000dynamics/r1000_dynamic_analysis/test_mass_calculation.m @@ -0,0 +1,14 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% test mass calculation function +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% dynamicBasicInfo = get_PSA_robot_arm_dynamic_basic_info(); +dynamicBasicInfo = get_robot_arm_dynamic_basic_info(); +theta = zeros(9,1); +% theta = [0;-pi/3;2*pi/3;-pi/3;pi/2;-pi/2;0;0;0]; +rotInfo = calculateArmRot(dynamicBasicInfo,theta); +jaco = calculateJacobian(dynamicBasicInfo,... + rotInfo,... + theta); +[massMatrix,~,gravityForce] = calculateMassMatrix(dynamicBasicInfo,... + 0,... + jaco); diff --git a/r1000dynamics/resources/project/Project.xml b/r1000dynamics/resources/project/Project.xml new file mode 100644 index 0000000..4a9e34e --- /dev/null +++ b/r1000dynamics/resources/project/Project.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/ProjectData.type.Info.xml b/r1000dynamics/resources/project/ProjectData.type.Info.xml new file mode 100644 index 0000000..3dae9b1 --- /dev/null +++ b/r1000dynamics/resources/project/ProjectData.type.Info.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category.xml new file mode 100644 index 0000000..1fba42f --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/artifact.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/artifact.type.Label.xml new file mode 100644 index 0000000..8bbe858 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/artifact.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/convenience.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/convenience.type.Label.xml new file mode 100644 index 0000000..2f9c7ea --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/convenience.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/derived.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/derived.type.Label.xml new file mode 100644 index 0000000..38593ab --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/derived.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/design.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/design.type.Label.xml new file mode 100644 index 0000000..8cbb332 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/design.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/none.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/none.type.Label.xml new file mode 100644 index 0000000..14875c1 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/none.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/other.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/other.type.Label.xml new file mode 100644 index 0000000..d211b9b --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/other.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/test.type.Label.xml b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/test.type.Label.xml new file mode 100644 index 0000000..f8fc820 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Categories/FileClassCategory.type.Category/test.type.Label.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian2.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian2.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian2.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian3.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian3.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/calculate_jacobian3.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/calculate_rot_info.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/calculate_rot_info.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/calculate_rot_info.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/code_generation.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/code_generation.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/code_generation.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/code_generation.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/code_generation.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/code_generation.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/myfunction.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/myfunction.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/myfunction.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/CMakeLists.txt.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/CMakeLists.txt.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/CMakeLists.txt.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File/joint_names_r1000_asm_1109_urdf.yaml.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File/joint_names_r1000_asm_1109_urdf.yaml.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/config.type.File/joint_names_r1000_asm_1109_urdf.yaml.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/export.log.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/export.log.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/export.log.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/display.launch.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/display.launch.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/display.launch.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/gazebo.launch.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/gazebo.launch.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/launch.type.File/gazebo.launch.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/base_link.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/base_link.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/base_link.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/endeffector_link.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/endeffector_link.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/endeffector_link.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link1.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link1.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link1.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link2.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link2.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link2.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link3.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link3.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link3.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link4.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link4.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link4.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link5.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link5.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link5.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link6.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link6.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link6.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link7.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link7.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link7.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link8.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link8.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/meshes.type.File/link8.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/package.xml.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/package.xml.type.File.xml new file mode 100644 index 0000000..38d9917 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/package.xml.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/base_link.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/base_link.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/base_link.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/endeffector_link.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/endeffector_link.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/endeffector_link.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link1.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link1.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link1.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link2.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link2.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link2.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link3.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link3.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link3.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link4.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link4.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link4.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link5.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link5.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link5.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link6.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link6.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link6.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link7.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link7.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link7.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link8.STL.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link8.STL.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/stl.type.File/link8.STL.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/1.type.DIR_SIGNIFIER.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/r1000_asm_1109_urdf.csv.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/r1000_asm_1109_urdf.csv.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/r1000_asm_1109_urdf.csv.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/r1000_asm_1109_urdf.urdf.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/r1000_asm_1109_urdf.urdf.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.type.File/urdf.type.File/r1000_asm_1109_urdf.urdf.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.zip.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.zip.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_asm_1109_urdf.zip.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_jacobian_analysis.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_jacobian_analysis.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_jacobian_analysis.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/r1000_jacobian_analysis2.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/r1000_jacobian_analysis2.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/r1000_jacobian_analysis2.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/rot.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/rot.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/rot.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.Files/vector2so3.m.type.File.xml b/r1000dynamics/resources/project/Root.type.Files/vector2so3.m.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.Files/vector2so3.m.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.ProjectPath/221a8ba8-8f5d-4a4a-9563-6d3760df55d8.type.Reference.xml b/r1000dynamics/resources/project/Root.type.ProjectPath/221a8ba8-8f5d-4a4a-9563-6d3760df55d8.type.Reference.xml new file mode 100644 index 0000000..2bf815d --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.ProjectPath/221a8ba8-8f5d-4a4a-9563-6d3760df55d8.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.ProjectPath/881558d8-2b9e-44d6-8af7-87f4796f4642.type.Reference.xml b/r1000dynamics/resources/project/Root.type.ProjectPath/881558d8-2b9e-44d6-8af7-87f4796f4642.type.Reference.xml new file mode 100644 index 0000000..19e112f --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.ProjectPath/881558d8-2b9e-44d6-8af7-87f4796f4642.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/Root.type.ProjectPath/f0a85309-a446-4c21-a69e-9d4ad2f5ef5a.type.Reference.xml b/r1000dynamics/resources/project/Root.type.ProjectPath/f0a85309-a446-4c21-a69e-9d4ad2f5ef5a.type.Reference.xml new file mode 100644 index 0000000..30f389c --- /dev/null +++ b/r1000dynamics/resources/project/Root.type.ProjectPath/f0a85309-a446-4c21-a69e-9d4ad2f5ef5a.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/resources/project/uuid-0cee5a93-3b7e-419d-a32c-a705ddbd37fc.xml b/r1000dynamics/resources/project/uuid-0cee5a93-3b7e-419d-a32c-a705ddbd37fc.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/r1000dynamics/resources/project/uuid-0cee5a93-3b7e-419d-a32c-a705ddbd37fc.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/r1000dynamics/rot.m b/r1000dynamics/rot.m new file mode 100644 index 0000000..1668f97 --- /dev/null +++ b/r1000dynamics/rot.m @@ -0,0 +1,22 @@ +function rotMatrix = rot(axis,angle) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% return a rotation matrix from a rotation around axis about angle +% axis: 1-x,2-y,3-z +% angle: +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +switch axis + case 1 + rotMatrix = [1 0 0; + 0 cos(angle) -sin(angle); + 0 sin(angle) cos(angle)]; + case 2 + rotMatrix = [cos(angle) 0 sin(angle); + 0 1 0; + -sin(angle) 0 cos(angle)]; + case 3 + rotMatrix = [cos(angle) -sin(angle) 0; + sin(angle) cos(angle) 0; + 0 0 1]; + otherwise + rotMatrix = eye(3); +end \ No newline at end of file diff --git a/r1000dynamics/vector2so3.m b/r1000dynamics/vector2so3.m new file mode 100644 index 0000000..ea467d1 --- /dev/null +++ b/r1000dynamics/vector2so3.m @@ -0,0 +1,10 @@ +function so3 = vector2so3(vector) +so3 = zeros(3,3); +so3(1,2) = -vector(3); +so3(1,3) = vector(2); + +so3(2,1) = vector(3); +so3(2,3) = -vector(1); + +so3(3,1) = -vector(2); +so3(3,2) = vector(1); \ No newline at end of file diff --git a/readDataFile.m b/readDataFile.m new file mode 100644 index 0000000..6589133 --- /dev/null +++ b/readDataFile.m @@ -0,0 +1,6 @@ +% function = readDataFile() +folder = 'C:\GitLab\Robotics\Identification\IRDYN\simpack_data\data\'; +posInfoFile = 'GC_cali.csv'; +currentInfoFile = 'GC_cur.csv'; +posData = readmatrix([folder posInfoFile]); +currentData = readmatrix([folder currentInfoFile]); \ No newline at end of file diff --git a/simpack_data/R1000-DVT-statics.sbr b/simpack_data/R1000-DVT-statics.sbr new file mode 100644 index 0000000..29d5072 Binary files /dev/null and b/simpack_data/R1000-DVT-statics.sbr differ diff --git a/untitled2.m b/untitled2.m new file mode 100644 index 0000000..e9718f2 --- /dev/null +++ b/untitled2.m @@ -0,0 +1,98 @@ +Mlist_CG=[] +for i = 0:N-1 + if i == 0 + M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); + else + % rotation_i = TransToRp(robot.T(:,:,i+1)) + M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1)); + end + Mlist_CG = cat(3, Mlist_CG, M); +end +%% +Mlist_CG=[] +for i = 0:N-1 + if i == 0 + M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); + else + rotation_i = diag([1,1,1]); + for j = 1:i + rotation_i = rotation_i*TransToRp(robot.T(:,:,i)); + rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1)); + end + M = TransInv(RpToTrans(rotation_i,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,com_pos(:,i+1)); + end + Mlist_CG = cat(3, Mlist_CG, M); +end +%% +com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; +com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3; +com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3; +com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3; +com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3; +com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3; +com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3; +com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3; +com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; +com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3; +com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3; +com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3; +com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; +com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3; +com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; +com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use this +com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; +% plot 3D: Get 3D coordinate of COM +ct=[]; +for i = 1:9 + if i == 1 + ct(:,i) = com_pos_R1(:,i); + elseif i< 9 + ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i); + else + ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3;0;0.05896]+com_pos_R1(:,i); + end +end +plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r') +axis equal +grid on +% plot 3D: Get 3D coordinate of CO +co=[]; +for i = 1:8 + if i == 1 + co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i); + elseif i<8 + %From base to ISA Origin + co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i); + else + %From base to ISA Origin + co(:,i) = co(:,i-1)-[0.3;0;0.05896]; + end +end +co = [zeros(3,1),co]; +hold on +plot3(co(1,:),co(2,:),co(3,:),'o','Color','b') +axis equal +grid on +%% +for i = 1:9 +co(:,i+1)-co(:,i); +end +%% +for i = 1:9 +tt(:,i)=robot.TW(1:3,1:3,i)*com_pos_R1(:,i); +end +kk=[]; +for i = 1:9 +kk(:,:,i)=TransInv(robot.TW(:,:,i))*Mlist_CG_Base(:,:,i); +end + +%% +yy=eye(4,4); +for i =1:9 +yy = yy*Mlist_CG(:,:,i) +end +%% +A=pascal(6) +AA = A; +B=A*E1gen(A,6,1)*E1gen(A,5,2) +% AA(:,[1,2,6]) = AA(:,[6,5,1]) \ No newline at end of file diff --git a/untitled3.m b/untitled3.m new file mode 100644 index 0000000..255ed74 --- /dev/null +++ b/untitled3.m @@ -0,0 +1,169 @@ +R = robot.kine.R; +P = robot.kine.t; +F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end) +F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1 + +FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)' +FF2 = F_Simpack(end-1,:,1)'+FF1 +%% +F1 = robot.regressor.A(:,:,end)*robot.pi(:,end); +F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2); +%% +F_Simpack(end,:,1) +F_Simpack(end-2,:,1) +%% +robot_pi_vecoter = reshape(robot.pi,[90,1]); +F = robot.regressor.U*robot_pi_vecoter; +FF = reshape(F,[6,9]) +%% +F_Simpack(:,:,1) +%% +robot_pi_vecoter = reshape(robot.pi,[90,1]); +tau_standard = robot.regressor.K*robot_pi_vecoter; +tau_standard = reshape(tau_standard,[1,9]) + +tau_base = robot.baseQR.regressor*robot.baseQR.baseParams; +tau_base = reshape(tau_base,[1,9]) +%% +tau_estimate = robot.baseQR.regressor*robot.sol.pi_b; +tau_estimate = reshape(tau_estimate,[1,9]) +%% +time = 0:0.1:1; +f=1; +q_J = sin(2*pi*f*time); +qd_J = (2*pi*f)*cos(2*pi*f*time); +qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]; +qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J]; +qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J]; +g = [0; 0; -9.8]; +tau = zeros([robot.ndof,length(q_J)]); +% pi -> [m;mc;I] 10 element +[nLnkPrms, nLnks] = size(robot.pi); +robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); +Wb = []; Tau = []; +for i = 1:length(q_J) + % regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i)); + standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); + regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i)); + tau=regressor*robot_pi; + Tau = vertcat(Tau, tau); +end +for i = 1:1:length(q_J) + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i)); + Wb = vertcat(Wb, Yb); +end +%% +time = 0:0.01:1; +f=1; +q_J = sin(2*pi*f*time); +qd_J = (2*pi*f)*cos(2*pi*f*time); +qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]; +qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J]; +qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J]; +g = [0; 0; -9.8]; +tau = zeros([robot.ndof,length(q_J)]); +% pi -> [m;mc;I] 10 element +[nLnkPrms, nLnks] = size(robot.pi); +robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); +Wb = []; Tau = [];Tau_estimate = []; +for i = 1:length(q_J) + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i),robot.baseQR); + tau_estimate=Yb*robot.sol.pi_b; + tau = Yb*robot.baseQR.baseParams; + Tau_estimate = horzcat(Tau_estimate, tau_estimate); + Tau = horzcat(Tau, tau); +end +%% get symbol result for GC +ndof = robot.ndof; +q_sym = sym('q%d',[ndof+1,1],'real'); +qd_sym = sym('qd%d',[ndof+1,1],'real'); +q2d_sym = sym('qdd%d',[ndof+1,1],'real'); +% init regressor +robot.regressor.m = sym('m%d',[ndof,1],'real'); +robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real'); +robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real'); +robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real'); +robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real'); +robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real'); +robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real'); +robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real'); +robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real'); +robot.regressor.izz = sym('i%d_zz',[ndof,1],'real'); +robot.regressor.im = sym('im%d',[ndof,1],'real'); +for i = 1:ndof + robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),... + robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),... + robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]'; +end +[nLnkPrms, nLnks] = size(robot.regressor.pi); +robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]); +qr_rank = robot.baseQR.numberOfBaseParameters; +E = robot.baseQR.permutationMatrix; +pi_lgr_sym = robot.regressor.pi; +pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters +pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres +beta = robot.baseQR.beta; +% all of the expressions below are equivalent +pi_lgr_base = pi1 + beta*pi2; +vpa(simplify(pi_lgr_base),2) +%% robot current and torque map +motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185]; +gearRatio = [101,101,100,100,100,100,100,100,100]; +% readDataFile; +% get pos +[nRow,nCol] = size(posData); +index_p = find(posData(:,nCol-1)==7); +jointPos_p = posData(index_p,1:9); +jointPos_p = jointPos_p(1:500:end,:); +index_n = find(posData(:,nCol-1)==-7); +jointPos_n = posData(index_n,1:9); +jointPos_n = jointPos_n(1:500:end,:); +q = (jointPos_p)'; +% dq = [zeros(robot.ndof,1),diff(q,1,2)]; +% get torque from simulator +Wb = []; Tau = []; +for i = 1:length(q) + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR); + tau = Yb*robot.baseQR.baseParams; + Tau = horzcat(Tau, tau); +end +% get toruqe from current +index_p = find(currentData(:,nCol-1)==7); +jointCur_p = currentData(index_p,1:9); +jointCur_p = jointCur_p(1:500:end,:); +index_n = find(currentData(:,nCol-1)==-7); +jointCur_n = currentData(index_n,1:9); +jointCur_n = jointCur_n(1:500:end,:); +current = (jointCur_p+jointCur_n)/2; +torque_cur = gearRatio.*motorConst.*current; +plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on; +plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:)) +xlabel('Time/s') +ylabel('Torque/Nm') +legend('Torque compute form current','Torque generate form simulator using CAD data') +title('Torque compute form current vs Torque generate form simulator using CAD data') +%% +Wb = []; Tau = []; +for i = 1:length(q) + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR); + tau = Yb*robot.sol.pi_b; + Tau = horzcat(Tau, tau); +end +plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on; +plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:)) +xlabel('Time/s') +ylabel('Torque/Nm') +legend('Torque compute form current','Torque form simulator using identificated data') +title('Torque compute form current vs Torque form simulator using identificated data') +%% +plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on; +xlabel('Time/s') +ylabel('Torque/Nm') +legend('Torque compute form current') +title('Torque compute form current') \ No newline at end of file diff --git a/verify_regressor_R1000.m b/verify_regressor_R1000.m new file mode 100644 index 0000000..fffff46 --- /dev/null +++ b/verify_regressor_R1000.m @@ -0,0 +1,45 @@ +% % function robot = verify_regressor(robot, opt) +% % verify: If full regressor dynamics is the same as basic dynamics +% ndof = robot.ndof; +% % Dynamics parameters +% link_mass = robot.m; +% com_pos = robot.com; +% link_inertia = robot.I; +% +% q_sym = sym('q%d',[ndof,1],'real'); +% % for i =1:ndof +% % robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;inertiaMatrix2Tensor(link_inertia(:,:,i))]; +% % end +% regressor = standard_regressor_R1000(q_sym); +% robot_pi_vector = reshape(robot.pi,[10*ndof,1]); +% tau=regressor*robot_pi_vector; +% %% R1000 +% tau_mat = standard_dynamics_R1000(q_sym); +% %% Check if screw method is equal to regressor +% isequal(simplify(tau),simplify(tau_mat)) +%% Numerical +ndof = robot.ndof; +time = 0:0.01:1; +f=1; +q_J = sin(2*pi*f*time); +qd_J = (2*pi*f)*cos(2*pi*f*time); +qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); +zero_ = zeros(1,length(q_J)); +q_J = ones(1,length(q_J)); +thetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]'; +dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +tau = zeros([ndof,length(q_J)]); +% Dynamics parameters +link_mass = robot.m; +com_pos = robot.com; +link_inertia = robot.I; +% for i =1:ndof +% robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1(:,i);inertiaMatrix2Tensor(link_inertia(:,:,i))]; +% end +robot_pi_vector = reshape(robot.pi,[10*ndof,1]); +for i = 1:length(q_J) +% regressor = standard_regressor_R1000(thetalist(i,:)'); + tau(:,i)=robot.regressor.K*robot_pi_vector; + tau_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)'); +end