From 6883fcc8fb86fd84a6a33ac4c7bbce48b6779bec Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Mon, 23 Sep 2024 02:57:34 +0800
Subject: [PATCH] seems can't generate symbol solution

---
 Identification_main.m |  2 +-
 R1000_Dynamics.m      | 77 ++++++++++++++++++++++---------------------
 2 files changed, 41 insertions(+), 38 deletions(-)

diff --git a/Identification_main.m b/Identification_main.m
index 06d078f..166d17c 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -13,7 +13,7 @@ robot = get_robot_R1000(file,opt);
 robot = get_Kinematics(robot, opt);
 %TODO verify kinematics via robotics toolbox or other software result
 
-R1000_Dynamics_num;
+R1000_Dynamics;
 % opt.Isreal = false;
 % robot = get_velocity(robot, opt);
 % robot = get_regressor(robot,opt);
diff --git a/R1000_Dynamics.m b/R1000_Dynamics.m
index 9977faf..9eed539 100644
--- a/R1000_Dynamics.m
+++ b/R1000_Dynamics.m
@@ -1,18 +1,16 @@
 %% R1000
 N=9;
+% traj
+time = 0:0.01:2;
+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));
 % Dynamics parameters
 link_mass = robot.m;
 com_pos = robot.com;
-% link_inertia = robot.I;
-link_inertia(:,:,1) = diag([1,1,1]);
-link_inertia(:,:,2) = diag([1,1,1]);
-link_inertia(:,:,3) = diag([1,1,1]);
-link_inertia(:,:,4) = diag([1,1,1]);
-link_inertia(:,:,5) = diag([1,1,1]);
-link_inertia(:,:,6) = diag([1,1,1]);
-link_inertia(:,:,7) = diag([1,1,1]);
-link_inertia(:,:,8) = diag([1,1,1]);
-link_inertia(:,:,9) = diag([1,1,1]);
+link_inertia = robot.I;
 
 q_sym = sym('q%d',[N,1],'real');
 qd_sym = sym('qd%d',[N,1],'real');
@@ -29,15 +27,29 @@ for i = 1:N
 end
 
 % Get the com pos transformation in each joint reference frame
-Mlist = [];
-for i = 1:N
-    M = robot.T(:,:,i)+transl(com_pos(i));
-    Mlist = cat(3, Mlist, M);
+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 = cat(3, Mlist, M);
+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=[[0;0;1;0;0;0],...
     [0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
     [0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
@@ -46,7 +58,7 @@ Slist=[[0;0;1;0;0;0],...
     [1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
     [0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
     [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
-    [0;0;0;cross(-[0;0;0],[0.2+0.5+0.45+0.12+0.28-0.3157;0;-0.4-0.126493])]];
+    [0;0;0;1;0;0]];
 
 Vlinear=sym(zeros(3,3));
 J=sym(zeros(6,N));
@@ -54,32 +66,23 @@ exf=[0;0;0;0;0;0];
 
 [V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
     = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
-    [0;0;-9.8], exf, Mlist, Glist, Slist);
-G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
-
-% Get the end efforce transformation in each joint reference frame
-Mlist = [];
-for i = 1:N+1
-    M = robot.T(:,:,i);
-    Mlist = cat(3, Mlist, M);
-end
-M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-Mlist = cat(3, Mlist, M);
-
-T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
-F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
+    [0;0;-9.8], exf, Mlist_CG, Glist, Slist);
+% G = FKinSpaceExpand_Sym(Mlist_CG, Slist, thetalist);
+% T=FKinSpaceExpand_Sym(Mlist_ED, Slist, thetalist);
+% F_Simpack = getSimpackF_Sym(G,T,Mlist_ED,Fmat);
 
 %Gen Files
-matlabFunction(F_Simpack,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
+matlabFunction(Fmat,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
     'Vars',{q_sym,qd_sym,q2d_sym});
 standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName);
 % traj
-time = 0:1:2;
-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);
-F_Simpack_list = feval(standard_dynamics_func, q_J',qd_J',qdd_J');
+% time = 0:1:2;
+% 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);
+% F_Simpack_list = feval(standard_dynamics_func, q_J',qd_J',qdd_J');
+
 
 % Use Body Twist cal linear vel, but can't cal the end frame vel
 % [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...