Identification_main.asv

@@ -1,22 +0,0 @@
-close all;clc;clear
-file = [];
-opt.robot_def = 'direct';
-opt.KM_method = 'SCREW';
-opt.Vel_method = 'Direct';
-opt.LD_method = 'Direct';
-opt.debug = true;
-opt.robotName = 'R1000';
-opt.reGenerate = false;
-opt.Isreal = true;
-
-robot = get_robot_R1000(file,opt);
-robot = get_Kinematics(robot, opt);
-
-R1000_Dynamics_num;
-% R1000_Dynamics;
-% getGravityForce;
-robot = get_velocity(robot, opt);
-robot = get_regressor(robot,opt);
-% symbol matched
-% verify_regressor_R1000;
-% robot = get_baseParams(robot, opt);

R1000_Dynamics_num.m

@@ -14,8 +14,8 @@ com_pos = robot.com;
 link_inertia = robot.I;
 
 thetalist = [zero_;zero_;q_J;q_J;zero_;zero_;zero_;zero_;zero_]';
-dthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
-ddthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
+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=[];

get_Kinematics.m

@@ -60,16 +60,6 @@ if(opt.Isreal)
             M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
             Mlist_CG = cat(3, Mlist_CG, M);
             robot.kine.Mlist_CG = Mlist_CG;
-
-            % Get the end efforce transformation in each joint reference frame
-            Mlist_ED = [];
-            for i = 1:length(thetalist)
-                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);
-            robot.kine.Mlist_ED = Mlist_ED;
         case 'SDH'
             theta = robot.theta;
             alpha = robot.alpha;

get_robot_R1000.m

@@ -118,8 +118,13 @@ switch opt.robot_def
         % ------------------------------------------------------------------------
         if(opt.Isreal)
             %FIXME: only consider the theta temply
-            robot.dtheta = 1000*pi/4*ones([ndof,1]);
-            robot.ddtheta = 1000*pi/4*ones([ndof,1]);  
+            robot.dtheta = zeros([ndof,1]);
+            robot.ddtheta = zeros([ndof,1]);
+%             robot.theta(end-1) = pi/4;
+%             robot.dtheta(end-2) = pi/4;
+%             robot.ddtheta(end-1) = 100*pi/4;
+%             robot.ddtheta(end-2) = 100*pi/4;
+%             robot.ddtheta(end-3) = 100*pi/4;
             robot.link_type = ['R','R','R','R','R','R','R','R','P'];
             switch opt.KM_method
                 case 'SDH'
@@ -153,8 +158,8 @@ switch opt.robot_def
                         [0;0;0;1;0;0]];
                     robot.theta = zeros([ndof,1]);
 %                     robot.theta(end-1) = pi/4;
-%                     robot.theta(3) = pi/4;
-%                     robot.theta(4) = pi/4;
+                    robot.theta(3) = pi/4;
+                    robot.theta(4) = pi/4;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;

get_velocity.m

@@ -17,7 +17,7 @@ switch opt.KM_method
             % hack: because we only know the com in the world frame
             % the inv of rotation is beacause we want to the com in
             % our defined frame. R*com is the wrong result
-            robot.pi(2:4,i) = robot.m(i)*robot.Home.R(:,:,i)'*robot.com(:,i);
+            robot.pi(2:4,i) = robot.Home.R(:,:,i)'*robot.pi(2:4,i);
             com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
             % get joint inertial
             robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
@@ -45,12 +45,11 @@ switch opt.KM_method
         robot.vel.w = V(1:3,2:end);
         robot.vel.dw = Vd(1:3,2:end);
         for i = 1:robot.ndof
-            robot.vel.v(:,i) = V(4:6,i+1);
+            robot.vel.v(:,i) = V(4:6,i+1) - vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i);
             robot.vel.dv(:,i) = Vd(4:6,i+1)- vec2skewSymMat(robot.vel.dw(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i)...
                 - vec2skewSymMat(robot.vel.w(:,i))*(vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i));
         end
 %         robot.vel.v = V(4:6,2:end);
-%         robot.vel.dv = Vd(4:6,2:end);
 
 %         robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
     case 'SDH'