Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

test grab

This commit is contained in:
cosmic_power 2025-06-11 18:17:20 +08:00
parent f1e0c4951a
commit 4c0ac1ccae
3 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Identification_main.m
View File

@ -30,5 +30,5 @@ robot = get_regressor(robot,opt);
% % verify_regressor_R1000; % % verify_regressor_R1000;
robot = get_baseParams(robot, opt); robot = get_baseParams(robot, opt);
% robot = estimate_dyn(robot,opt); % robot = estimate_dyn(robot,opt);
% robot = estimate_dyn_form_data(robot,opt); robot = estimate_dyn_form_data(robot,opt);
robot = estimate_dyn_MLS(robot,opt); % robot = estimate_dyn_MLS(robot,opt);

2
grab_main.m
View File

@ -21,7 +21,7 @@ get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
robot = feval(get_Kinematics_func,robot,opt); 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; theta_delta = theta - theta_stow;
phi = linspace(0,1,100)*ones(1,6); %? phi = linspace(0,1,100)*ones(1,6); %?
theta_phi = theta - theta_delta * phi; theta_phi = theta - theta_delta * phi;

17
test_grab2.m
View File

@ -21,7 +21,9 @@ robot = feval(get_Kinematics_func,robot,opt);
theta_stow = deg2rad([0;113;-160;47;80;90;90;0]); 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_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; theta_delta = theta_init - theta_stow;
traj_space = []; traj_space = [];
@ -37,9 +39,8 @@ Ft(1) = 0;
for i = 1:1000 for i = 1:1000
Acc_phi = inv(Mphi)*(Ft(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2); 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 = 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; phi = phi + Vel_phi*dt;
if phi>=1 if phi>=1
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_ = VecTose3(dtraj_space(:,i))*[traj_space(1:3,4,i);1];
dx(:,i) = dx_(1:3); dx(:,i) = dx_(1:3);
% mehod 1: only use linear force % mehod 1: only use linear force
Fext_sensor = [0;0;0;30;-40;0]; % Fext_sensor = [0;0;0;30;-40;0];
permutationMatrix = diag([-1,-1,1,1]); % permutationMatrix = diag([-1,-1,1,1]);
Fext_base = Adjoint(permutationMatrix*inv(traj_space(:,:,i)))'*Fext_sensor; % 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(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) = sin(2*pi*dt*i);
% Ft(i+1) = 10; Ft(i+1) = 10;
end end
%% %%
% 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新） % 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）