12 lines
345 B
Mathematica
12 lines
345 B
Mathematica
|
function m = SE3_transf(xi_hat,theta)
|
||
|
|
%SE3_TRANSF Summary of this function goes here
|
||
|
|
% w_hat = xi(1:3,1:3)
|
||
|
|
% v = xi(1:3,1)
|
||
|
|
% w = xi(4:6,1)
|
||
|
|
tmp1 = rodrig_formula(xi_hat(1:3,1:3), theta);
|
||
|
|
xi = tw2tw_cord(xi_hat);
|
||
|
|
m = [tmp1, (eye(3,3) - tmp1)*cross(xi(4:6,1), xi(1:3,1)) + xi(4:6,1)*xi(4:6,1)'*xi(1:3,1)*theta;
|
||
|
|
0, 0, 0, 1];
|
||
|
|
end
|
||
|
|
|