add grab
This commit is contained in:
cosmic_power
parent
f1e27552ab
commit
f1e0c4951a
|
|
@ -0,0 +1,19 @@
|
|||
function GravityForce = calculateGravityForce(thetaMea)
|
||||
% Example:
|
||||
% thetalist = [0;0;0;0;0;pi/2;0;0;0];
|
||||
% calculateGravityForce([thetalist])
|
||||
|
||||
% code start
|
||||
opt.robot_def = 'direct';
|
||||
opt.KM_method = 'SCREW';
|
||||
opt.Vel_method = 'Direct';
|
||||
opt.LD_method = 'Direct';
|
||||
opt.debug = true;
|
||||
opt.robotName = 'R1000_DVT';
|
||||
opt.reGenerate = false;
|
||||
opt.Isreal = true;
|
||||
zero_ = zeros(length(thetaMea),1);
|
||||
|
||||
robot = get_robot_R1000_DVT(zero_,zero_,zero_,opt);
|
||||
robot = get_Kinematics_R1000_DVT(robot, opt);
|
||||
GravityForce = getGravityForce(thetaMea, robot, opt);
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
function GravityForce = getGravityForce(thetalist,robot,opt)
|
||||
dthetalist = zeros([robot.ndof,1]);
|
||||
ddthetalist = zeros([robot.ndof,1]);
|
||||
exf = zeros([6,1]);
|
||||
|
||||
% Get info from robot
|
||||
Mlist_CG = robot.kine.Mlist_CG;
|
||||
Mlist_ED = robot.kine.Mlist_ED;
|
||||
Slist = robot.slist;
|
||||
|
||||
% Get general mass matrix
|
||||
Glist=zeros(6,6,robot.ndof);
|
||||
link_inertia = zeros(3,3,robot.ndof);
|
||||
for i = 1:robot.ndof
|
||||
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),robot.m(i)*diag([1,1,1])];
|
||||
Glist(:,:,i) = Gb;
|
||||
end
|
||||
|
||||
[~,~,~,Fmat,~] ...
|
||||
= InverseDynamics_debug(thetalist, dthetalist, ddthetalist, ...
|
||||
robot.gravity, exf, Mlist_CG, Glist, Slist);
|
||||
G = FKinSpaceExpand(Mlist_CG, Slist, thetalist);
|
||||
T = FKinSpaceExpand(Mlist_ED, Slist, thetalist);
|
||||
F_Simpack = getSimpackF(G,T,Mlist_ED,Fmat);
|
||||
% get J7
|
||||
permutationMatrix = diag([1,1,-1,1,1,-1]);
|
||||
GravityForce = permutationMatrix*F_Simpack(:,7);
|
||||
end
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
function Flist = getSimpackF(T_GC,T_O,Mlist,Fmat)
|
||||
N=size(Fmat,1);
|
||||
Flist = zeros(6,N);
|
||||
|
||||
%base frame
|
||||
% Adgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
|
||||
% Flist(:,1) = Adgab'*Fmat(1,:)';
|
||||
% for i = 2: N
|
||||
% % Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
|
||||
% Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i));
|
||||
% Flist(:,i) = Adgab'*Fmat(i,:)';
|
||||
% endAdgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
|
||||
for i = 1: N
|
||||
% Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
|
||||
Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i));
|
||||
Flist(:,i) = Adgab'*Fmat(i,:)';
|
||||
end
|
||||
end
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
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
|
||||
|
|
@ -1,49 +0,0 @@
|
|||
%% 外力投影
|
||||
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
|
||||
|
||||
124
test_grab2.m
124
test_grab2.m
|
|
@ -20,22 +20,26 @@ 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_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_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));
|
||||
Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/20;phi=zeros(1,1);
|
||||
% Mphi = diag(5*ones(1,1));
|
||||
Mphi = diag(100*ones(1,1));
|
||||
% Kphi = diag(40*ones(1,1));
|
||||
Kphi = diag(80*ones(1,1));
|
||||
Cphi = diag(0*ones(1,1));
|
||||
phi_log =[];
|
||||
phi_log =[];Velphi_log =[];Accphi_log =[];
|
||||
|
||||
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));
|
||||
phi = phi + Vel_phi*dt;
|
||||
if phi>=1
|
||||
phi=1;
|
||||
|
|
@ -46,6 +50,8 @@ for i = 1:1000
|
|||
Acc_phi = 0;
|
||||
Vel_phi = 0;
|
||||
end
|
||||
Accphi_log = [Accphi_log,Acc_phi];
|
||||
Velphi_log = [Velphi_log,Vel_phi];
|
||||
phi_log = [phi_log,phi];
|
||||
theta_phi(:,i) = theta_init - theta_delta .* phi;
|
||||
dtheta_phi = -theta_delta;
|
||||
|
|
@ -58,17 +64,43 @@ for i = 1:1000
|
|||
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
|
||||
traj_space(:,:,i) = robot.kine.TW(:,:,7);
|
||||
Jaco = JacobianSpace(robot.slist(:,1:7),theta(1:7));
|
||||
dtraj_space(:,i) =Jaco*dtheta_phi(1:7); %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];
|
||||
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(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) = 1;
|
||||
% Ft(i+1) = 10;
|
||||
end
|
||||
%%
|
||||
% 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）
|
||||
x_axis = quiver3(0, 0, 0, 0, 0, 0, 'r');
|
||||
x_axis.LineWidth = 3;
|
||||
x_axis.AutoScaleFactor = 0.5;
|
||||
hold on;
|
||||
test = traj_space(1:3,4,:);
|
||||
reshape(test,[3,size(test,3)]);
|
||||
for i = 1:1000
|
||||
% 绘制机械臂
|
||||
% DVT.plot([theta_phi(:,i);0]')
|
||||
% T_L8 = DVT_L8.fkine(theta_phi(:,i));
|
||||
|
||||
% 设置坐标轴范围
|
||||
axis([-1 2.0 -1 1 -2 1.6]);
|
||||
plot3(test(1,i), test(2,i), test(3,i), 'bo', 'MarkerSize', 5, 'MarkerFaceColor', 'b');
|
||||
% 更新箭头的位置和方向（而不是重新创建）
|
||||
set(x_axis, 'XData', test(1,i), 'YData', test(2,i), 'ZData', test(3,i), ...
|
||||
'UData', dx(1,i)/norm(dx(:,i)), 'VData', dx(2,i)/norm(dx(:,i)), 'WData', dx(3,i)/norm(dx(:,i)));
|
||||
% 设置视角
|
||||
% view([view_angle1 view_angle2]);
|
||||
% 刷新图形
|
||||
drawnow;
|
||||
end
|
||||
%%
|
||||
L1=Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset',0,'qlim',deg2rad([-110,110]),'modified');
|
||||
|
|
@ -81,27 +113,61 @@ L7=Link('revolute', 'd', 0.40, 'a', 0, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg
|
|||
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'); %连接连杆
|
||||
% 创建仅包含前8个连杆的子机器人
|
||||
links_before_L9 = DVT.links(1:8); % 提取L1-L8
|
||||
DVT_L8 = SerialLink(links_before_L9, 'name', 'DVT_up_to_L8');
|
||||
% 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]);
|
||||
% 预先计算所有轨迹点
|
||||
traj_points = zeros(3, 100); % 假设总共有100个点
|
||||
for i = 1:100
|
||||
T_L8 = DVT_L8.fkine(theta_phi(:,i));
|
||||
traj_points(:,i) = T_L8.t; % 存储所有轨迹点
|
||||
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;
|
||||
% 图形初始化
|
||||
figure;
|
||||
axis([-1 2.0 -1 2 -2 1.6]);
|
||||
view_angle1 = -156;view_angle2=46;
|
||||
view([view_angle1 view_angle2]);
|
||||
drawnow;
|
||||
frame = getframe(gcf); %get frame
|
||||
isVideoRecordEnable = 0;
|
||||
hold on;
|
||||
grid on;
|
||||
|
||||
% 一次性绘制全部轨迹点
|
||||
plot3(traj_points(1,:), traj_points(2,:), traj_points(3,:),...
|
||||
'b-o', 'MarkerSize', 4, 'MarkerFaceColor', 'b', 'LineWidth', 1.5);
|
||||
|
||||
% 初始化箭头
|
||||
x_axis = quiver3(0,0,0,0,0,0, 'r');
|
||||
x_axis.LineWidth = 3;
|
||||
x_axis.AutoScaleFactor = 0.5;
|
||||
|
||||
% 视频录制设置
|
||||
if isVideoRecordEnable
|
||||
myVideo = VideoWriter('DVT1_sim', 'MPEG-4');
|
||||
myVideo.FrameRate = 30;
|
||||
open(myVideo);
|
||||
end
|
||||
% 主循环（只更新机械臂和箭头）
|
||||
for i = 1:100
|
||||
% 绘制机械臂（保留轨迹不擦除）
|
||||
DVT.plot([theta_phi(:,i);0]');
|
||||
|
||||
% 更新箭头
|
||||
set(x_axis, 'XData',traj_points(1,i), 'YData',traj_points(2,i), 'ZData',traj_points(3,i),...
|
||||
'UData',dx(1,i)/norm(dx(:,i)), 'VData',dx(2,i)/norm(dx(:,i)), 'WData',dx(3,i)/norm(dx(:,i)));
|
||||
% view([view_angle1 view_angle2]);
|
||||
% 录制视频
|
||||
if isVideoRecordEnable
|
||||
frame = getframe(gcf);
|
||||
writeVideo(myVideo, frame);
|
||||
end
|
||||
|
||||
drawnow limitrate; % 更高效的刷新方式
|
||||
end
|
||||
|
||||
% 确保关闭视频文件
|
||||
if isVideoRecordEnable
|
||||
close(myVideo);
|
||||
disp('视频录制完成');
|
||||
end
|
||||
Loading…
Reference in New Issue