51 lines
1.5 KiB
Matlab
51 lines
1.5 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 = deg2rad([0;113;-160;47;80;90;90;0]);
|
|
theta_stow = [pi;0;0;0;0;0;0;0];
|
|
theta_delta = theta(1:8) - theta_stow;
|
|
phi = linspace(0,1,100); %?
|
|
theta_phi = theta(1:8) - theta_delta .* phi;
|
|
dtheta_phi = -theta_delta;
|
|
|
|
traj_space = [];
|
|
for i = 1:size(theta_phi,2)
|
|
%hack theta
|
|
theta = zeros(9,1);
|
|
theta(1:8) = theta_phi(:,i);
|
|
%get robot
|
|
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);
|
|
%FK
|
|
traj_space(:,:,i) = robot.kine.TW(:,:,8);
|
|
Jaco = JacobianSpace(robot.slist(:,1:8),theta(1:8));
|
|
dtraj_space(:,i) =Jaco*dtheta_phi; %fix
|
|
dx_ = VecTose3(dtraj_space(:,i))*[traj_space(1:3,4,i);1];
|
|
dx(:,i) = dx_(1:3);
|
|
% mehod 1: only use linear force
|
|
Fext = [0;0;0;1;1;0];
|
|
% Ft(:,i) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
|
|
Ft(i) = Fext(4:6)' * dx(:,i)/norm(dx(:,i));
|
|
end
|
|
|
|
|