diff --git a/Identification_main.m b/Identification_main.m
new file mode 100644
index 0000000..da7990d
--- /dev/null
+++ b/Identification_main.m
@@ -0,0 +1,6 @@
+file = [];
+opt.robot_def = 'direct';
+opt.KM_method = 'MDH';
+robot = get_robot(file,opt);
+robot.theta = [1,0,0];
+robot = get_Kinematics(robot, opt);
diff --git a/get_Kinematics.m b/get_Kinematics.m
new file mode 100644
index 0000000..35efb6c
--- /dev/null
+++ b/get_Kinematics.m
@@ -0,0 +1,29 @@
+function robot = get_Kinematics(robot, opt)
+switch opt.KM_method
+    case 'SDH'
+    case 'MDH'
+        theta = robot.theta;
+        alpha = robot.alpha;
+        a = robot.a;
+        d = robot.d;
+        robot.Fkine = eye(4,4);
+        ndof = length(theta); % special for MDH
+        % init transform matrix
+        robot.R = zeros([3,3,ndof]);
+        robot.t = zeros([3,1,ndof]);
+        robot.T = zeros([4,4,ndof]);
+        for i = 1:ndof
+            robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;...
+                sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));...
+                sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))];
+            robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))];
+            Transform = eye(4,4);
+            Transform(1:3,1:3) = robot.R(:,:,i);
+            Transform(1:3,4) = robot.t(:,:,i);
+            robot.T(:,:,i) = Transform;
+            robot.Fkine = robot.Fkine*robot.T(:,:,i);
+        end
+    otherwise
+        disp('Bad opt.KM_method!')
+        return;
+end
\ No newline at end of file
diff --git a/get_rb_dynamics.m b/get_rb_dynamics.m
new file mode 100644
index 0000000..1bda8c5
--- /dev/null
+++ b/get_rb_dynamics.m
@@ -0,0 +1,92 @@
+function [taulist]= get_rb_dynamics(thetalist, dthetalist, ddthetalist, g,...
+                                   dh_list, mass_list, mass_center_list, inertia_tensor_list, f_tip)
+%---------变量定义-----------------
+% thetalist:6x1,关节变量，dthetalist:6x1,关节变量一阶导数, ddthetalist:6x1,关节变量二阶导数
+% g: 1x1,重力加速度
+% dh_list:6x4,modified_DH参数
+% mass_list:6x1,连杆质量，mass_center_list:6x3,连杆质心相对于坐标系{i}坐标，
+% inertia_tensor_list: 3x3x6,连杆相对于质心坐标系的惯量张量
+% f_tip: 2x3,机械臂末端施加外力和力矩
+
+%taulist:6x1,各关节所需力矩
+
+
+%R：3x3x6,旋转矩阵，P：3x6,后一连杆坐标系在前一连杆坐标系中的位置
+%w：3x6,连杆角速度，dw：3x6,连杆角加速度，dv：3x6,连杆线加速度，dvc：3x6,连杆质心线加速度
+%Ic:3x3x6,等同于inertia_tensor_list
+%Pc:3x6, mass_center_list的转置
+%F:3x6,各轴所受合力，N:3x6,各轴所受合力矩
+%f:3x6,前一轴给后一轴的力，n:3x6,前一轴给后一轴的力矩
+
+dof_num = size(dthetalist,1);
+
+alpha = dh_list(:,1);
+a = dh_list(:,2);
+d = dh_list(:,3);
+theta = dh_list(:,4);
+
+m = mass_list;
+Pc = mass_center_list';
+Ic = inertia_tensor_list;
+
+Z=[0;0;1];
+%转换矩阵建立
+
+theta = theta + thetalist;
+T=zeros(4,4,dof_num);R=zeros(3,3,dof_num);P=zeros(3,dof_num);
+for i=1:dof_num
+    T(:,:,i)=[cos(theta(i))                   -sin(theta(i))                0                  a(i)
+              sin(theta(i))*cos(alpha(i))  cos(theta(i))*cos(alpha(i))  -sin(alpha(i))   -d(i)*sin(alpha(i))
+              sin(theta(i))*sin(alpha(i))  cos(theta(i))*sin(alpha(i))  cos(alpha(i))    d(i)*cos(alpha(i))
+              0                              0                              0                  1];
+    R(:,:,i)=T(1:3,1:3,i);
+    P(:,i)=T(1:3,4,i);
+end
+
+TT = eye(4,4);
+for i = 1:dof_num
+    TT = TT*T(:,:,i);
+end
+
+%运动学正向递推
+w0 = zeros(3,1); dw0 = zeros(3,1);
+dv0 = [0;0;g];
+w = zeros(3,dof_num); dw = zeros(3,dof_num);
+dv = zeros(3,dof_num); dvc = zeros(3,dof_num);
+F = zeros(3,dof_num); N = zeros(3,dof_num);
+
+%i = 0
+w(:,1) = R(:,:,1)' * w0 + dthetalist(1) * Z;
+dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dthetalist(1) * Z) + ddthetalist(1) * Z;
+dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
+dvc(:,1) = cross(dw(:,1), Pc(:,1))+cross(w(:,1), cross(w(:,1), Pc(:,1))) + dv(:,1);
+for i = 1:dof_num-1
+   w(:,i+1) = R(:,:,i+1)' * w(:,i) + dthetalist(i+1) * Z ;
+   dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dthetalist(i+1) * Z)+ ddthetalist(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));
+   dvc(:,i+1) = cross(dw(:,i+1), Pc(:,i+1)) + cross(w(:,i+1), cross(w(:,i+1), Pc(:,i+1))) + dv(:,i+1);
+end
+
+for i = 1:dof_num
+   F(:,i)=m(i)*dvc(:,i) ;
+   N(:,i)=Ic(:,:,i) * dw(:,i) + cross(w(:,i), Ic(:,:,i) * w(:,i));
+end
+
+%动力学逆向递推
+%先计算杆6的力和力矩
+taulist = zeros(dof_num,1);
+f=zeros(3,dof_num); n=zeros(3,dof_num);
+
+f(:,dof_num) = F(:,dof_num) + f_tip(1,:)';
+n(:,dof_num) = N(:,dof_num) + f_tip(2,:)' + cross(Pc(:,dof_num), F(:,dof_num));
+taulist(dof_num) = n(:,dof_num)' * Z;
+%再计算杆5到1的力和力矩
+for i=dof_num-1:-1:1
+   f(:,i) = R(:,:,i+1) * f(:,i+1) + F(:,i);
+   n(:,i) = N(:,i) + R(:,:,i+1) * n(:,i+1) + cross(Pc(:,i), F(:,i))...
+            + cross(P(:,i+1), R(:,:,i+1) * f(:,i+1));
+   taulist(i) = n(:,i)' * Z;
+   
+end     
+
+end
\ No newline at end of file
diff --git a/get_rne_mdh.m b/get_rne_mdh.m
new file mode 100644
index 0000000..c26fbac
--- /dev/null
+++ b/get_rne_mdh.m
@@ -0,0 +1,188 @@
+function tau = get_rne_mdh(robot, a1, a2, a3, a4, a5)
+
+	z0 = [0;0;1];
+	grav = robot.gravity;	% default gravity from the object
+	fext = zeros(6, 1);
+
+	% Set debug to:
+	%	0 no messages
+	%	1 display results of forward and backward recursions
+	%	2 display print R and p*
+	debug = 0;
+
+	n = robot.n;
+	if numcols(a1) == 3*n
+		Q = a1(:,1:n);
+		Qd = a1(:,n+1:2*n);
+		Qdd = a1(:,2*n+1:3*n);
+		np = numrows(Q);
+		if nargin >= 3,	
+			grav = a2(:);
+		end
+		if nargin == 4
+			fext = a3;
+		end
+	else
+		np = numrows(a1);
+		Q = a1;
+		Qd = a2;
+		Qdd = a3;
+		if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ...
+			numrows(Qd) ~= np || numrows(Qdd) ~= np
+			error('bad data');
+		end
+		if nargin >= 5,	
+			grav = a4(:);
+		end
+		if nargin == 6
+			fext = a5;
+		end
+    end
+	
+    if robot.issym || any([isa(Q,'sym'), isa(Qd,'sym'), isa(Qdd,'sym')])
+        tau = zeros(np,n, 'sym');
+    else
+        tau = zeros(np,n);
+    end
+
+	for p=1:np
+		q = Q(p,:).';
+		qd = Qd(p,:).';
+		qdd = Qdd(p,:).';
+	
+		Fm = [];
+		Nm = [];
+		pstarm = [];
+		Rm = [];
+		w = zeros(3,1);
+		wd = zeros(3,1);
+		vd = grav(:);
+
+	%
+	% init some variables, compute the link rotation matrices
+	%
+		for j=1:n
+			link = robot.links(j);
+			Tj = link.A(q(j));
+            switch link.type
+                case 'R'
+                    D = link.d;
+                case 'P'
+                    D = q(j);
+            end
+			alpha = link.alpha;
+			pm = [link.a; -D*sin(alpha); D*cos(alpha)];	% (i-1) P i
+			if j == 1
+				pm = t2r(robot.base) * pm;
+				Tj = robot.base * Tj;
+			end
+			Pm(:,j) = pm;
+			Rm{j} = t2r(Tj);
+			if debug>1
+				Rm{j}
+				Pm(:,j).'
+			end
+		end
+
+	%
+	%  the forward recursion
+	%
+		for j=1:n
+			link = robot.links(j);
+
+			R = Rm{j}.';	% transpose!!
+			P = Pm(:,j);
+            Pc = link.r;
+            
+            %
+            % trailing underscore means new value
+            %
+            switch link.type
+                case 'R'
+                    % revolute axis
+                    w_ = R*w + z0*qd(j);
+                    wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j);
+                    %v = cross(w,P) + R*v;
+                    vd_ = R * (cross(wd,P) + ...
+                        cross(w, cross(w,P)) + vd);
+                    
+                case 'P'
+                    % prismatic axis
+                    w_ = R*w;
+                    wd_ = R*wd;
+                    %v = R*(z0*qd(j) + v) + cross(w,P);
+                    vd_ = R*(cross(wd,P) + ...
+                        cross(w, cross(w,P)) + vd ...
+                        ) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j);
+            end
+			% update variables
+			w = w_;
+			wd = wd_;
+			vd = vd_;
+
+			vdC = cross(wd,Pc).' + ...
+				cross(w,cross(w,Pc)).' + vd;
+			F = link.m*vdC;
+			N = link.I*wd + cross(w,link.I*w);
+			Fm = [Fm F];
+			Nm = [Nm N];
+			if debug
+				fprintf('w: '); fprintf('%.3f ', w)
+				fprintf('\nwd: '); fprintf('%.3f ', wd)
+				fprintf('\nvd: '); fprintf('%.3f ', vd)
+				fprintf('\nvdbar: '); fprintf('%.3f ', vdC)
+				fprintf('\n');
+			end
+		end
+
+	%
+	%  the backward recursion
+	%
+
+		fext = fext(:);
+		f = fext(1:3);		% force/moments on end of arm
+		nn = fext(4:6);
+
+		for j=n:-1:1
+			
+			%
+			% order of these statements is important, since both
+			% nn and f are functions of previous f.
+			%
+			link = robot.links(j);
+			
+			if j == n
+				R = eye(3,3);
+				P = [0;0;0];
+			else
+				R = Rm{j+1};
+				P = Pm(:,j+1);		% i/P/(i+1)
+			end
+			Pc = link.r;
+			
+			f_ = R*f + Fm(:,j);
+			nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)).' + ...
+				cross(P,R*f);
+			
+			f = f_;
+			nn = nn_;
+
+			if debug
+				fprintf('f: '); fprintf('%.3f ', f)
+				fprintf('\nn: '); fprintf('%.3f ', nn)
+				fprintf('\n');
+			end
+            switch link.type
+                case 'R'
+                    % revolute
+                    tau(p,j) = nn.'*z0 + ...
+                        link.G^2 * link.Jm*qdd(j) - ...
+                        friction(link, qd(j));
+                case 'P'
+                    % prismatic
+                    tau(p,j) = f.'*z0 + ...
+                        link.G^2 * link.Jm*qdd(j) - ...
+                        friction(link, qd(j));
+            end
+		end
+	end
\ No newline at end of file
diff --git a/get_robot.m b/get_robot.m
new file mode 100644
index 0000000..325b46f
--- /dev/null
+++ b/get_robot.m
@@ -0,0 +1,50 @@
+function robot = get_robot(file,opt)
+switch opt.robot_def
+    case 'direct'
+        ndof = 2;
+        % Kinematics parameters
+        switch opt.KM_method
+            case 'SDH'
+            case 'MDH'
+                robot.theta = zeros([1,ndof+1]);
+                robot.a = [0,1,1];
+                robot.d = [0,0,0];
+                robot.alpha = [0,0,0];
+            otherwise
+                disp('Bad opt.KM_method!')
+                return;
+        end
+        % Dynamics parameters
+        link_mass = [1,1];
+        axis_of_rot(:,:,1) = [0;0;1];
+        axis_of_rot(:,:,2) = [0;0;1];
+        com_pos(:,:,1) = [1/2;0;0];
+        com_pos(:,:,2) = [1/2;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]);
+        link_inertia(:,:,2) = 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);
+            % 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-...
+                link_mass(i)*com_vec2mat*com_vec2mat);
+            robot.pi(:,i) = [robot.I_vec(:,i); robot.mc(:,i); robot.m(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
\ No newline at end of file
diff --git a/test_kinematics.m b/test_kinematics.m
new file mode 100644
index 0000000..1d0a8b9
--- /dev/null
+++ b/test_kinematics.m
@@ -0,0 +1,28 @@
+mdh = 1;
+if mdh==1
+    %       theta    d        a        alpha     offset
+    L1=Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,40]),'modified');
+    L2=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'modified');
+    L3=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'modified');
+    Two_bar=SerialLink([L1 L2 L3],'name','Two_bar'); %连接连杆
+    Two_bar.teach();
+%     Two_bar.plot([0 0])%机械臂图
+else
+    %       theta    d        a        alpha     offset
+    L1=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,40]),'standard');
+    L2=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'standard');
+    Two_bar=SerialLink([L1 L2],'name','Two_bar'); %连接连杆
+%     Two_bar.plot([0 0])%机械臂图
+    Two_bar.teach();
+end
+
+
+% a = [0, -0.42500, -0.39225, 0, 0, 0];
+% d = [0.0892, 0, 0, 0.10915, 0.09465, 0.0823];
+% alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0];
+% for i = 1:6
+%     L(i)=Link([0 d(i) a(i) alpha(i)]);
+%     L(i).qlim=[-2*pi,2*pi];
+% end
+% UR5=SerialLink(L,'name','UR5');
+% UR5.teach();
\ No newline at end of file