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