From a50a0698d6a91a7b0e41bf42a3d770bc97478450 Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Fri, 27 Sep 2024 01:22:35 +0800 Subject: [PATCH] testing --- R1000_Dynamics_num_regressor.m | 96 ++++++++++++++++++++++++++++++++++ Untitled.m | 17 ++++++ 2 files changed, 113 insertions(+) create mode 100644 R1000_Dynamics_num_regressor.m create mode 100644 Untitled.m 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