58 lines
2.0 KiB
Matlab
58 lines
2.0 KiB
Matlab
% function robot = verify_regressor(robot, opt)
|
|
% verify: If full regressor dynamics is the same as basic dynamics
|
|
ndof = robot.ndof;
|
|
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');
|
|
pi1=[2;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
|
|
pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
|
|
% 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); |