35 lines
1.1 KiB
Matlab
35 lines
1.1 KiB
Matlab
%*** CHAPTER 3: RIGID-BODY MOTIONS ***
|
|
|
|
function T = MatrixExp6(se3mat)
|
|
% Takes a se(3) representation of exponential coordinates.
|
|
% Returns a T matrix in SE(3) that is achieved by traveling along/about the
|
|
% screw axis S for a distance theta from an initial configuration T = I.
|
|
% Example Input:
|
|
%{
|
|
clear; clc;
|
|
se3mat = [ 0, 0, 0, 0;
|
|
0, 0, -1.5708, 2.3562;
|
|
0, 1.5708, 0, 2.3562;
|
|
0, 0, 0, 0]
|
|
T = MatrixExp6(se3mat)
|
|
%}
|
|
% Output:
|
|
% T =
|
|
% 1.0000 0 0 0
|
|
% 0 0.0000 -1.0000 -0.0000
|
|
% 0 1.0000 0.0000 3.0000
|
|
% 0 0 0 1.0000
|
|
|
|
omgtheta = so3ToVec(se3mat(1: 3, 1: 3));
|
|
if NearZero(norm(omgtheta))
|
|
T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1];
|
|
else
|
|
[omghat, theta] = AxisAng3(omgtheta);
|
|
omgmat = se3mat(1: 3, 1: 3) / theta;
|
|
T = [MatrixExp3(se3mat(1: 3, 1: 3)), ...
|
|
(eye(3) * theta + (1 - cos(theta)) * omgmat ...
|
|
+ (theta - sin(theta)) * omgmat * omgmat) ...
|
|
* se3mat(1: 3, 4) / theta;
|
|
0, 0, 0, 1];
|
|
end
|
|
end |