Code covered by the BSD License

# Toolbox kinematics

### Daniel Klawitter (view profile)

29 Mar 2010 (Updated )

This toolbox deals with basic concepts in kinematics and special interpolation algorithms.

Dualquaternion.m
classdef Dualquaternion
%Dualquaternion - class for calculations with dual quaternions

% o history
%   Daniel Klawitter
%   created 20-10-2009 - 17:00
%
% o summary
%   a dual quaternion or biquaternion contains two objects of
%   quaternion class (see Quaternion.m)
%   Q_d = Q + epsilon * Q
%   methods for multiplication, addition and so on are implemented
%
%   dual quaternions can represent spatial displacements
%   functions for
%       o dual translation quaternion creation
%       o dual rotation quaternion creation
%       o dual spatial displacement quaternion creation
%   are implemented in class HomogeneousVector
%
% o TODO:
%

properties
realpart % this property is a quaternion, so it also have subproperties scal and vect
dualpart % this property is a quaternion, so it also have subproperties scal and vect
end

methods
% constructor with no input or two quaternions
function obj = Dualquaternion(q1,q2)
if nargin == 0
obj.realpart = Quaternion;
obj.dualpart = Quaternion;
else
obj.realpart = q1;
obj.dualpart = q2;
end
end

% overload of the plus operation '+'
function obj = plus(a,b)
obj = Dualquaternion;
obj.realpart = a.realpart + b.realpart;
obj.dualpart = a.dualpart + b.dualpart;
end

% overload of the minus operation '-'
function obj = minus(a,b)
obj = Dualquaternion;
obj.realpart = a.realpart - b.realpart;
obj.dualpart = a.dualpart - b.dualpart;
end

