From 3f327f27df4e56c1a9659a353797d18520ecd997 Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Mon, 16 Dec 2024 23:21:27 +0800
Subject: [PATCH] add currentSensor identification

---
 get_GCTraj_R1000_DVT.m | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/get_GCTraj_R1000_DVT.m b/get_GCTraj_R1000_DVT.m
index 64cceee..49106c3 100644
--- a/get_GCTraj_R1000_DVT.m
+++ b/get_GCTraj_R1000_DVT.m
@@ -2,8 +2,9 @@
 gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
 motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
-load("C:\GitLab\gtools\development_files\SLX_Development\R1000\SA System\lab12.mat");
+load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\lab12.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
+isCurrentSensor = true;
 % J9 traj
 i = 9;
 fileData=eval(strcat('fileData', num2str(i)));
@@ -33,7 +34,11 @@ for i=8:-1:2
     data = fileData.data;
     dataLength = length(data);
     d1Length = floor(length(data) / 2);
-    idntfcnTrjctry(i).tau = data(:,6*(i+1)+11*3)*sensorDir(i);
+    if(isCurrentSensor)
+        idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i);
+    else
+        idntfcnTrjctry(i).tau = data(:,6*(i+1)+11*3)*sensorDir(i);
+    end
     idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';
     q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
     for j=1:robot.ndof