From 2cdaef06194d1ef5ef5b54e4ec6f2ec866476889 Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Mon, 21 Oct 2024 00:58:54 +0800
Subject: [PATCH] regressor work but don't know why

---
 Identification_main.asv | 22 ++++++++++++++++++++++
 R1000_Dynamics_num.m    |  6 +++---
 get_Kinematics.m        | 10 ++++++++++
 get_robot_R1000.m       |  4 ++--
 get_velocity.m          |  9 +++++----
 5 files changed, 42 insertions(+), 9 deletions(-)
 create mode 100644 Identification_main.asv

diff --git a/Identification_main.asv b/Identification_main.asv
new file mode 100644
index 0000000..f95dfe1
--- /dev/null
+++ b/Identification_main.asv
@@ -0,0 +1,22 @@
+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);
diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index f48d273..33a9fd7 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -13,9 +13,9 @@ 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 = [q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
+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]';
 
 % Get general mass matrix
 Glist=[];
diff --git a/get_Kinematics.m b/get_Kinematics.m
index ad6c42f..8965b3e 100644
--- a/get_Kinematics.m
+++ b/get_Kinematics.m
@@ -60,6 +60,16 @@ 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;
diff --git a/get_robot_R1000.m b/get_robot_R1000.m
index 98fe59e..6d776a8 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -118,8 +118,8 @@ switch opt.robot_def
         % ------------------------------------------------------------------------
         if(opt.Isreal)
             %FIXME: only consider the theta temply
-            robot.dtheta = zeros([ndof,1]);
-            robot.ddtheta = pi/4*ones([ndof,1]);
+            robot.dtheta = 1000*pi/4*ones([ndof,1]);
+            robot.ddtheta = 1000*pi/4*ones([ndof,1]);  
             robot.link_type = ['R','R','R','R','R','R','R','R','P'];
             switch opt.KM_method
                 case 'SDH'
diff --git a/get_velocity.m b/get_velocity.m
index f069b3c..bba1201 100644
--- a/get_velocity.m
+++ b/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.Home.R(:,:,i)'*robot.pi(2:4,i);
+            robot.pi(2:4,i) = robot.m(i)*robot.Home.R(:,:,i)'*robot.com(:,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,11 +45,12 @@ 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) - 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));
+            robot.vel.v(:,i) = V(4:6,i+1);
+            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'