kinematic

figure;  
h = plot3(0,0,0);
p = get(h,'Parent');
xlim(p,'manual');  
xlim(p,[-2 2]);
ylim(p,'manual');
ylim(p,[-2 2]);
zlim(p,'manual');
zlim(p,[-2 2]);
axis vis3d;grid on;
point1.x =0;point1.y = 0;point1.z = 0;
point2.x = 0;point2.y = 0;point2.z = 0;
point3.x = 0;point3.y = 0;point3.z = 0;
l= line([point1.x, point2.x],[point1.y,point2.y],[point1.z,point2.z],'Color','r','LineWidth',4);
l1 = line([point2.x, point3.x],[point2.y,point3.y],[point2.z,point3.z],'Color','b','LineWidth',4);
hold on;
plot3(point3.x,point3.y,point3.z,'xk');

theta2=atan2(...
	abs(x^3+x*y^2-sqrt(4*x^2*(y^2)-(x^4)*y^2+4*y^4-2*x^2*(y^4)-y^6))/(2*(x^2 + y^2)),abs(...
	(x^2+y^2-x^4/(x^2+y^2)-(x^2*(y^2))/(x^2+y^2)+(x*...
		sqrt(-(y^2)*(-4*x^2+x^4-4*y^2+2*x^2*(y^2)+y^4)))/(x^2+y^2))/(2*y)))*180




theta3=atan2((1/2)*(2*x-x^3/(x^2+y^2)-(x*y^2)/(x^2+y^2)+...
	abs(sqrt(-(y^2)*(-4*x^2+x^4-4*y^2+2*x^2*y^2+y^4))/(x^2+y^2))),...
	(-x^2+y^+x^4/(x^2+y^2)+(x^2*(y^2))/(x^2+y^2)-(x*...
		abs(sqrt(-(y^2)*(-4*x^2+x^4-4*y^2+2*x^2*(y^2)+y^4)))/(x^2+y^2))/(2*y)))*180+theta2


[point2,point3] = fromDHandTheta(theta1,theta2,theta3) %(each individual theta)
set(l,'ZData',[point1.z,point2.z],'YData',[point1.y,point2.y],'XData',[point1.x,point2.x]);
set(l1,'ZData',[point2.z,point3.z],'YData',[point2.y,point3.y],'XData',[point2.x,point3.x]);