Code covered by the BSD License

### Highlights fromKinematics Toolbox

from Kinematics Toolbox by Brad Kratochvil
The kinematics toolbox is intended for prototyping robotics and computer vision related tasks.

noisetwist(xi, np, nt)
```function out = noisetwist(xi, np, nt)
%NOISETWIST  applies uniformly distributed noise to a twist
%
%	OUT = NOISETWIST(XI)
%	OUT = NOISETWIST(XI, NP)
%	OUT = NOISETWIST(XI, NP, NT)
%
% XI is the twist; NP is the magnitude of noise to the rotational
% component of XI; NT is the magnitude of noise to the translational
% compoenent of XI.
%

% \$Id: noisetwist.m,v 1.1 2009-03-17 16:40:18 bradleyk Exp \$

switch nargin
case 0
out = randtwist();
return;
case 1
np = 0.1;
nt = 0.1;
case 2
nt = np;
end

if ~istwist(twist(xi)),
error('SCREWS:twist', 'xi must be a twist');
end

z = zeros(3,1);

% we seperate these to keep w/ level 2 calibration errors
if isequalf(z, xi(4:6,1)), % pure translation
out(1:3,1) = xi(1:3) + rand(3,1)*np;
out(4:6,1) = z(3,1);
elseif isequalf(0, xi(4:6,1)'*xi(1:3,1)), % pure rotation
out(4:6,1) = noiseskew(xi(4:6), nt);
% let's new a new point q to keep this revolute
[q flag] = lsqr(skew(out(4:6,1)),xi(1:3,1));
out(1:3, 1) = cross(out(4:6,1),q);
else
out(1:3,1) = xi(1:3) + rand(3,1)*np;
out(4:6,1) = noiseskew(xi(4:6), nt);
end

if ~isequal(zeros(6,1), isnan(out)) || ~isequal(zeros(6,1), isinf(out)),
warning('SCREWS:twist', 'problem applying noise, trying again');
% recursively try again
out = noisetwist(xi,np,nt);
end

if ~istwist(twist(out)),
error('SCREWS:twist', 'error applying noise');
end

end```