% overload of the multiplication sign '*'
% succesfully tested
function obj = mtimes(a,b)
obj = Dualquaternion;
obj.realpart.scal = a.realpart.scal * b.realpart.scal - (a.realpart.vect * b.realpart.vect');
obj.dualpart.scal = a.realpart.scal * b.dualpart.scal + a.dualpart.scal * b.realpart.scal...
-(a.dualpart.vect*b.realpart.vect' + a.realpart.vect*b.dualpart.vect');
obj.realpart.vect =   a.realpart.scal.*b.realpart.vect + ...
b.realpart.scal.*a.realpart.vect +...
cross(a.realpart.vect,b.realpart.vect);
obj.dualpart.vect = a.realpart.scal .* b.dualpart.vect + a.dualpart.scal.*b.realpart.vect +...
b.realpart.scal.*a.dualpart.vect + b.dualpart.scal.*a.realpart.vect +...
cross(a.dualpart.vect,b.realpart.vect)+cross(a.realpart.vect,b.dualpart.vect);
end

% overload of the multiplication with scalars '.*'
function obj = times(a,b)
% multiply with a dual scalar
if isa(a,'DualNumber')
obj = Dualquaternion;
obj.realpart.scal = a.real*b.realpart.scal;
obj.realpart.vect = a.real.*b.realpart.vect;
obj.dualpart.scal = a.dual*b.realpart.scal;
obj.dualpart.vect = a.dual.*b.realpart.vect;
% multiply with a real scalar
else
obj = Dualquaternion;
obj.realpart = a.*b.realpart;
obj.dualpart = a.*b.dualpart;
end
end

% calculate the conjugated quaternion
function obj = conjugate(a)
obj = Dualquaternion;
obj.realpart.scal = a.realpart.scal;
obj.dualpart.scal = a.dualpart.scal;
obj.realpart.vect = -a.realpart.vect;
obj.dualpart.vect = -a.dualpart.vect;
end

% calculate the dual conjugated quaternion
function obj = dualconjugate(a)
obj = Dualquaternion;
obj.realpart.scal = a.realpart.scal;
obj.dualpart.scal = -a.dualpart.scal;
obj.realpart.vect = a.realpart.vect;
obj.dualpart.vect = -a.dualpart.vect;
end

% note that this norm does not define a norm on H_d
function obj = norm(a)
obj = Dualquaternion;
obj = a*conjugate(a);
% the norm to the power of 2 is a dual number (look class DualNumber)
obj = DualNumber(obj.realpart.scal,a.dualpart.scal);
% uses the sqrt-function of the class DualNumber
obj = sqrt(obj);
end

% returns the translationla part of a dual quaternion that
% represents a spatial displacement
% returns dual quaternion
function obj = getTranslation(a)
obj = a*Dualquaternion(conjugate(a.realpart),Quaternion);
obj = 1/(obj.realpart.scal/2).*obj;
end

% returns dual quaternion that represents a pure rotation
function obj = getRotation(a)
% these two operations are on Quaternions
% for formula see Jttler - Visualization of moving onjects
obj = inv(a.realpart*conjugate(a.realpart)) * a.realpart;

% transform to dual Quaternion
obj = Dualquaternion(obj,Quaternion);
end

% default: depends on the dual quaternion
function obj = displace(Q,x)
obj = Q*x*dualconjugate(conjugate(Q));
%             obj = normalize(obj);
end

% function that calculates the spatial displacement
% if user wants to take influence on the order of the seperate
% operations
% first rotate the translate
function obj = displaceRT(Q,x)
T = getTranslation(Q);
R = getRotation(Q);
obj = R*x*dualconjugate(conjugate(R));
obj = T*obj*dualconjugate(conjugate(T));
%             obj = normalize(obj);
end

% first tranlate teh rotate
function obj = displaceTR(Q,x)
T = getTranslation(Q);
R = getRotation(Q);
obj = T*x*dualconjugate(conjugate(T));
obj = R*obj*dualconjugate(conjugate(R));
%             obj = normalize(obj);
end

function study = getStudyParameters(obj)
study(1)  = obj.realpart.scal;
study(2:4)= obj.realpart.vect;
study(5)  = obj.dualpart.scal;
study(6:8)= obj.dualpart.vect;
study = STUDYparameter(study);

end

% function to bring the realpart.scal property to the value 1
function obj = normalize(a)
obj = (1/sqrt(a.realpart.scal^2 + sum(a.realpart.vect.^2) ) ).*a;
end

function disp(obj)
disp('class Dualquaternion');
disp([num2str(obj.realpart.scal),'+ ',num2str(obj.realpart.vect(1)),'i+ ',num2str(obj.realpart.vect(2)),'j+ ',num2str(obj.realpart.vect(3)),'k+ epsilon*(',...
num2str(obj.dualpart.scal),'+ ',num2str(obj.dualpart.vect(1)),'i+ ',num2str(obj.dualpart.vect(2)),'j+ ',num2str(obj.dualpart.vect(3)),'k)'])
end

function erg = latex1(a)
erg=[num2str(a.realpart.scal),'+',num2str(a.dualpart.scal),'\epsilon+(',num2str(a.realpart.vect(1)),'+',num2str(a.dualpart.vect(1)),'\epsilon)\mathbf{i}',...
'+(',num2str(a.realpart.vect(2)),'+',num2str(a.dualpart.vect(2)),'\epsilon)\mathbf{j}','+(',num2str(a.realpart.vect(3)),'+',num2str(a.dualpart.vect(3)),'\epsilon)\mathbf{k}'];
end

function erg = latex2(a)
erg=[num2str(a.realpart.scal),'+',num2str(a.realpart.vect(1)),'\mathbf{i}+',num2str(a.realpart.vect(2)),'\mathbf{j}+',num2str(a.realpart.vect(3)),'\mathbf{k}',...
'+(',num2str(a.dualpart.scal),'+',num2str(a.dualpart.vect(1)),'\mathbf{i}+',num2str(a.dualpart.vect(2)),'\mathbf{j}+',num2str(a.dualpart.vect(3)),'\mathbf{k})\epsilon',];
end
end

end