fourtest.m

function [newa, newb, psi, err] = fourtest(links, connections, th)
% phi = angle of rotation for rotating joint
phi = pi/2 - th;
O = links(connections(1),:);
oa = links(connections(2),:)- O;
bc = links(connections(4),:) - links(connections(3),:);
a1 = norm(oa);
a2 = norm(links(connections(3),:) - links(connections(2),:));
a3 = norm(bc);
a4 = norm(links(connections(4),:) - O);
newa = rotate(th,'thx')*translate(oa)*[0;0;0;1];
minpotnorm = Inf;
for theta2 = 0:0.05:2*pi
    potb = newa(1:3)+[0;-a2*cos(theta2);a2*sin(theta2)];
    potnorm = norm(links(connections(4),:) - potb(:)');
    if potnorm - a3 <= 0.005
        newb = potb';
        newth = theta2;
        break
    elseif potnorm < minpotnorm
        newb = potb';
        newth = theta2;
        minpotnorm = potnorm;
    end
end

bc = newb - links(connections(4),:);
psi = atan(bc(3)/bc(2));
a1p = norm(newa(1:3)'- O);
a2p = norm(newb - newa(1:3)');
a3p = norm(newb - links(connections(4),:));
err = [a1-a1p;a2-a2p;a3-a3p];
newa = newa(1:3)';
end