fixing regressor

Identification_main.m

@@ -6,19 +6,17 @@ opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
 opt.debug = true;
 opt.robotName = 'R1000';
-
+opt.reGenerate = false;
 opt.Isreal = false;
+
 robot = get_robot_R1000(file,opt);
-% robot.theta = [1,1,0];
-
-
 robot = get_Kinematics(robot, opt);
 
 % R1000_Dynamics_num;
 % R1000_Dynamics;
-% opt.Isreal = false;
+% getGravityForce;
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot,opt);
 % symbol matched
-% verify_regressor
+% verify_regressor_R1000;
 % robot = get_baseParams(robot, opt);

R1000_Dynamics_num.m

@@ -12,7 +12,7 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+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_]';
 

getGravityForce.asv

@@ -0,0 +1,30 @@
+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;
+
+% 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];
+
+for i = 1:N
+gravityForces(:,i) = Glist(:,:,i)*[zeros(3,1);gravity];
+Jacoblist(:,:,i) = JacobianSpace(Slist(:,1:i),thetalist(1:i));
+end
+
+for i = N:-1:1
+    gravityTorques(i) = transpose(Jacoblist(:,:,i))*gravityForces(:,i);
+end

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;

get_regressor.m

@@ -71,8 +71,13 @@ switch opt.LD_method
                     sprintf('size of K=%dx%d.',size(robot.regressor.K));
                 end
         end
+%         matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
+%             'Vars',{q_sym,qd_sym,q2d_sym});
+tic
         matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
-            'Vars',{q_sym,qd_sym,q2d_sym});
+            'Vars',{q_sym});
+        compileTime = toc;
+        fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
 %     matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
 %                    'Vars',{q_sym,qd_sym,q2d_sym});
     case 'Lagrange'

get_robot_R1000.m

@@ -224,5 +224,5 @@ switch opt.robot_def
         return
 end
 %Gravity
-gravity = [0;0;9.8];
+gravity = [0;0;-9.806];
 robot.gravity = gravity;

get_velocity.m

@@ -2,19 +2,27 @@ function robot = get_velocity(robot, opt)
 switch opt.KM_method
     case 'SCREW'
         % init q
-%         q = robot.theta;
-%         qd = robot.dtheta;
-%         qdd = robot.ddtheta;
-%     [V,Vd,~,~,~] = InverseDynamics_debug(thetalist, dthetalist, ddthetalist, ...
-%                                            g, Ftip,Mlist, Glist, Slist);
-%     robot.vel.w = w;
-%     robot.vel.v = v;
-%     robot.vel.dw = dw;
-%     robot.vel.dv = dv;
-    robot.vel.w = zeros(3,robot.ndof);
-    robot.vel.v = zeros(3,robot.ndof);
-    robot.vel.dw = zeros(3,robot.ndof);
-    robot.vel.dv = zeros(3,robot.ndof);
+        q = robot.theta;
+        qd = robot.dtheta;
+        qdd = robot.ddtheta;
+        Mlist_CG = robot.kine.Mlist_CG;
+        % Get general mass matrix
+        link_mass = robot.m;
+        com_pos = robot.com;
+        link_inertia = robot.I;
+        Slist=robot.slist;
+        Glist=[];
+        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);
+        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,:);
     case 'SDH'
     case 'MDH'
         switch opt.Vel_method

inertiaMatrix2Tensor.m

@@ -0,0 +1,10 @@
+function out = inertiaMatrix2Tensor(I)
+% -----------------------------------------------------------------------
+% This function conerts ineria matrix of the link to vector in 
+% 'Gautier-Khalil' notation (because it is used in all their papers)
+% Input:
+%   I - 3x3 inertia matrix of the link
+% Output:
+%   out - [Ixx Ixy Ixz Iyy Iyz Izz]
+% -----------------------------------------------------------------------
+    out = [I(1,1) I(1,2) I(1,3) I(2,2) I(2,3) I(3,3)]';

verify_regressor_R1000.asv

@@ -0,0 +1,87 @@
+% 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,81 +1,40 @@
 % function robot = verify_regressor(robot, opt)
 % verify: If full regressor dynamics is the same as basic dynamics
 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');
-pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
-pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
-% 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);
+% Dynamics parameters
+link_mass = robot.m;
+com_pos = robot.com;
+link_inertia = robot.I;
 
-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);
+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=robot.regressor.K*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
-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
+% %% 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