From e2e7406f4a64c2adf2150da310cf9f544dc8adfc Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Sun, 20 Oct 2024 01:39:18 +0800
Subject: [PATCH] fix bug when match regressor and dynamics

---
 Identification_main.m |  4 ++--
 R1000_Dynamics_num.m  |  2 +-
 get_regressor.m       |  2 +-
 get_robot_R1000.m     |  2 +-
 untitled3.m           | 18 ++++++++++++++++++
 5 files changed, 23 insertions(+), 5 deletions(-)
 create mode 100644 untitled3.m

diff --git a/Identification_main.m b/Identification_main.m
index f95dfe1..3db19f1 100644
--- a/Identification_main.m
+++ b/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_regressor(robot,opt);
+% 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 d9a898b..be29c92 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -13,7 +13,7 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+thetalist = [zero_;q_J;zero_;q_J;zero_;q_J;zero_;zero_;zero_]';
 dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 
diff --git a/get_regressor.m b/get_regressor.m
index 66bde4f..36de173 100644
--- a/get_regressor.m
+++ b/get_regressor.m
@@ -43,7 +43,7 @@ switch opt.LD_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.TW(1:3,1:3,i)'*robot.pi(2:4,i);
+                    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); ...
                         zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
diff --git a/get_robot_R1000.m b/get_robot_R1000.m
index 7876683..a892fcc 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -152,7 +152,7 @@ switch opt.robot_def
                         [0;-1;0;cross(-[0;-1;0],co(:,8))]...
                         [0;0;0;1;0;0]];
                     robot.theta = zeros([ndof,1]);
-%                     robot.theta(2) = 1;
+%                     robot.theta(2) = pi/4;
 %                     robot.theta(4) = 1;
 %                     robot.theta(6) = 1;
                 otherwise
diff --git a/untitled3.m b/untitled3.m
new file mode 100644
index 0000000..0feee9c
--- /dev/null
+++ b/untitled3.m
@@ -0,0 +1,18 @@
+R = robot.kine.R;
+P = robot.kine.t;
+F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
+F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1
+
+FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
+FF2 = F_Simpack(end-1,:,1)'+FF1
+%%
+F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
+F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
+%%
+F_Simpack(end,:,1)
+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)
\ No newline at end of file