% This file runs the toolbox through a series of tests.

It's good to
% verify the functionality of the toolbox when individual functions are
% changed.
clear;
global DebugLevel;
DebugLevel = 0;
fprintf('starting screws test\n');
syms b_1 b_2 b_3 real;
a = [1;0;0];
b = [b_1;b_2;b_3];
a_hat = skew(a);
b_hat = skew(b);
% test pack
c = skewcoords(a_hat);
% random test
e = randskew();
e_hat = skew(e);
f = skewcoords(e_hat);
%% equality tests
if ~isequalf(a, c),
error('problem with skew pack/unpack');
end
if ~isequalf(e, f);
error('problem with skew pack/unpack');
end
if ~isequal(a_hat', -a_hat),
error('problem with skew');
end
if ~isequal(b_hat', -b_hat),
error('problem with skew');
end
if ~isequal(e_hat', -e_hat),
error('problem with skew');
end
%% Test twists
syms w_1 w_2 w_3 v_1 v_2 v_3 theta real;
o = createtwist([0; 0; 1], [2; 0; 0]);
o_t = createtwist([0; 0; 0], [1; 0; 0]);
p = createtwist([w_1; w_2; w_3], [v_1; v_2; v_3]);
p_t = createtwist([0; 0; 0], [v_1; v_2; v_3]);
q = randtwist('s');
q_t = randtwist('t');
o_hat = twist(o);
q_hat = twist(q);
if o ~= twistcoords(o_hat);
error('problem with twist pack/unpack');
end
if ~isequalf(q,twistcoords(q_hat));
error('problem with twist pack/unpack');
end
%% Test matrix exponentials
% skews
e_a = skewexp(a_hat, 1);
e_e = fast_skewexp(skewcoords(e_hat));
% check against matlab expm function
if ~isequalf(e_a, expm(a_hat)),
error('problem with skewexp operator');
end
if ~isequalf(e_e, expm(e_hat)),
error('problem with skewexp operator');
end
%% twists
e_o_r = twistexp(o_hat, pi);
e_o_t = twistexp(twist(o_t), pi);
e_q_r = twistexp(q_hat, 1);
e_q_t = twistexp(twist(q_t), 1);
% check against matlab expm function
if ~isequalf(e_o_r, expm(pi*o_hat)),
error('problem with twistexp operator');
end
if ~isequalf(e_o_t, expm(twist(pi*o_t))),
error('problem with twistexp operator');
end
if ~isequalf(e_q_r, expm(q_hat)),
error('problem with twistexp operator');
end
if ~isequalf(e_q_t, expm(twist(q_t))),
error('problem with twistexp operator');
end
%% Test the adjoint operator
X = e_q_r;
Y = e_q_t;
% we do this numerically because symbolically takes too much time.
if ~isequalf(inv(ad(X)), ad(inv(X))),
error('problem with inverting adjoint operator');
end
if ~isequalf(inv(ad(Y)), ad(inv(Y))),
error('problem with inverting adjoint operator');
end
if ~isequalf(ad(X)*ad(X), ad(X*X)),
error('problem with adjoint operator associativity');
end
if ~isequalf(ad(Y)*ad(Y), ad(Y*Y)),
error('problem with adjoint operator associativity');
end
if ~isequalf(ad(X)*ad(Y), ad(X*Y)),
error('problem with adjoint operator associativity');
end
%% test the is functions
if ~isskew(e_hat),
error('problem with isskew');
end
if ~isrot(e_a),
error('problem with isrot');
end
if ~istwist(o_hat),
error('problem with istwist');
end
%% Test homtotwist amd twistlog
for j=1:100,
q_hat = randtwist();
angle = 0:0.1*pi:2*pi;
for i=1:size(angle, 2),

h = twistexp(q_hat, angle(i));
[xi theta] = homtotwist(h);
[xi_hat] = twistlog(h);
if ~isequalf(twistexp(xi, theta), h, 6),
error('homtotwist test failed')
end
if ~isequalf(twistexp(xi_hat), h, 6),
error('twistlog test failed')
end
end
end
%% let's do these tests a few times
for i=1:100,
% adjoint test
 a = twist(randtwist());
b = twist(randtwist());
c = twist(randtwist());
test1 = ad(expm(a))*ad(expm(b))*ad(expm(c));
test2 = ad(expm(a)*expm(b)*expm(c));
if ~isequalf(test1,test2),
error('adjoint test not the same');
end
% multiplication test
r = rand(3,1);
test3 = [twistcoords(a) twistcoords(b) twistcoords(c)]*r;
test4 = twistcoords(a*r(1) + b*r(2) + c*r(3));
if ~isequalf(test3, test4),
error('problem with multiplication test');
end
end
%% test graphics
named_figure('skew');
for i=1:10,
clf;
drawskewtraj(randskew(),0:pi/20:1/2*pi);
nice3d;
end
named_figure('twist');
for i=1:10,
clf;
drawtwisttraj(randtwist(),0:pi/20:1/2*pi);
nice3d;
end
named_figure('3 DOF robot');
clf;
r = robot({randtwist('r');randtwist('t');randtwist('s')},randhom());
fk = fkine(r, [0:pi/50:pi ; 0:pi/25:2*pi ; 0:pi/25:2*pi]);
animframetraj(fk);
% if we want to save a movie
% clf;
% animframetraj(fk, 1.0, '/tmp', 'example_frame_traj_movie');
fprintf('test screws success!!\n\n');