63 lines
1.7 KiB
Matlab
63 lines
1.7 KiB
Matlab
close all;clc;clear
|
|
file = [];
|
|
opt.robot_def = 'direct';
|
|
opt.KM_method = 'SCREW';
|
|
opt.Vel_method = 'Direct';
|
|
opt.LD_method = 'Direct';
|
|
opt.debug = false;
|
|
opt.robotName = 'R1000_DVT';
|
|
opt.reGenerate = false;
|
|
opt.Isreal = true;
|
|
opt.isJointTorqueSensor = true;
|
|
opt.isSixAxisFTSensor = false;
|
|
|
|
theta = zeros(9,1);
|
|
dtheta = zeros(9,1);
|
|
ddtheta = zeros(9,1);
|
|
|
|
get_robot_func = sprintf('get_robot_%s',opt.robotName);
|
|
robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
|
|
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;0];
|
|
theta_delta = theta - theta_stow;
|
|
phi = linspace(0,1,100)*ones(1,6); %?
|
|
theta_phi = theta - theta_delta * phi;
|
|
dtheta_phi = -phi;
|
|
|
|
robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
|
|
get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
|
|
robot = feval(get_Kinematics_func,robot,opt);
|
|
traj_space = [];
|
|
traj_space = [traj_space;robot.TW];
|
|
dtraj_space = jaco*dtheta_phi; %fix
|
|
%% 外力估计
|
|
t=linspace(0,100,100);
|
|
% Fext = 100*sin(2*pi*t);
|
|
Fext = 10;
|
|
% Ft = Fext*dtraj_space; %fix
|
|
% % saturation and switch
|
|
%% 阻抗控制
|
|
Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/2000;phi=zeros(1,1);
|
|
Mphi = diag(10*ones(1,1));
|
|
Kphi = diag(80*ones(1,1));
|
|
Cphi = diag(0*ones(1,1));
|
|
phi_log =[];
|
|
for i = 1:10000
|
|
Acc_phi = inv(Mphi)*(Fext-Kphi*Vel_phi-Cphi*Vel_phi.^2);
|
|
Vel_phi = Vel_phi + Acc_phi*dt;
|
|
phi = phi + Vel_phi*dt;
|
|
if phi>=1
|
|
phi=1;
|
|
Acc_phi = 0;
|
|
Vel_phi = 0;
|
|
end
|
|
phi_log = [phi_log,phi];
|
|
end
|
|
%% 关节运动
|
|
theta = zeros(8,1);
|
|
theta_stow = deg2rad([0;113;-160;47;80;90;90;0]);
|
|
theta_delta = theta - theta_stow;
|
|
theta_phi = theta - theta_delta * phi_log; |