Matlab Lab3

syms dteta1 teta1 real

hat=@(x)([0 -x(3) x(2) x(4);...
    x(3) 0 -x(1)       x(5);...
    -x(2) x(1) 0       x(6);...
    0 0 0 0 ]);
u1=[0 0 0]';
w1=[1 0 0]';
p10=[0 0 2 1]';


s1=[w1; -cross(w1, u1)];


A1=expm(hat(s1)*teta1);

p1=A1*p10;


g = A1;
dg = hat(s1) * dteta1 * A1;
vb = inv(g)*dg;
vb = [vb(3,2) ; vb(1,3) ; vb(2,1); vb(1,4) ; vb(2,4) ; vb(3,4)];
vb = simplify(vb);


m = 1;
J = [ 0.33 0    0;...
      0    0.33 0;...
      0    0    0];

M = [J zeros(3);...
    zeros(3) m*eye(3)];
T = 0.5 * vb' * M * vb;
p1 = A1 * p10;
V = m * 9.81 * p1(3)
V = simplify(V);


///////////////////////////////////////////////////////////////////////////////
    function symwa
[t,x]=ode45(@robot1, 0:0.1:10, [pi/3 0])
for i=1:length(t)

    hat=@(x)([0 -x(3) x(2) x(4);...
    x(3) 0 -x(1)       x(5);...
    -x(2) x(1) 0       x(6);...
    0 0 0 0 ]);

u1=[0 0 0]';
w1=[1 0 0]';
p10=[0 0 2 1]';

s1=[w1; -cross(w1, u1)];
A1=expm(hat(s1)*x(i,1));
    p1 = A1 * p10;
    punkty = [ [0;0;0;1] p1 ]
    plot3( punkty(1,:), punkty(2,:), punkty(3,:) )
    axis([-1 1 -1 1 -1 1])

    pause(0.1)
end

function dx = robot1(t, x)

theta1 = x(1)
dtheta1 = x(2)


D = 33/100;
C = 0;
G = -9.81 * sin(theta1)
dx = [x(2) ;...
        inv(D) * (-C*x(2)-G-0.5*x(2))];