Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feature/R1000-identification #2

Merged
cosmic_power merged 48 commits from feature/R1000-identification into main 2024-12-16 15:25:53 +00:00
Conversation 0 Commits 48 Files Changed 129 +75 -138
7 changed files with 75 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6561c775dc - Show all commits

2
Identification_main.m
View File

@ -7,7 +7,7 @@ opt.LD_method = 'Direct';
opt.debug = true;
opt.robotName = 'R1000';
opt.reGenerate = false;
opt.Isreal = false;
opt.Isreal = true;
robot = get_robot_R1000(file,opt);
robot = get_Kinematics(robot, opt);

2
get_Kinematics.m
View File

@ -25,6 +25,8 @@ if(opt.Isreal)
%stack result into kine structure
robot.kine.TW = robot.TW;
robot.kine.T = robot.T;
robot.kine.R = robot.kine.T(1:3,1:3,:);
robot.kine.t = robot.kine.T(1:3,4,:);
robot.kine.Fkine = robot.Fkine;
% get the CG at the world base frame
% FIXME: Fix this hack

16
get_regressor.m
View File

@ -24,8 +24,8 @@ end
[nLnkPrms, nLnks] = size(robot.regressor.pi);
robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
% init matrix
R = robot.R;
P = robot.t;
R = robot.kine.R;
P = robot.kine.t;
w = robot.vel.w ;
dw = robot.vel.dw ;
dv = robot.vel.dv ;
@ -67,17 +67,19 @@ switch opt.LD_method
end
robot.regressor.K = delta_*robot.regressor.U;
if(opt.debug)
sprintf('size of U=%dx%d.',size(robot.regressor.U));
sprintf('size of K=%dx%d.',size(robot.regressor.K));
sprintf('size of U=%dx%d.',size(robot.regressor.U))
sprintf('size of K=%dx%d.',size(robot.regressor.K))
end
end
% matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
% 'Vars',{q_sym,qd_sym,q2d_sym});
tic
% matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
% 'Vars',{q_sym,qd_sym,q2d_sym});
if(opt.reGenerate)
tic
matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
'Vars',{q_sym});
compileTime = toc;
fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
end
% matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
% 'Vars',{q_sym,qd_sym,q2d_sym});
case 'Lagrange'

3
get_robot_R1000.m
View File

@ -117,6 +117,9 @@ switch opt.robot_def
% Kinematics parameters
% ------------------------------------------------------------------------
if(opt.Isreal)
%FIXME: only consider the theta temply
robot.dtheta = zeros([1,ndof]);
robot.ddtheta = zeros([1,ndof]);
switch opt.KM_method
case 'SDH'
case 'MDH'

23
get_velocity.m
View File

