## Attitudetrinterp.m ( File view )

• By ZHUZIZZQ 2015-01-29
• View(s)：25
• Point(s)： 0
```			%#codegen
function TransMatrix = Attitudetrinterp(StartMat, EndMat, PlanUnit)

TransMatrix = zeros(3,3,length(PlanUnit));
q0 = tr2q(StartMat);
q1 = tr2q(EndMat);

Transq = interp(q0, q1, PlanUnit);

for k = 1:length(PlanUnit)
TransMatrix(:,:,k) = q2tr(Transq(:,k));
end
end

function q = interp(q1, q2, r)
%Quaternion.interp Interpolate quaternions
%
% QI = Q1.interp(Q2, S) is a unit-quaternion that interpolates a rotation
% between Q1 for S=0 and Q2 for S=1.
%
% If S is a vector QI is a vector of quaternions, each element
% corresponding to sequential elements of S.
%
% Notes::
% - This is a spherical linear interpolation (slerp) that can be interpretted
%   as interpolation along a great circle arc on a sphere.
% - The value of S is clipped to the interval 0 to 1.
%

q = zeros(4,length(r)); % preallocate space for Quaternion vector
cosTheta = q1'*q2;
% take shortest path along the great circle, patch by Gauthier Gras
if cosTheta < 0
q1 = - q1;
cosTheta = - cosTheta;
end;

theta = acos(cosTheta);

% clip values of r
r(r<0) = 0;
r(r>1) = 1;

for i=1:length(r)
if theta == 0
q(:,i) = q1;
else
q(:,i) = (sin((1-r(i))*theta) * q1 + sin(r(i)*theta) * q2) / sin(theta);
end
end
end

function q = tr2q(t)
q = [1;0;0;0];

qs = sqrt(trace(t)+1)/2.0;
kx = t(3,2) - t(2,3);   % Oz - Ay
ky = t(1,3) - t(3,1);   % Ax - Nz
kz = t(2,1) - t(1,2);   % Ny - Ox

if (t(1,1) >= t(2,2)) && (t(1,1) >= t(3,3))
kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1
ky1 = t(2,1) + t(1,2);          % Ny + Ox
kz1 = t(3,1) + t(1,3);          % Nz + Ax
elseif (t(2,2) >= t(3,3))
kx1 = t(2,1) + t(1,2);          % Ny + Ox
ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1
kz1 = t(3,2) + t(2,3);          % Oz + Ay
else
kx1 = t(3,1) + t(1,3);          % Nz + Ax
ky1 = t(3,2) + t(2,3);          % Oz + Ay
kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1
end

kx = kx + kx1;
ky = ky + ky1;
kz = kz + kz1;
else
kx = kx - kx1;
ky = ky - ky1;
kz = kz - kz1;
end
nm = norm([kx ky kz]);
if nm == 0
q = [1 0 0 0]';
else
qs = sqrt(1 - qs^2) / nm;
q = [qs; qs*[kx; ky; kz]];
end
end

%Q2TR   Convert unit-quaternion to homogeneous transform
%
%   T = q2tr(Q)
%
%   Return the rotational homogeneous transform corresponding to the unit
%   quaternion Q.
%

function r = q2tr(q)

s = q(1);
x = q(2);
y = q(3);
z = q(4);

r = [   1-2*(y^2+z^2)   2*(x*y-s*z) 2*(x*z+s*y)
2*(x*y+s*z) 1-2*(x^2+z^2)   2*(y*z-s*x)
2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2)   ];

end
```
...
Expand＞ ＜Close

Point(s): 0

0 lines left, continue to read

## File list

Tips: You can preview the content of files by clicking file names^_^
Name Size Date
Attitudetrinterp.m3.32 kB2014-12-11 10:01
01.97 kB
...
• Sent successfully!
• 机器人姿态插补算法.zi
• 0 point

### Attitudetrinterp.m (1.52 kB)

Need 0 point

Tip: this source code project contains 2 packages, please click the allow button on the browser pop-up dialog,after you click the download button.

Don't have an account？ Register now
Need any help?
Mail to: support@codeforge.com

### 切换到中文版？

CodeForge Chinese Version
CodeForge English Version

### ^_^"Oops ...

Sorry!This guy is mysterious, its blog hasn't been opened, try another, please!

### Warm tip!

Favorite by Ctrl+D