From 4c0ac1ccae9b3d48d7b8be60b7040a246046a672 Mon Sep 17 00:00:00 2001
From: cosmic_power <rui.cao@csrbtx.com>
Date: Wed, 11 Jun 2025 18:17:20 +0800
Subject: [PATCH] test grab

---
 Identification_main.m |  4 ++--
 grab_main.m           |  2 +-
 test_grab2.m          | 17 +++++++++--------
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/Identification_main.m b/Identification_main.m
index 4b79760..fbc82f1 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -30,5 +30,5 @@ robot = get_regressor(robot,opt);
 % % verify_regressor_R1000;
 robot = get_baseParams(robot, opt);
 % robot = estimate_dyn(robot,opt);
-% robot = estimate_dyn_form_data(robot,opt);
-robot = estimate_dyn_MLS(robot,opt);
\ No newline at end of file
+robot = estimate_dyn_form_data(robot,opt);
+% robot = estimate_dyn_MLS(robot,opt);
\ No newline at end of file
diff --git a/grab_main.m b/grab_main.m
index 0cfe550..bec485b 100644
--- a/grab_main.m
+++ b/grab_main.m
@@ -21,7 +21,7 @@ get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
 robot = feval(get_Kinematics_func,robot,opt);
 
 %%
-theta_stow = [0;113;-160;47;80;90;90;0];
+theta_stow = [0;113;-160;47;80;90;90;0;0];
 theta_delta = theta - theta_stow;
 phi = linspace(0,1,100)*ones(1,6); %?
 theta_phi = theta - theta_delta * phi;
diff --git a/test_grab2.m b/test_grab2.m
index bd4d229..fd89ce8 100644
--- a/test_grab2.m
+++ b/test_grab2.m
@@ -21,7 +21,9 @@ robot = feval(get_Kinematics_func,robot,opt);
 
 theta_stow = deg2rad([0;113;-160;47;80;90;90;0]);
 % theta_stow = [pi/3;1.1;-2.1;0.345;0;3.14;0;0.262];
-theta_init = [0;1.1;-2.1;0.345;0;3.14;0;0.262];
+% theta_init = deg2rad([0;0;-160;47;80;90;90;0]);
+theta_init = deg2rad([0;110;-160;47;80;90;90;0]);
+% theta_init = [0;1.1;-2.1;0.345;0;3.14;0;0.262];
 theta_delta = theta_init - theta_stow;
 traj_space = [];
 
@@ -37,9 +39,8 @@ Ft(1) = 0;
 
 for i = 1:1000
     Acc_phi = inv(Mphi)*(Ft(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2);
-    Acc_phi = min(0.1, max(-0.1, Acc_phi));
     Vel_phi = Vel_phi + Acc_phi*dt;
-    Vel_phi = min(0.01, max(-0.01, Vel_phi));
+    Vel_phi = min(1, max(-1, Vel_phi));
     phi = phi + Vel_phi*dt;
     if phi>=1
         phi=1;
@@ -70,13 +71,13 @@ for i = 1:1000
     dx_ = VecTose3(dtraj_space(:,i))*[traj_space(1:3,4,i);1];
     dx(:,i) = dx_(1:3);
     % mehod 1: only use linear force
-    Fext_sensor = [0;0;0;30;-40;0]; 
-    permutationMatrix = diag([-1,-1,1,1]);
-    Fext_base = Adjoint(permutationMatrix*inv(traj_space(:,:,i)))'*Fext_sensor;
+%     Fext_sensor = [0;0;0;30;-40;0]; 
+%     permutationMatrix = diag([-1,-1,1,1]);
+%     Fext_base = Adjoint(permutationMatrix*inv(traj_space(:,:,i)))'*Fext_sensor;
 %     Ft(i+1) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
-    Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
+%     Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
 %     Ft(i+1) = sin(2*pi*dt*i);
-%     Ft(i+1) = 10;
+    Ft(i+1) = 10;
 end
 %%
 % 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）