MatLab_Helix

clear all
close all

theta = [ pi/2; pi/2; pi/2 ] ;
Xhistory = zeros(1, 0);
Yhistory = zeros(1, 0);
Zhistory = zeros(1, 0);
thetahistory = zeros( 3, 0);
q = [0,0,0]' ;

a = [120,80];
R=100;
H=20;
dt=50;
umrundungen=5;

niter=10;

fig1 = figure( 'Position', [100, 100, 600, 600]);
for t = -umrundungen*pi:pi/dt:umrundungen*pi;
    figure( fig1);
    p=[R*cos(t);R*sin(t);H*t/pi];
    %inverse kinematic
    for i = 1:niter

        r = a(1) * [cos(theta(1)) * cos(theta(3)) ;
            cos(theta(1)) * sin(theta(3)) ;
            sin(theta(1))] ;

        s = a(2) * [cos(theta(1) + theta(2)) * cos(theta(3)) ;
            cos(theta(1) + theta(2)) * sin(theta(3)) ;
            sin(theta(1) + theta(2))] ;

        f =  q + r + s;

        F_theta1 = [-a(1) * sin(theta(1)) * cos(theta(3)) - a(2) * sin(theta(1) + theta(2)) * cos(theta(3)) ;
            -a(1) * sin(theta(1)) * sin(theta(3)) - a(2) * sin(theta(1) + theta(2)) * sin(theta(3)) ;
            a(1) * cos(theta(1)) + a(2) * cos(theta(1) + theta(2))] ;

        F_theta2 = [-a(2) * sin(theta(1) + theta(2)) * cos(theta(3)) ;
            -a(2) * sin(theta(1) + theta(2)) * sin(theta(3));
            a(2) * cos(theta(1) + theta(2))] ;


        F_theta3 = [-a(1)*cos(theta(1)) *sin(theta(3)) - a(2)*cos(theta(1) + theta(2) ) * sin(theta(3)) ;
            a(1)*cos(theta(1)) *cos(theta(3)) + a(2)*cos(theta(1) + theta(2) ) * cos(theta(3)) ;
            0 ] ;

        J = [F_theta1  F_theta2  F_theta3] ;

        dtheta = (J'*J) \ J' * (p-f);
        theta = theta + dtheta;
    end

    % save points that have been reached by end effector
    Xhistory( end+1) = f(1);
    Yhistory( end+1) = f(2);
    Zhistory( end+1) = f(3);
    thetahistory( :, end+1) = theta;

    % plot manipulator and point history
    plot3([20 0 r(1) f(1)], [0 0 r(2), f(2)],[0 0 r(3) f(3)], 'r', 'Linewidth', 3);
    %plot3([20 0 s(1) f(1)], [0 0 s(2), f(2)],[0 0 s(3) f(3)], 'r', 'Linewidth', 3);
    hold on
    grid on
    plot3( Xhistory', Yhistory', Zhistory', '.');
    hold off

    axis([-100, 100, -100, 100, -100, 100]);
end
% figure(2)
% plot( thetahistory(1,:));
% hold on
% plot( thetahistory(2,:), 'r');
% plot( thetahistory(3,:), 'g');
% hold off