@ -1,10 +1,6 @@
function robot = get_velocity(robot, opt)
switch opt.KM_method
case 'SCREW'
% init q
q = robot.theta;
qd = robot.dtheta;
qdd = robot.ddtheta;
Mlist_CG = robot.kine.Mlist_CG;
% Get general mass matrix
link_mass = robot.m;
@ -15,14 +11,31 @@ switch opt.KM_method
for i = 1:robot.ndof
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
Glist = cat(3, Glist, Gb);
% ?
% mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
% robot.pi(2:4,i) = mc;
end
if opt.Isreal
% init q
q = robot.theta';
qd = robot.dtheta';
qdd = robot.ddtheta';
[V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
else
% init q
q = robot.theta;
qd = robot.dtheta;
qdd = robot.ddtheta;
[V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
end
% FIXME: twist is not equal to velocity
robot.vel.w = V(1:3,:);
robot.vel.v = V(4:6,:);
robot.vel.dw = Vd(1:3,:);
robot.vel.dv = Vd(4:6,:);
% robot.vel.dv = Vd(4:6,:);
robot.vel.dv=[zeros(2,robot.ndof);robot.gravity(end)*ones(1,robot.ndof)];
case 'SDH'
case 'MDH'
switch opt.Vel_method

87
verify_regressor_R1000.asv
View File

@ -1,87 +0,0 @@
% function robot = verify_regressor(robot, opt)
% verify: If full regressor dynamics is the same as basic dynamics
ndof = robot.ndof;
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
q_sym = sym('q%d',[ndof+1,1],'real');
qd_sym = sym('qd%d',[ndof+1,1],'real');
q2d_sym = sym('qdd%d',[ndof+1,1],'real');
for i =1:ndof
pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;];
end
% pi2=zeros([10,1]);
pi=[pi1;pi2];
regressor = standard_regressor_Two_bar(q_sym,qd_sym,q2d_sym);
tau=regressor*pi;
%% Two-bar
N=2;
thetalist = q_sym(1:N);
dthetalist = qd_sym(1:N);
ddthetalist = q2d_sym(1:N);
Gb= [diag([1,1,1]),zeros(3,3);
zeros(3,3),diag([1,1,1])];
Glist = cat(3, Gb, Gb);
% Glist = cat(3, Gb, zeros([6,6]));
M01 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M23 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, M01, M12, M23);
Slist=[[0;0;1;0;0;0],...
[0;0;1;0;-1;0]];
Adgab_mat = sym(zeros(6,6,N+1));
Fmat=sym(zeros(N,6));
F1=sym(zeros(N,6));
V1=sym(zeros(6,N+1));
G=sym(zeros(4,4,N));
T=sym(zeros(4,4,N));
Vlinear=sym(zeros(3,3));
Vd1=sym(zeros(6,N+1));
Gb= [diag([1,1,1]),zeros(3,3);
zeros(3,3),diag([1,1,1])];
J=sym(zeros(6,N));
exf=[0;0;0;0;0;0];
[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
= InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
[0;0;0], exf, Mlist, Glist, Slist);
G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M23 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, M01, M12, M23);
T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
% Use Body Twist cal linear vel, but can't cal the end frame vel
[V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
[0;0;0], exf, Mlist, Glist, Slist);
j=1;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
j=2;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
%% Check if screw method is equal to regressor
isequal(simplify(tau),simplify(tau_mat))
%% Numerical
clear pi;
ndof = robot.ndof;
time = 0:0.01:2;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
q=[q_J;-q_J];
qd=[qd_J; -qd_J];
qdd=[qdd_J; -qdd_J];
g = [0; 0; -9.8];
robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% pi2=zeros([10,1]);
robot_pi=[robot_pi1;robot_pi2];
tau = zeros([2,100]);
for i = 1:length(q_J)
regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
tau(:,i)=regressor*robot_pi;
end

68
verify_regressor_R1000.m
View File

@ -1,40 +1,44 @@
% function robot = verify_regressor(robot, opt)
% verify: If full regressor dynamics is the same as basic dynamics
% % function robot = verify_regressor(robot, opt)
% % verify: If full regressor dynamics is the same as basic dynamics
% ndof = robot.ndof;
% % Dynamics parameters
% link_mass = robot.m;
% com_pos = robot.com;
% link_inertia = robot.I;
%
% q_sym = sym('q%d',[ndof,1],'real');
% % for i =1:ndof
% % robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;inertiaMatrix2Tensor(link_inertia(:,:,i))];
% % end
% regressor = standard_regressor_R1000(q_sym);
% robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
% tau=regressor*robot_pi_vector;
% %% R1000
% tau_mat = standard_dynamics_R1000(q_sym);
% %% Check if screw method is equal to regressor
% isequal(simplify(tau),simplify(tau_mat))
%% Numerical
ndof = robot.ndof;
time = 0:0.01:1;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J));
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
tau = zeros([ndof,length(q_J)]);
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
q_sym = sym('q%d',[ndof,1],'real');
% for i =1:ndof
% robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;inertiaMatrix2Tensor(link_inertia(:,:,i))];
% robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1(:,i);inertiaMatrix2Tensor(link_inertia(:,:,i))];
% end
% regressor = standard_regressor_R1000(q_sym);
robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
tau=robot.regressor.K*robot_pi_vector;
%% R1000
tau_mat = standard_dynamics_R1000(q_sym);
%% Check if screw method is equal to regressor
isequal(simplify(tau),simplify(tau_mat))
% %% Numerical
% clear pi;
% ndof = robot.ndof;
% time = 0:0.01:2;
% f=1;
% q_J = sin(2*pi*f*time);
% qd_J = (2*pi*f)*cos(2*pi*f*time);
% qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
% q=[q_J;-q_J];
% qd=[qd_J; -qd_J];
% qdd=[qdd_J; -qdd_J];
% g = [0; 0; -9.8];
% robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% % pi2=zeros([10,1]);
% robot_pi=[robot_pi1;robot_pi2];
% tau = zeros([2,100]);
% for i = 1:length(q_J)
% regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
% tau(:,i)=regressor*robot_pi;
% end
for i = 1:length(q_J)
regressor = standard_regressor_R1000(thetalist(i,:)');
tau(:,i)=robot.regressor.K*robot_pi_vector;
tau_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)');
end