DRR cv3

function R = myinvd(robot,J,dJ,ddJ,cnf)
%
% myinvd(robot,J,dJ,ddJ,cnf) - inverse dynamic
%
% Input parameters:
%   robot: struct  ... description of the manipulator. You can ignore and
%                      use description and parameters from your variant.
%   J: [2 x n]     ... joint coordinates. Each column J(:,i) contains
%                      2 joint coordinates (angles and or displacements).
%   dJ: [2 x n]    ... joint velocities. Each column dJ(:,i) contains
%                      2 joint velocities (angular velocity).
%   ddJ: [2 x n]   ... joint accelerations. Each column dJ(:,i) contains
%                      2 joint accelerations (angular acceleration).
%   cnf: [1 x n]   ... robot configuration sign. Configuration of final two
%                      links is defined by position of end point in
%                      relation to line defined by their oposit ends.
%
% Output parameters:
%   R: [2 x n]     ... torques and forces provided by drives for each input
%                      configuration which is defined by J and cnf. Each
%                      column of R must contain torques for corresponding
%                      input configuration. If there is no solution for
%                      input configuration, the output value for the
%                      configuration should be NaN.
%
% All angles are in radians !

% preallocation
R = zeros(2, size(J,2));
R(:) = NaN;
for i = 1:size(J,2)
    R(:,i) = my_myinvd(robot,J(:,i),dJ(:,i),ddJ(:,i),cnf(i));
end

% R(:,4:end)=NaN;
% R(:,1:2)=NaN;
end

function output = my_myinvd(robot,J,dJ,ddJ,cnf)
%% SET CONSTANTS %%
phi4 = robot.rotations(1);
P1 = [robot.origins(1);
      robot.origins(2)]/10;
P2 = [robot.origins(3);
      robot.origins(4)]/10;
P1x = P1(1);
P1y = P1(2);
P2x = P2(1);
P2y = P2(2);

L1 = robot.links(1,2)/20;
L2 = robot.links(2,2)/20;
L3 = robot.links(2,1)/20;

C=mydkt(robot, J);
C1=C{1};
C2=C{2};

d=J(1)/10;
dd = dJ(1)/10;
ddd = ddJ(1)/10;

phi1 = J(2);
phi1d = dJ(2);
phi1dd = ddJ(2);

Ax=d*cos(phi4)+P1x;
Ay=d*sin(phi4)+P1y;
Bx=L1*2*cos(phi1)+P2x;
By=L1*2*sin(phi1)+P2y;

%% DECIDE SOLUTION %%

Wx1=C1(1)/10;
Wy1=C1(2)/10;
Wx2=C2(1)/10;
Wy2=C2(2)/10;

K=(By-Ay)*Wx1-(Bx-Ax)*Wy1 +Ay*(Bx-Ax)-Ax*(By-Ay)

if (abs(sign(K)-cnf) > 1e-3)
    Wx=Wx2;
    Wy=Wy2;
else
    Wx=Wx1;
    Wy=Wy1;
end

if isnan(Wx) || isnan(Wy)
    output = [NaN;NaN];
    return
end

%% VECTOR METHOD %%

phi2 = atan2(Wy-By, Wx-Bx);
phi3 = atan2(Ay-Wy, Ax-Wx);

qd = [dd; phi1d];
qdd = [ddd; phi1dd];

Jq = [-0.5*cos(phi4) -L1*sin(phi1);-0.5*sin(phi4) L1*cos(phi1)];
Jz = [-L2*sin(phi2) -L3*sin(phi3); L2*cos(phi2) L3*cos(phi3)];

zd = -(Jz\Jq)*qd;
phi2d = zd(1);
phi3d = zd(2);

Jqd = [0 -L1*cos(phi1)*phi1d; 0 -L1*sin(phi1)*phi1d];
Jzd = [-L2*cos(phi2)*phi2d -L3*cos(phi3)*phi3d; -L2*sin(phi2)*phi2d -L3*sin(phi3)*phi3d];
zdd = -inv(Jz)*(Jq*qdd + Jzd*zd + Jqd*qd);
phi2dd = zdd(1);
phi3dd = zdd(2);

%% WORLD COORDINATES %%

Axdd = ddd*cos(phi4);
Aydd = ddd*sin(phi4);

S1xdd = -L1*(cos(phi1)*phi1d^2 + sin(phi1)*phi1dd);
S1ydd = L1*(-sin(phi1)*phi1d^2 + cos(phi1)*phi1dd);
S2xdd = 2*S1xdd - L2*(cos(phi2)*phi2d^2 + sin(phi2)*phi2dd);
S2ydd = 2*S1ydd + L2*(-sin(phi2)*phi2d^2 + cos(phi2)*phi2dd);
S3xdd = Axdd + L3*(cos(phi3)*phi3d^2 + sin(phi3)*phi3dd);
S3ydd = Aydd - L3*(-sin(phi3)*phi3d^2 + cos(phi3)*phi3dd);

f1= [           -1             0              1             0             0             0              0             0 0 0];
f2= [            0            -1              0             1             0             0              0             0 0 0];
f3= [-L1*sin(phi1)  L1*cos(phi1)  -L1*sin(phi1)  L1*cos(phi1)             0             0              0             0 0 1];
f4= [            0             0             -1             0             1             0              0             0 0 0];
f5= [            0             0              0            -1             0             1              0             0 0 0];
f6= [            0             0  -L2*sin(phi2)  L2*cos(phi2) -L2*sin(phi2)  L2*cos(phi2)              0             0 0 0];
f7= [            0             0              0             0            -1             0              1             0 0 0];
f8= [            0             0              0             0             0            -1              0             1 0 0];
f9= [            0             0              0             0 -L3*sin(phi3)  L3*cos(phi3)  -L3*sin(phi3)  L3*cos(phi3) 0 0];
f10=[            0             0              0             0             0             0     -cos(phi4)    -sin(phi4) 1 0];

D = [f1;f2;f3;f4;f5;f6;f7;f8;f9;f10]';


%% INVERZNI DYNAMICKA ULOHA &&
ro = 2;
ma = 0.6;
m1 = L1*ro*2;
m2 = L2*ro*2;
m3 = L3*ro*2;
I1 = m1*4*L1^2/12;
I2 = m2*4*L2^2/12;
I3 = m3*4*L3^2/12;

M = diag([m1 m1 I1 m2 m2 I2 m3 m3 I3 ma]);
% Q =      [0  0  ma 0  0  0  0  0  0  cos(phi4)*F  sin(phi4)*F  0];
a = [S1xdd S1ydd phi1dd S2xdd S2ydd phi2dd S3xdd S3ydd phi3dd ddd]';
R = D'\(M*a);
% lambda = R(1:10);
Moment = R(9);
F = R(10);
output = [Moment; F];
end



% here assign values to R for each input configuration