82 lines
3.0 KiB
Matlab
82 lines
3.0 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 = true;
|
|
opt.robotName = 'R1000';
|
|
|
|
opt.Isreal = true;
|
|
robot = get_robot_R1000(file,opt);
|
|
% robot.theta = [1,1,0];
|
|
|
|
%TODO verify kinematics via robotics toolbox or other software result
|
|
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
|
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
|
|
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
|
|
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3;
|
|
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
|
|
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3;
|
|
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3;
|
|
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3;
|
|
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
|
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3;
|
|
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
|
|
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3;
|
|
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
|
|
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
|
|
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
|
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
|
|
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
|
|
|
% Get 3D coordinate of CO
|
|
co=[];
|
|
for i = 1:8
|
|
if i == 1
|
|
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
|
elseif i<8
|
|
%From base to ISA Origin
|
|
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
|
|
else
|
|
%From base to ISA Origin
|
|
co(:,i) = co(:,i-1)-[0;0;0.05896];
|
|
end
|
|
end
|
|
co = [zeros(3,1),co];
|
|
|
|
% temp slist
|
|
robot.slist=[[0;0;1;co(:,1)],...
|
|
[0;-1;0;cross(-[0;-1;0],co(:,2))]...
|
|
[0;-1;0;cross(-[0;-1;0],co(:,3))]...
|
|
[0;-1;0;cross(-[0;-1;0],co(:,4))]...
|
|
[0;0;1;cross(-[0;0;1],co(:,5))]...
|
|
[1;0;0;cross(-[1;0;0],co(:,6))]...
|
|
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
|
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
|
[0;0;0;1;0;0]];
|
|
|
|
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
|
|
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
|
|
robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
|
|
robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]];
|
|
robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]];
|
|
robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]];
|
|
robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]];
|
|
robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]];
|
|
robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]];
|
|
for i=1:9
|
|
robot.Home.P(:,i) = co(:,i);
|
|
robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i));
|
|
end
|
|
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
|
|
|
robot = get_Kinematics(robot, opt);
|
|
|
|
R1000_Dynamics_num;
|
|
% opt.Isreal = false;
|
|
% robot = get_velocity(robot, opt);
|
|
% robot = get_regressor(robot,opt);
|
|
% symbol matched
|
|
% verify_regressor
|
|
% robot = get_baseParams(robot, opt); |