Identification_main.m

@@ -12,7 +12,7 @@ opt.Isreal = true;
 robot = get_robot_R1000(file,opt);
 robot = get_Kinematics(robot, opt);
 
-% R1000_Dynamics_num;
+R1000_Dynamics_num;
 % R1000_Dynamics;
 % getGravityForce;
 robot = get_velocity(robot, opt);

get_regressor.m

@@ -43,6 +43,8 @@ switch opt.LD_method
                     % size of matrix A is 6*10, need to -1
                     robot.regressor.A(:,:,i) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
                         zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
+                    %? match screw method
+                    robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
                 end
                 % construct matrix U, size: [6*n,10*n]
                 % U_ = sym(zeros([6*ndof,10*ndof]));

get_velocity.asv

@@ -0,0 +1,106 @@
+function robot = get_velocity(robot, opt)
+switch opt.KM_method
+    case 'SCREW'
+        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);
+            % ?
+            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
+        % 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=[zeros(2,robot.ndof);robot.gravity(end)*ones(1,robot.ndof)];
+    case 'SDH'
+    case 'MDH'
+        switch opt.Vel_method
+            case 'Direct'
+                Z = [0,0,1]';
+                w0 = zeros(3,1); dw0 = zeros(3,1);
+%                 dv0 = robot.gravity;
+                v0 = zeros(3,1);dv0 = zeros(3,1);
+                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外推公式 参考robotics toolbox
+                %第一关节
+                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
+                    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;
+                robot.vel.dw = dw;
+                robot.vel.dv = dv;
+            otherwise
+                disp('Bad opt.Vel_method!')
+                return;
+        end
+    otherwise
+        disp('Bad opt.KM_method!')
+        return;
+end

get_velocity.m

@@ -31,11 +31,12 @@ switch opt.KM_method
                 robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
         end
         % FIXME: twist is not equal to velocity
+        % FIXME: Need to get the velocity represent at base frame
         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=[zeros(2,robot.ndof);robot.gravity(end)*ones(1,robot.ndof)];
+        robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
     case 'SDH'
     case 'MDH'
         switch opt.Vel_method