regressor result is wrong

Identification_main.m

@@ -7,7 +7,7 @@ opt.LD_method = 'Direct';
 opt.debug = true;
 opt.robotName = 'R1000';
 opt.reGenerate = false;
-opt.Isreal = false;
+opt.Isreal = true;
 
 robot = get_robot_R1000(file,opt);
 robot = get_Kinematics(robot, opt);

get_Kinematics.m

@@ -25,6 +25,8 @@ if(opt.Isreal)
             %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 

get_regressor.m

@@ -24,8 +24,8 @@ end
 [nLnkPrms, nLnks] = size(robot.regressor.pi);
 robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
 % init matrix
-R = robot.R;
+R = robot.kine.R;
-P = robot.t;
+P = robot.kine.t;
 w = robot.vel.w ;
 dw = robot.vel.dw ;
 dv = robot.vel.dv ;
@@ -67,17 +67,19 @@ switch opt.LD_method
                 end
                 robot.regressor.K = delta_*robot.regressor.U;
                 if(opt.debug)
-                    sprintf('size of U=%dx%d.',size(robot.regressor.U));
+                    sprintf('size of U=%dx%d.',size(robot.regressor.U))
-                    sprintf('size of K=%dx%d.',size(robot.regressor.K));
+                    sprintf('size of K=%dx%d.',size(robot.regressor.K))
                 end
         end
-%         matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
+        %         matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
-%             'Vars',{q_sym,qd_sym,q2d_sym});
+        %             'Vars',{q_sym,qd_sym,q2d_sym});
-tic
+        if(opt.reGenerate)
-        matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
+            tic
-            'Vars',{q_sym});
+            matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
-        compileTime = toc;
+                'Vars',{q_sym});
-        fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
+            compileTime = toc;
+            fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
+        end
 %     matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
 %                    'Vars',{q_sym,qd_sym,q2d_sym});
     case 'Lagrange'

get_robot_R1000.m

@@ -117,6 +117,9 @@ switch opt.robot_def
         % Kinematics parameters
         % ------------------------------------------------------------------------
         if(opt.Isreal)
+            %FIXME: only consider the theta temply
+            robot.dtheta = zeros([1,ndof]);
+            robot.ddtheta = zeros([1,ndof]);
             switch opt.KM_method
                 case 'SDH'
                 case 'MDH'

get_velocity.m

@@ -1,10 +1,6 @@
 function robot = get_velocity(robot, opt)
 switch opt.KM_method
     case 'SCREW'
-        % init q
-        q = robot.theta;
-        qd = robot.dtheta;
-        qdd = robot.ddtheta;
         Mlist_CG = robot.kine.Mlist_CG;
         % Get general mass matrix
         link_mass = robot.m;
@@ -15,14 +11,31 @@ switch opt.KM_method
         for i = 1:robot.ndof
             Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
             Glist = cat(3, Glist, Gb);
