3D Krane in Matlab

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]' ;%ursprung atm noch fest
a = [90,80];%armlänge
niter=100;%iterationen
t=0;


Q =[  0 60 60 50 50 20 20 30 30 40 40 30 30 20 20 50 50 60 60  0  0 10 10  0  0;...
      0  0 20 20 10 10 40 40 30 30 60 60 50 50 80 80 70 70 90 90 80 80 10 10  0;...
     20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20 20];
%Punkte=getCurve(Q,1);
%Punkte=getBallSurf(50,25,50);
%Punkte=getCubeWire(50,1);
Punkte=getCubeBetterWire(50,1);
%Punkte=getCubeSurf(50,10);
%Punkte=getCubeSolid(10,5);
%Punkte=getDog();
%Punkte=getDogSD();
%Punkte=getDogHD();
%Punkte=getHelix(100,100,100);
%Punkte=getTorus(50,30,50,25);
%Punkte=getBezierGrad3([100;100;100],[100;100;-100],[-100;-100;-100],0.01);
%Punkte=getBezierGrad4([100;100;100],[-100;100;-100],[-100;-100;-100],[-100;-100;100],0.01);
%Punkte=getBezierGrad5([100;100;100],[100;100;-100],[-100;100;-100],[100;-100;100],[-100;-100;-100],0.01);
%Punkte=getBezierGrad6([100;100;100],[100;100;-100],[-100;100;100],[100;-100;100],[-100;100;100],[-100;-100;-100],0.01);

anzahlDerPunkte=length(Punkte)
fig1 = figure( 'Position', [100, 100, 600, 600]);
for t =1:1:anzahlDerPunkte;
    figure( fig1);
    p=Punkte(:,t);
    %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([q(1)+20 q(1) r(1) f(1)], [q(2) q(2) r(2), f(2)],[q(3) q(3) r(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