23 lines
712 B
Mathematica
23 lines
712 B
Mathematica
|
function T = MatrixExp6_Sym(se3mat)
|
||
|
|
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
|
||
|
|
% 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
|
||
|
|
|
||
|
|
T = expm(se3mat);
|
||
|
|
end
|