IRDYn/get_GCTraj_R1000_EVT.m

% function robot = get_GCTraj_R1000_EVT(robot,opt)
% J9 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(9,:)=q_J;
q(8,:)=pi/3*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(9).t = time;
idntfcnTrjctry(9).q = q;
idntfcnTrjctry(9).qd = qd;
idntfcnTrjctry(9).qdd = qdd;

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(9,:)=q_J;
q(8,:)=-pi/3*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(9,:)=q_J;
q(6,:)=pi/2*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(9,:)=q_J;
q(6,:)=-pi/2*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);

% J8 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(8,:)=pi/3*q_J;
q(6,:)=pi/2*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(8).t = time;
idntfcnTrjctry(8).q = q;
idntfcnTrjctry(8).qd = qd;
idntfcnTrjctry(8).qdd = qdd;

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(8,:)=pi/3*q_J;
q(6,:)=-pi/2*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(8).t = vertcat(idntfcnTrjctry(8).t,time);
idntfcnTrjctry(8).q = vertcat(idntfcnTrjctry(8).q,q);
idntfcnTrjctry(8).qd = vertcat(idntfcnTrjctry(8).qd,qd);
idntfcnTrjctry(8).qdd = vertcat(idntfcnTrjctry(8).qdd,qdd);

% J7 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
% q(7,:)=q_J;
% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J;
% q(4,:)=q_J;q(9,:)=q_J;
qd=zeros(robot.ndof,length(q_J));qdd=qd;
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
idntfcnTrjctry(7).t = time;
idntfcnTrjctry(7).q = q;
idntfcnTrjctry(7).qd = qd;
idntfcnTrjctry(7).qdd = qdd;

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
% q(7,:)=q_J;
% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J;
% q(4,:)=q_J;q(9,:)=q_J;
qd=zeros(robot.ndof,length(q_J));qdd=qd;
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
idntfcnTrjctry(7).t = vertcat(idntfcnTrjctry(7).t,time);
idntfcnTrjctry(7).q = vertcat(idntfcnTrjctry(7).q,q);
idntfcnTrjctry(7).qd = vertcat(idntfcnTrjctry(7).qd,qd);
idntfcnTrjctry(7).qdd = vertcat(idntfcnTrjctry(7).qdd,qdd);

% J6 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(6,:)=pi/2*q_J;
q(4,:)=pi/4*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(6).t= time;
idntfcnTrjctry(6).q= q;
idntfcnTrjctry(6).qd= qd;
idntfcnTrjctry(6).qdd= qdd;

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(6,:)=pi/2*q_J;
q(4,:)=-pi/4*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(6).t= vertcat(idntfcnTrjctry(6).t,time);
idntfcnTrjctry(6).q= vertcat(idntfcnTrjctry(6).q,q);
idntfcnTrjctry(6).qd= vertcat(idntfcnTrjctry(6).qd,qd);
idntfcnTrjctry(6).qdd= vertcat(idntfcnTrjctry(6).qdd,qdd);

% J5 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(5,:)=pi/2*q_J;
q(4,:)=pi/4*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(5).t = time;
idntfcnTrjctry(5).q = q;
idntfcnTrjctry(5).qd = qd;
idntfcnTrjctry(5).qdd = qdd;

time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(5,:)=pi/2*q_J;
q(4,:)=-pi/4*ones(size(q_J));
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(5).t = vertcat(idntfcnTrjctry(5).t,time);
idntfcnTrjctry(5).q = vertcat(idntfcnTrjctry(5).q,q);
idntfcnTrjctry(5).qd = vertcat(idntfcnTrjctry(5).qd,qd);
idntfcnTrjctry(5).qdd = vertcat(idntfcnTrjctry(5).qdd,qdd);

% J4 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(4,:)=pi/4*q_J;
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(4).t = time;
idntfcnTrjctry(4).q = q;
idntfcnTrjctry(4).qd = qd;
idntfcnTrjctry(4).qdd = qdd;

% J3 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(3,:)=pi/4*q_J;
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(3).t = time;
idntfcnTrjctry(3).q = q;
idntfcnTrjctry(3).qd = qd;
idntfcnTrjctry(3).qdd = qdd;

% J2 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(2,:)=pi/4*q_J;
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(2).t = time;
idntfcnTrjctry(2).q = q;
idntfcnTrjctry(2).qd = qd;
idntfcnTrjctry(2).qdd = qdd;

% J1 traj
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
q=zeros(robot.ndof,length(q_J));
q(1,:)=pi/4*q_J;
qd=zeros(robot.ndof,length(q_J));qdd=qd;
idntfcnTrjctry(1).t = time;
idntfcnTrjctry(1).q = q;
idntfcnTrjctry(1).qd = qd;
idntfcnTrjctry(1).qdd = qdd;

% pi -> [m;mc;I] 10 element 
[nLnkPrms, nLnks] = size(robot.pi);
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
for i=1:robot.ndof
    q = idntfcnTrjctry(i).q;
    qd = idntfcnTrjctry(i).qd;
    qdd = idntfcnTrjctry(i).qdd;
    [nRow,nCol] = size(idntfcnTrjctry(i).qd);
    for j = 1:nRow/robot.ndof
        for k = 1:nCol
            standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
            regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
                qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k));
            tau_Full=regressor*robot_pi;
            tau(j,k) = tau_Full(i);
        end
    end
    idntfcnTrjctry(i).tau = tau;
end