try to add grab to untow
This commit is contained in:
cosmic_power
parent
4da2d08ae4
commit
f1e27552ab
|
|
@ -0,0 +1,26 @@
|
|||
% 定义phi的范围
|
||||
phi = linspace(0, 10, 100); % 从0到10，100个点
|
||||
|
||||
% 初始化向量数组
|
||||
x = zeros(size(phi));
|
||||
y = zeros(size(phi));
|
||||
z = (1 - exp(-3*phi)) ./ (1 - exp(-3));
|
||||
|
||||
% 创建图形
|
||||
figure;
|
||||
hold on;
|
||||
axis equal;
|
||||
grid on;
|
||||
|
||||
% 绘制向量
|
||||
plot(phi,z);
|
||||
|
||||
% 设置图形属性
|
||||
% xlabel('X');
|
||||
% ylabel('Y');
|
||||
zlabel('Z');
|
||||
title('Vector Field Trajectory');
|
||||
legend('Trajectory');
|
||||
|
||||
% % 设置视角
|
||||
% view(3);
|
||||
|
|
@ -39,7 +39,7 @@ switch opt.robot_def
|
|||
robot.com_pos_R1 = com_pos_R1;
|
||||
robot.com_pos_R2 = com_pos_R2;
|
||||
% FIXME:fix this hack
|
||||
% Get 3D coordinate of CO
|
||||
% Get 3D coordinate of next joint axis
|
||||
co=[];
|
||||
for i = 1:8
|
||||
if i == 1
|
||||
|
|
|
|||
|
|
@ -0,0 +1,52 @@
|
|||
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];
|
||||
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);
|
||||
% Ft = Fext*dtraj_space; %fix
|
||||
% % saturation and switch
|
||||
% %%
|
||||
Vel_phi=zeros(6,1);Acc_phi=zeros(6,1);dt=1/2000;phi=zeros(6,1);
|
||||
Mphi = diag(10);
|
||||
Kphi = diag(ones(1,6));
|
||||
Cphi = diag(ones(1,6));
|
||||
phi_log =[];
|
||||
for i = 1:100
|
||||
Acc_phi = inv(Mphi)*(Fext(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2);
|
||||
Vel_phi = Vel_phi + Acc_phi*dt;
|
||||
phi = phi + Vel_phi*dt;
|
||||
phi_log = [phi_log,phi];
|
||||
end
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
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];
|
||||
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;
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
%% 外力投影
|
||||
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
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
%% 外力投影
|
||||
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 = [pi;0;0;0;0;0;0;0];
|
||||
theta_delta = theta(1:8) - theta_stow;
|
||||
theta
|
||||
traj_space = [];
|
||||
for i = 1:1000
|
||||
phi = linspace(0,1,100).*ones(8,1); %?
|
||||
theta_phi = theta(1:8) - theta_delta .* phi;
|
||||
dtheta_phi = -theta_delta .* linspace(1,1,100).*ones(8,1);
|
||||
|
||||
%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(:,i); %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
|
||||
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
%% 外力投影
|
||||
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 = [2*pi;0;0;0;0;0;0;0];
|
||||
theta_init = theta(1:8);
|
||||
theta_delta = theta_init - theta_stow;
|
||||
traj_space = [];
|
||||
|
||||
Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/10;phi=zeros(1,1);
|
||||
Mphi = diag(5*ones(1,1));
|
||||
Kphi = diag(40*ones(1,1));
|
||||
Cphi = diag(0*ones(1,1));
|
||||
phi_log =[];
|
||||
|
||||
Ft(1) = 0;
|
||||
|
||||
for i = 1:1000
|
||||
Acc_phi = inv(Mphi)*(Ft(i)-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;
|
||||
elseif phi<=0
|
||||
phi=0;
|
||||
Acc_phi = 0;
|
||||
Vel_phi = 0;
|
||||
end
|
||||
phi_log = [phi_log,phi];
|
||||
theta_phi(:,i) = theta_init - theta_delta .* phi;
|
||||
dtheta_phi = -theta_delta;
|
||||
|
||||
%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;10;10;0];
|
||||
% Ft(i+1) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
|
||||
% Ft(i+1) = Fext(4:6)' * dx(:,i)/norm(dx(:,i));
|
||||
% Ft(i+1) = sin(2*pi*dt*i);
|
||||
Ft(i+1) = 1;
|
||||
end
|
||||
%%
|
||||
L1=Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset',0,'qlim',deg2rad([-110,110]),'modified');
|
||||
L2=Link('revolute', 'd', 0, 'a', 0.2, 'alpha', pi/2, 'offset',0,'qlim',deg2rad([-120,120]),'modified');
|
||||
L3=Link('revolute', 'd', 0, 'a', 0.5, 'alpha', 0, 'offset',0,'qlim',deg2rad([-160,160]),'modified');
|
||||
L4=Link('revolute', 'd', 0, 'a', 0.45, 'alpha', 0, 'offset',0,'qlim',deg2rad([-160,160]),'modified');
|
||||
L5=Link('revolute', 'd', 0, 'a', 0.12, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg2rad([-90,90]),'modified');
|
||||
L6=Link('revolute', 'd', 0.28, 'a', 0, 'alpha', -pi/2, 'offset',0,'qlim',deg2rad([-270,270]),'modified');
|
||||
L7=Link('revolute', 'd', 0.40, 'a', 0, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg2rad([-270,270]),'modified');
|
||||
L8=Link('revolute', 'd', 0, 'a', 0, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg2rad([-71,79]),'modified');
|
||||
L9=Link('prismatic', 'a', 0.126493, 'alpha', -pi/2, 'theta',0,'offset',-(0.59295-0.2772),'qlim',[0,0.342],'modified');
|
||||
DVT=SerialLink([L1 L2 L3 L4 L5 L6 L7 L8 L9],'name','DVT'); %连接连杆
|
||||
% DVT.plot([0 0 0 0 0 0 0 0 0])%机械臂图
|
||||
isVideoRecordEnable = 1;
|
||||
view_angle1 = -136;
|
||||
view_angle2 = 32;
|
||||
if isVideoRecordEnable
|
||||
myVideo = VideoWriter('DVT1_sim'); %open video file
|
||||
myVideo.FrameRate = 30; %can adjust this, 5 - 10 works well for me
|
||||
open(myVideo);
|
||||
axis([-1 2.0 -1 1 -2 1.6]);
|
||||
view([view_angle1 view_angle2]);
|
||||
end
|
||||
|
||||
for i = 1:1000
|
||||
if isVideoRecordEnable
|
||||
% drawing
|
||||
DVT.plot([theta_phi(:,i);0]')%机械臂图
|
||||
axis([-1 2.0 -1 1 -2 1.6]);
|
||||
hold on;
|
||||
view([view_angle1 view_angle2]);
|
||||
drawnow;
|
||||
frame = getframe(gcf); %get frame
|
||||
writeVideo(myVideo, frame);
|
||||
end
|
||||
end
|
||||
|
|
@ -55,7 +55,7 @@ end
|
|||
plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
|
||||
axis equal
|
||||
grid on
|
||||
% plot 3D: Get 3D coordinate of CO
|
||||
% plot 3D: Get 3D coordinate of next joint axis
|
||||
co=[];
|
||||
for i = 1:8
|
||||
if i == 1
|
||||
Loading…
Reference in New Issue