+            % ?
+%             mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
+%             robot.pi(2:4,i) = mc;
+        end
+        if opt.Isreal
+            % init q
+            q = robot.theta';
+            qd = robot.dtheta';
+            qdd = robot.ddtheta';
+            [V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
+                robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
+        else
+            % init q
+            q = robot.theta;
+            qd = robot.dtheta;
+            qdd = robot.ddtheta;
+            [V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
+                robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
         end
-        [V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
-            robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
         % FIXME: twist is not equal to velocity
         robot.vel.w = V(1:3,:);
         robot.vel.v = V(4:6,:);
         robot.vel.dw = Vd(1:3,:);
-        robot.vel.dv = Vd(4:6,:);
+%         robot.vel.dv = Vd(4:6,:);
+        robot.vel.dv=[zeros(2,robot.ndof);robot.gravity(end)*ones(1,robot.ndof)];
     case 'SDH'
     case 'MDH'
         switch opt.Vel_method

verify_regressor_R1000.asv

@@ -1,87 +0,0 @@
-% 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,1],'real');
-qd_sym = sym('qd%d',[ndof+1,1],'real');
-q2d_sym = sym('qdd%d',[ndof+1,1],'real');
-for i =1:ndof
-pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;];
-end
-% pi2=zeros([10,1]);
-pi=[pi1;pi2];
-regressor = standard_regressor_Two_bar(q_sym,qd_sym,q2d_sym);
-tau=regressor*pi;
-%% Two-bar
-N=2;
-thetalist = q_sym(1:N);
-dthetalist = qd_sym(1:N);
-ddthetalist = q2d_sym(1:N);
-
-Gb= [diag([1,1,1]),zeros(3,3);
-    zeros(3,3),diag([1,1,1])];
-Glist = cat(3, Gb, Gb);
-% Glist = cat(3, Gb, zeros([6,6]));
-M01 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-M23 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-Mlist = cat(3, M01, M12, M23);
-Slist=[[0;0;1;0;0;0],...
-    [0;0;1;0;-1;0]];
-Adgab_mat = sym(zeros(6,6,N+1));
-Fmat=sym(zeros(N,6));
-F1=sym(zeros(N,6));
-V1=sym(zeros(6,N+1));
-G=sym(zeros(4,4,N));
-T=sym(zeros(4,4,N));
-Vlinear=sym(zeros(3,3));
-Vd1=sym(zeros(6,N+1));
-Gb= [diag([1,1,1]),zeros(3,3);
-    zeros(3,3),diag([1,1,1])];
-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;0], exf, Mlist, Glist, Slist);
-G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
-M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-M23 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-Mlist = cat(3, M01, M12, M23);
-T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
-F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
-% 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);
-%% Check if screw method is equal to regressor
-isequal(simplify(tau),simplify(tau_mat))
-%% Numerical
-clear pi;
-ndof = robot.ndof;
-time = 0:0.01: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);
-q=[q_J;-q_J];
-qd=[qd_J; -qd_J];
-qdd=[qdd_J; -qdd_J];
-g = [0; 0; -9.8];
-robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
-robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
-% pi2=zeros([10,1]);
-robot_pi=[robot_pi1;robot_pi2];
-tau = zeros([2,100]);
-for i = 1:length(q_J)
-    regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
-    tau(:,i)=regressor*robot_pi;
-end

verify_regressor_R1000.m

@@ -1,40 +1,44 @@
-% function robot = verify_regressor(robot, opt)
+% % function robot = verify_regressor(robot, opt)
-% verify: If full regressor dynamics is the same as basic dynamics
+% % 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));
+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_]';
+tau = zeros([ndof,length(q_J)]);
 % 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))];
+%     robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1(:,i);inertiaMatrix2Tensor(link_inertia(:,:,i))];
 % end
-% regressor = standard_regressor_R1000(q_sym);
 robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
-tau=robot.regressor.K*robot_pi_vector;
+for i = 1:length(q_J)
-%% R1000
+    regressor = standard_regressor_R1000(thetalist(i,:)');
-tau_mat = standard_dynamics_R1000(q_sym);
+    tau(:,i)=robot.regressor.K*robot_pi_vector;
-%% Check if screw method is equal to regressor
+    tau_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)');
-isequal(simplify(tau),simplify(tau_mat))
+end
-% %% Numerical
-% clear pi;
-% ndof = robot.ndof;
-% time = 0:0.01: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);
-% q=[q_J;-q_J];
-% qd=[qd_J; -qd_J];
-% qdd=[qdd_J; -qdd_J];
-% g = [0; 0; -9.8];
-% robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
-% robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
-% % pi2=zeros([10,1]);
-% robot_pi=[robot_pi1;robot_pi2];
-% tau = zeros([2,100]);
-% for i = 1:length(q_J)
-%     regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
-%     tau(:,i)=regressor*robot_pi;
-% end