diff --git a/Identification_main.m b/Identification_main.m
index 93e4a6a..06d078f 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -1,19 +1,22 @@
+close all;clc;clear
 file = [];
 opt.robot_def = 'direct';
 opt.KM_method = 'MDH';
 opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
 opt.debug = true;
-opt.robotName = 'Two_bar';
+opt.robotName = 'R1000';
 
-opt.Isreal = false;
+opt.Isreal = true;
 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
 
-opt.Isreal = false;
-robot = get_velocity(robot, opt);
-robot = get_regressor(robot,opt);
+R1000_Dynamics_num;
+% opt.Isreal = false;
+% 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
+% robot = get_baseParams(robot, opt);
\ No newline at end of file
diff --git a/R1000_Dynamics.m b/R1000_Dynamics.m
new file mode 100644
index 0000000..9977faf
--- /dev/null
+++ b/R1000_Dynamics.m
@@ -0,0 +1,90 @@
+%% R1000
+N=9;
+% Dynamics parameters
+link_mass = robot.m;
+com_pos = robot.com;
+% link_inertia = robot.I;
+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]);
+
+q_sym = sym('q%d',[N,1],'real');
+qd_sym = sym('qd%d',[N,1],'real');
+q2d_sym = sym('qdd%d',[N,1],'real');
+thetalist = qd_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
+
+% Get the com pos transformation in each joint reference frame
+Mlist = [];
+for i = 1:N
+    M = robot.T(:,:,i)+transl(com_pos(i));
+    Mlist = cat(3, Mlist, M);
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist = cat(3, Mlist, M);
+
+%TODO: Get Slist form DH table method
+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;cross(-[0;0;0],[0.2+0.5+0.45+0.12+0.28-0.3157;0;-0.4-0.126493])]];
+
+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.8], exf, Mlist, Glist, Slist);
+G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
+
+% Get the end efforce transformation in each joint reference frame
+Mlist = [];
+for i = 1:N+1
+    M = robot.T(:,:,i);
+    Mlist = cat(3, Mlist, M);
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist = cat(3, Mlist, M);
+
+T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
+F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
+
+%Gen Files
+matlabFunction(F_Simpack,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
+    'Vars',{q_sym,qd_sym,q2d_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..810105b
--- /dev/null
+++ b/R1000_Dynamics_num.m
@@ -0,0 +1,88 @@
+%% R1000
+N=9;
+% traj
+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);
+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_GC = [];
+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_GC = cat(3, Mlist_GC, M);
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_GC = cat(3, Mlist_GC, 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)] ...
+        = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
+        [0;0;-9.8], exf, Mlist_GC, Glist, Slist);
+    G(:,:,:,i) = FKinSpaceExpand(Mlist_GC, Slist, thetalist(i,:)');
+    T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
+    F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
+end
+% plot Torque
+F_Simpack = pagetranspose(F_Simpack);
+figure(1)
+for i = 1:3
+    subplot(3,1,i);
+    hold on;
+    plot(time,-reshape(F_Simpack(1,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/get_robot_R1000.m b/get_robot_R1000.m
index cf76a75..9c8a038 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -1,17 +1,17 @@
 function robot = get_robot_R1000(file,opt)
 switch opt.robot_def
     case 'direct'
-        ndof = 8;
+        ndof = 9;
         robot.ndof = ndof;
         % Kinematics parameters
         if(opt.Isreal)
             switch opt.KM_method
                 case 'SDH'
                 case 'MDH'
-                    robot.theta = zeros([1,ndof]);
-                    robot.a = [0,1,1];
-                    robot.d = [0,0,0];
-                    robot.alpha = [0,0,0];
+                    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.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];
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;
@@ -27,37 +27,51 @@ switch opt.robot_def
             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];
-                    robot.d = [0,0,0,0,0,0.28,0.40,0];
-                    robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2];
+                    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];
+                    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(ndof)=q_sym;
+            %init vd
+            robot.vd = robot.d; 
+            robot.vd(ndof)=q_sym;
+            %init accd
+            robot.accd = robot.d;
+            robot.accd(ndof)=q_sym;
         end
         % Dynamics parameters
-        link_mass = [1,1,1,1,1,1,1,1];
+        link_mass = [17.42,7.7,2.42,5.16,2.22,1.78,2.32,2.92 0.1];
         %TODO in process, seems axis_of_rot useless 
-        axis_of_rot(:,:,1) = [0;0;1];
-        axis_of_rot(:,:,2) = [0;0;1];
-        axis_of_rot(:,:,3) = [0;0;1];
-        axis_of_rot(:,:,4) = [0;0;1];
-        axis_of_rot(:,:,5) = [0;0;1];
-        axis_of_rot(:,:,6) = [0;0;1];
-        axis_of_rot(:,:,7) = [0;0;1];
-        axis_of_rot(:,:,8) = [0;0;1];
+        axis_of_rot(:,1) = [0;0;1];
+        axis_of_rot(:,2) = [0;0;1];
+        axis_of_rot(:,3) = [0;0;1];
+        axis_of_rot(:,4) = [0;0;1];
+        axis_of_rot(:,5) = [0;0;1];
+        axis_of_rot(:,6) = [0;0;1];
+        axis_of_rot(:,7) = [0;0;1];
+        axis_of_rot(:,8) = [0;0;1];
+        axis_of_rot(:,9) = [0;0;1];
         % 画图
