diff --git a/Identification_main.m b/Identification_main.m index 06d078f..32b11d8 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -12,6 +12,23 @@ robot = get_robot_R1000(file,opt); % robot.theta = [1,1,0]; robot = get_Kinematics(robot, opt); %TODO verify kinematics via robotics toolbox or other software result +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 +com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; R1000_Dynamics_num; % opt.Isreal = false; diff --git a/R1000_Dynamics_num.asv b/R1000_Dynamics_num.asv new file mode 100644 index 0000000..750bd4c --- /dev/null +++ b/R1000_Dynamics_num.asv @@ -0,0 +1,137 @@ +%% R1000 +N=9; +% 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)); +% Dynamics parameters +link_mass = robot.m; +com_pos = robot.com; +link_inertia = robot.I; + +thetalist = [zero_;zero_;zero_;zero_;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_]'; + +% 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 + +% 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,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(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); + +% 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)),TransToRp(robot.T(:,:,i))*(-com_pos_R2(:,i-1)+com_pos_R1(:,i))); +% elseif i==9 +% M = RpToTrans(TransToRp(robot.T(:,:,i)),TransToRp(robot.T(:,:,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 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=[[0;0;1;0;0;0],... + [0;-1;0;cross(-[0;-1;0],[0.2;0;0])]... + [0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]... + [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]... + [0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]... + [1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]... + [0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]... + [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]... + [0;0;0;1;0;0]]; + +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 + 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.m b/R1000_Dynamics_num.m index a514628..743d1d0 100644 --- a/R1000_Dynamics_num.m +++ b/R1000_Dynamics_num.m @@ -24,22 +24,51 @@ for i = 1:N end % Get the com pos transformation in each joint reference frame -Mlist_CG = []; +% 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)); + M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); else - M = TransInv(T(:,:,i+1))*robot.T(:,:,i+1)*transl(com_pos(:,i+1)); + 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 -for i = i:N - M = transl(com_pos(:,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)),TransToRp(robot.T(:,:,i))*(-com_pos_R2(:,i-1)+com_pos_R1(:,i))); +% elseif i==9 +% M = RpToTrans(TransToRp(robot.T(:,:,i)),TransToRp(robot.T(:,:,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 end efforce transformation in each joint reference frame Mlist_ED = []; for i = 1:N @@ -72,6 +101,8 @@ for i = 1:length(q_J) 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 diff --git a/get_robot_R1000.asv b/get_robot_R1000.asv new file mode 100644 index 0000000..e5cfda2 --- /dev/null +++ b/get_robot_R1000.asv @@ -0,0 +1,175 @@ +function robot = get_robot_R1000(file,opt) +switch opt.robot_def + case 'direct' + ndof = 9; + robot.ndof = ndof; + % Kinematics parameters + if(opt.Isreal) + switch opt.KM_method + case 'SDH' + case 'MDH' + robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0]; + robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896]; + robot.d = [0,0,0,0,0,0.28,0.40,0,0]; + robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2]; + robot.link_type = ['R','R','R','R','R','R','R','R','P']; + otherwise + disp('Bad opt.KM_method!') + return; + end + else + % Create symbolic generilized coordiates, their first and second deriatives + q_sym = sym('q%d',[ndof,1],'real'); + qd_sym = sym('qd%d',[ndof,1],'real'); + q2d_sym = sym('qdd%d',[ndof,1],'real'); + + robot.theta = q_sym; + robot.dtheta = qd_sym; + robot.ddtheta = q2d_sym; + %R1000 ISA + robot.theta(ndof) = 0; + robot.dtheta(ndof) = 0; + robot.ddtheta(ndof) = 0; + switch opt.KM_method + case 'SDH' + case 'MDH' + robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493]; + robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157]; + robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2]; + robot.link_type = ['R','R','R','R','R','R','R','R','P']; + otherwise + disp('Bad opt.KM_method!') + return; + end + robot.d = sym(robot.d); + robot.d(ndof)=q_sym(ndof); + %init vd + robot.vd = robot.d; + robot.vd(ndof)=q_sym(ndof); + %init accd + robot.accd = robot.d; + robot.accd(ndof)=q_sym(ndof); + end + % Dynamics parameters + link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2]; + %TODO in process, seems axis_of_rot useless + axis_of_rot(:,1) = [0;0;1]; + axis_of_rot(:,2) = [0;-1;0]; + axis_of_rot(:,3) = [0;-1;0]; + axis_of_rot(:,4) = [0;-1;0]; + axis_of_rot(:,5) = [0;0;1]; + axis_of_rot(:,6) = [1;0;0]; + axis_of_rot(:,7) = [0;0;-1]; + axis_of_rot(:,8) = [0;-1;0]; + axis_of_rot(:,9) = [0;0;0]; + % 画图 + % com_pos(:,1) = [0;0;0.122]; + % com_pos(:,2) = [0.373;0;0]; + % com_pos(:,3) = [0.188;0;0]; + % com_pos(:,4) = [0.05;0;-0.05]; + % com_pos(:,5) = [0;0.13;0]; + % com_pos(:,6) = [0;0.028;0.049]; + % com_pos(:,7) = [0;0;0.102]; + % com_pos(:,8) = [0;0.06;0]; + % com_pos(:,9) = [0;0;0]; + + % com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; + % com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3; + % com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3; + % com_pos(:,4) = [5.4048685e+01 5.8463901e+01 -5.0915631e+00]'*10^-3; + % com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; + % com_pos(:,6) = [4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3; + % com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; + % com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; + % com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; + + 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]'; + + % the inertia tensor wrt the frame oriented as the body frame and with the + % origin in the COM + % link_inertia(:,:,1) = diag([1,1,1]); + % link_inertia(:,:,2) = diag([1,1,1]); + % link_inertia(:,:,3) = diag([1,1,1]); + % link_inertia(:,:,4) = diag([1,1,1]); + % link_inertia(:,:,5) = diag([1,1,1]); + % link_inertia(:,:,6) = diag([1,1,1]); + % link_inertia(:,:,7) = diag([1,1,1]); + % link_inertia(:,:,8) = diag([1,1,1]); + % link_inertia(:,:,9) = diag([1,1,1]); + link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];... + [-1.1694021e+04 2.3289532e+05 -3.0395414e+02];... + [-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6; + link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];... + [-9.0569033e+01 2.9714754e+05 6.8396715e+00];... + [4.7526575e+04 6.8396715e+00 2.8138392e+05]]*10^-6; + link_inertia(:,:,3) = [[4.4513887e+03 1.9981964e-01 -3.0303891e+02];... + [1.9981964e-01 6.7952039e+04 -8.8585864e-02];... + [-3.0303891e+02 -8.8585864e-02 6.9958344e+04]]*10^-6; + link_inertia(:,:,4) = [[1.1642351e+04 2.2997175e+03 2.9159431e+03];... + [2.2997175e+03 2.6031269e+04 -1.3518384e+02];... + [2.9159431e+03 -1.3518384e+02 2.4694742e+04]]*10^-6; + link_inertia(:,:,5) = [[3.0930544e+03 8.3558814e-01 -2.8169092e+03];... + [8.3558814e-01 1.2796446e+04 -3.3666469e+00];... + [-2.8169092e+03 -3.3666469e+00 1.2128856e+04]]*10^-6; + link_inertia(:,:,6) = [[3.6635776e+03 -7.0081461e+00 2.2392870e+00];... + [-7.0081461e+00 1.8152305e+03 -2.4828765e+02];... + [2.2392870e+00 -2.4828765e+02 3.4602935e+03]]*10^-6; + link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];... + [-3.6340953e+00 1.3222824e+04 -4.3625500e+02];... + [ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6; + link_inertia(:,:,8) = [[1.5162872e+03 -4.6245133e+01 4.4556929e+03];... + [-4.6245133e+01 5.9901606e+04 6.0350548e+0];... + [4.4556929e+03 6.0350548e+0 5.8819998e+04]]*10^-6; + link_inertia(:,:,9) = [[1.1730106e+03 3.3834506e+0 4.6097678e+01];... + [3.3834506e+0 1.7996852e+03 5.2088778e+0];... + [4.6097678e+01 5.2088778e+0 1.3960734e+03]]*10^-6; + % verify if link_inertia is issymmetric + for i = 1:ndof + if(issymmetric(link_inertia(:,:,i))==false) + fprintf('Bad definition of inertia matrix %d\n',i) + return; + end + end + % manipulator regressor + for i = 1:ndof + robot.m(i) = link_mass(i); + robot.axis(:,i) = axis_of_rot(i); + robot.com(:,i) = com_pos(:,i); + robot.I(:,:,i) = link_inertia(:,:,i); + robot.mc(:,i) = link_mass(i)*com_pos(:,i); + % the inertia tensor wrt the frame oriented as the body frame and with the + % origin in the Joint i + com_vec2mat = vec2skewSymMat(com_pos(:,i)); + robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia(:,:,i)-... + link_mass(i)*com_vec2mat*com_vec2mat); + robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)]; + end + case 'urdf' + robot = parse_urdf(file); + case 'mat' + robot = []; + disp('TODO mat robot define options!') + otherwise + robot = []; + disp('Bad robot define options!') + return +end +%Gravity +gravity = [0;0;9.8]; +robot.gravity = gravity; \ No newline at end of file diff --git a/get_robot_R1000.m b/get_robot_R1000.m index 6ecb2ff..7afd076 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -44,15 +44,15 @@ switch opt.robot_def robot.d = sym(robot.d); robot.d(ndof)=q_sym(ndof); %init vd - robot.vd = robot.d; + robot.vd = robot.d; robot.vd(ndof)=q_sym(ndof); %init accd robot.accd = robot.d; robot.accd(ndof)=q_sym(ndof); end % Dynamics parameters - link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2]; - %TODO in process, seems axis_of_rot useless + link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73+1.2,0]; + %TODO in process, seems axis_of_rot useless axis_of_rot(:,1) = [0;0;1]; axis_of_rot(:,2) = [0;-1;0]; axis_of_rot(:,3) = [0;-1;0]; @@ -63,15 +63,15 @@ switch opt.robot_def axis_of_rot(:,8) = [0;-1;0]; axis_of_rot(:,9) = [0;0;0]; % 画图 -% com_pos(:,1) = [0;0;0.122]; -% com_pos(:,2) = [0.373;0;0]; -% com_pos(:,3) = [0.188;0;0]; -% com_pos(:,4) = [0.05;0;-0.05]; -% com_pos(:,5) = [0;0.13;0]; -% com_pos(:,6) = [0;0.028;0.049]; -% com_pos(:,7) = [0;0;0.102]; -% com_pos(:,8) = [0;0.06;0]; -% com_pos(:,9) = [0;0;0]; + % com_pos(:,1) = [0;0;0.122]; + % com_pos(:,2) = [0.373;0;0]; + % com_pos(:,3) = [0.188;0;0]; + % com_pos(:,4) = [0.05;0;-0.05]; + % com_pos(:,5) = [0;0.13;0]; + % com_pos(:,6) = [0;0.028;0.049]; + % com_pos(:,7) = [0;0;0.102]; + % com_pos(:,8) = [0;0.06;0]; + % com_pos(:,9) = [0;0;0]; com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3; @@ -82,17 +82,36 @@ switch opt.robot_def com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; + + 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 + com_pos_R1(:,9)=[0 0 0]'*10^-3; + % the inertia tensor wrt the frame oriented as the body frame and with the % origin in the COM -% link_inertia(:,:,1) = diag([1,1,1]); -% link_inertia(:,:,2) = diag([1,1,1]); -% link_inertia(:,:,3) = diag([1,1,1]); -% link_inertia(:,:,4) = diag([1,1,1]); -% link_inertia(:,:,5) = diag([1,1,1]); -% link_inertia(:,:,6) = diag([1,1,1]); -% link_inertia(:,:,7) = diag([1,1,1]); -% link_inertia(:,:,8) = diag([1,1,1]); -% link_inertia(:,:,9) = diag([1,1,1]); + % link_inertia(:,:,1) = diag([1,1,1]); + % link_inertia(:,:,2) = diag([1,1,1]); + % link_inertia(:,:,3) = diag([1,1,1]); + % link_inertia(:,:,4) = diag([1,1,1]); + % link_inertia(:,:,5) = diag([1,1,1]); + % link_inertia(:,:,6) = diag([1,1,1]); + % link_inertia(:,:,7) = diag([1,1,1]); + % link_inertia(:,:,8) = diag([1,1,1]); + % link_inertia(:,:,9) = diag([1,1,1]); link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];... [-1.1694021e+04 2.3289532e+05 -3.0395414e+02];... [-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6; diff --git a/untitled2.m b/untitled2.m new file mode 100644 index 0000000..a69ce23 --- /dev/null +++ b/untitled2.m @@ -0,0 +1,53 @@ +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; +com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; +ct=[] +for i = 1:7 + if i == 1 + ct(:,i) = com_pos_R1(:,i); + elseif i< 8 + ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i); + elseif i==8 + % ct(:,) + end +end \ No newline at end of file