match vel with standard

Identification_main.m

@@ -15,8 +15,8 @@ robot = get_Kinematics(robot, opt);
 R1000_Dynamics_num;
 % R1000_Dynamics;
 % getGravityForce;
-% robot = get_velocity(robot, opt);
+robot = get_velocity(robot, opt);
-% robot = get_regressor(robot,opt);
+robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
 % robot = get_baseParams(robot, opt);

R1000_Dynamics_num.asv

@@ -0,0 +1,180 @@
+%% R1000
+N=9;
+% traj
+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));
+q_J = ones(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_;q_J;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
+    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)
+    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_CG = [];
+% 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_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% Get the com pos transformation in each joint reference frame
+% FIXME: BUG here
+% Mlist_CG=[];
+% for i = 0:N-1
+%     if i == 0
+%         M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+%     else
+%         rotation_i = diag([1,1,1]);
+%         for j = 1:i
+%             rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
+%             rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
+%         end
+%         M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
+%     end
+%     Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% ct=[];
+% Mlist_CG=[];
+% for i = 1:N
+%     if i == 1
+%         ct(:,i) = com_pos_R1(:,i);
+%     elseif i< 9
+%         ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
+%     else
+%         ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+%     end
+%     robot.Home.com(:,i) = ct(:,i);
+%     M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
+%     Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% Mlist_CG=[];
+% for i = 1:N
+%         if i == 1
+%         M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
+%     elseif i<=8
+%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
+%     elseif i==9
+%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
+%         end
+%         Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% get the CG at the world base frame
+com_pos_R1 = robot.com_pos_R1;
+com_pos_R2 = robot.com_pos_R2;
+ct=[];
+Mlist_CG_Base=[];
+for i = 1:N
+    if i == 1
+        ct(:,i) = com_pos_R1(:,i);
+    elseif i< 9
+        ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
+    else
+        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+    end
+    robot.Home.com(:,i) = ct(:,i);
+    M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
+    Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
+end
+% get the CG at the last GC frame
+Mlist_CG=[];
+for i = 1:N
+    if i == 1
+        Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
+    else
+        Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
+    end
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_CG = cat(3, Mlist_CG, 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=robot.slist;
+
+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),taumat(:,i)] ...
+        = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
+        [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+    G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
+    T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
+    %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  
+    %why we need Mlist_ED
+    %please explain this more
+    F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
+end
+% plot Torque
+% above 2020b 
+% F_Simpack = pagetranspose(F_Simpack);
+% below 2020b
+
+for i = 1:3
+    subplot(3,1,i);
+    hold on;
+    %added minus, so should be the same as simpack
+    plot(time,taumat(i+6,:))
+    xlabel('time(s)')
+    ylabel('Torque(Nm)')
+%     plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
+end
+
+F_Simpack = permute(F_Simpack,[2 1 3]);
+figure(2)
+for i = 1:3
+    subplot(3,1,i);
+    hold on;
+    %added minus, so should be the same as simpack
+    plot(time,-reshape(F_Simpack(8,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);

R1000_Dynamics_num.m

@@ -13,13 +13,14 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;q_J;zero_;q_J;zero_;q_J;zero_;zero_;zero_]';
+thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+dthetalist = [zero_;q_J;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
+    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
     Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
     Glist = cat(3, Glist, Gb);
 end

get_regressor.m

@@ -28,6 +28,7 @@ R = robot.kine.R;
 P = robot.kine.t;
 w = robot.vel.w ;
 dw = robot.vel.dw ;
+v = robot.vel.v ;
 dv = robot.vel.dv ;
 switch opt.LD_method
     case 'Direct'
@@ -37,16 +38,14 @@ switch opt.LD_method
                     p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
                     w_skew(:,:,i) = vec2skewSymMat(w(:,i));
                     dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
+                    v_skew(:,:,i) = vec2skewSymMat(v(:,i));
                     dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
                     w_l(:,:,i) = vec2linearSymMat(w(:,i));
                     dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
-                    % 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.Home.R(:,:,i)'*robot.pi(2:4,i);
                     % 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); ...
+                    % Traversaro el 2015
-                        zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
+                    robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
+                        zeros(3,1),-(dv_skew(:,:,i)+w_skew(:,:,i)*v_skew(:,:,i)-v_skew(:,:,i)*w_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

get_robot_R1000.m

@@ -120,6 +120,7 @@ switch opt.robot_def
             %FIXME: only consider the theta temply
             robot.dtheta = zeros([ndof,1]);
             robot.ddtheta = zeros([ndof,1]);
+            robot.dtheta(2) = 1;
             robot.link_type = ['R','R','R','R','R','R','R','R','P'];
             switch opt.KM_method
                 case 'SDH'

get_velocity.asv

@@ -0,0 +1,109 @@
+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
+        % FIXME: Need to get the velocity represent at base frame
+        robot.vel.w = V(1:3,2:end);
+        for i = i:robot.ndof
+        robot.vel.v = V(4:6,i+1)+robot.vel.w*robot.kine.t();
+        end
+        robot.vel.dw = Vd(1:3,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'
+    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);
+                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.kine.R;
+                P = robot.kine.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-1
+%                     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

@@ -12,8 +12,17 @@ switch opt.KM_method
             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);
+            %             mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
-%             robot.pi(2:4,i) = mc;
+            %             robot.pi(2:4,i) = mc;
+            % 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.Home.R(:,:,i)'*robot.pi(2:4,i);
+            com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
+            robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
+            robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
+            robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
+            robot.pi(5:end,i) = robot.I_vec(:,i);
         end
         if opt.Isreal
             % init q
@@ -32,9 +41,12 @@ switch opt.KM_method
         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.w = V(1:3,2:end);
-        robot.vel.v = V(4:6,:);
+        for i = 1:robot.ndof
-        robot.vel.dw = Vd(1:3,:);
+            robot.vel.v(:,i) = V(4:6,i+1)-vec2skewSymMat(robot.vel.w(:,i))*robot.pi(2:4,i)/robot.m(i);
+        end
+%         robot.vel.v = V(4:6,2:end);
+        robot.vel.dw = Vd(1:3,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'

untitled3.m

@@ -15,4 +15,5 @@ F_Simpack(end-2,:,1)
 robot_pi_vecoter = reshape(robot.pi,[90,1]);
 F = robot.regressor.U*robot_pi_vecoter;
 FF = reshape(F,[6,9])
+%%
 F_Simpack(:,:,1)