-        com_pos(:,:,1) = [0;0;0];
-        com_pos(:,:,2) = [0.2/2;0;0];
-        com_pos(:,:,3) = [0.5/2;0;0];
-        com_pos(:,:,4) = [0.45/2;0;0];
-        com_pos(:,:,5) = [0.12/2;0;0];
-        com_pos(:,:,6) = [0.28/2;0;0];
-        com_pos(:,:,7) = [0.4/2;0;0];
-        com_pos(:,:,8) = [0.2772/2;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];
         % 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]);
@@ -68,19 +82,20 @@ switch opt.robot_def
         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]);
         % 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*com_pos(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);
-            robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
+            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.I_vec(:,i); robot.mc(:,i); robot.m(i)];
+            robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)];
         end
     case 'urdf'
         robot = parse_urdf(file);
diff --git a/get_velocity.asv b/get_velocity.asv
deleted file mode 100644
index bb70b5f..0000000
--- a/get_velocity.asv
+++ /dev/null
@@ -1,31 +0,0 @@
-function robot = get_velocity(robot, opt)
-switch opt.Vel_method
-    case 'Direct'
-        Z = [0,0,1]';
-        w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity;
-        % w = zeros(3,number);
-        % dw = zeros(3,number);
-        % dv = zeros(3,number);
-        theta  = robot.theta;
-        dtheta = robot.dtheta;
-        ddtheta = robot.ddtheta;
-        R = robot.R;
-        P = robot.t;
-        % 1-n外推公式
-        %第一关节
-        w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z;
-        dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z;
-        dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
-        %后面n-1关节
-        for i = 1:robot.ndof
-            w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ;
-            dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z;
-            dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
-        end
-        robot.vel.w = w;
-        robot.vel.dw = dw;
-        robot.vdv = dv;
-    otherwise
-        disp('Bad opt.KM_method!')
-        return;
-end
\ No newline at end of file
diff --git a/get_velocity.m b/get_velocity.m
index 8281c5a..b8b8703 100644
--- a/get_velocity.m
+++ b/get_velocity.m
@@ -8,26 +8,53 @@ switch opt.KM_method
                 w0 = zeros(3,1); dw0 = zeros(3,1);
 %                 dv0 = robot.gravity;
                 v0 = zeros(3,1);dv0 = zeros(3,1);
-                % w = zeros(3,number);
-                % dw = zeros(3,number);
-                % dv = zeros(3,number);
-                theta  = robot.theta;
-                dtheta = robot.dtheta;
-                ddtheta = robot.ddtheta;
+                link_type = robot.link_type;
+                % init q
+                q = robot.theta;
+                qd = robot.dtheta;
+                qdd = robot.ddtheta;   
+                for i = 1:robot.ndof
+                    switch link_type(i)
+                        case 'R'
+                            %Do nothing
+                        case 'P'
+                            q(i) = robot.d(i);
+                            qd(i) = robot.vd(i);
+                            qdd(i) = robot.accd(i);
+                    end
+                end
                 R = robot.R;
                 P = robot.t;
-                % 1-n外推公式
+                % 1-n外推公式 参考robotics toolbox
                 %第一关节
-                w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z;
-                v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
-                dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z;
-                dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
+                switch link_type(1)
+                    case 'R' %revolute
+                        w(:,1) = R(:,:,1)' * w0 + qd(1) * Z;
+                        v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
+                        dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, qd(1) * Z) + qdd(1) * Z;
+                        dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0);
+                    case 'P' %prismatic
+                        w(:,1) = R(:,:,1)' * w0;
+                        v(:,1) = R(:,:,1)' * (Z*qd(1)+v0) + cross(w0, P(:,1));
+                        dw(:,1) = R(:,:,1)' * dw0;
+                        dv(:,1) = R(:,:,1)' * (cross(dw0, P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0)+...
+                            2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
+                end
                 %后面n-1关节
                 for i = 1:robot.ndof
-                    w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ;
-                    v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
-                    dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z;
-                    dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
+                    switch link_type(i)
+                        case 'R' %revolute
+                            w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(i+1) * Z ;
+                            v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
+                            dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), qd(i+1) * Z)+ qdd(i+1) * Z;
+                            dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
+                        case 'P' %prismatic
+                            w(:,i+1) = R(:,:,i+1)' * w(:,i);
+                            v(:,i+1) = R(:,:,i+1)' * (Z*qd(:,i)+v(:,i)) + cross(w(:,i), P(:,i+1));
+                            dw(:,i+1) = R(:,:,i+1)' * dw(:,i);
+                            dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i))+...
+                                2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
+                    end
                 end
                 robot.vel.w = w;
                 robot.vel.v = v;