Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Debug MatrixLog3 and MatrixLog6 for MATLAB and Mathematica

This commit is contained in:
HuanWeng 2018-12-24 02:05:24 -06:00
parent 8a8ebe1e9c
commit 21586238aa
3 changed files with 28 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
packages/Mathematica/ModernRobotics.m
View File

@ -300,7 +300,7 @@ Returns the corresponding se(3) representation of exponential coordinates.
Example:
Input:
se3mat = MatrixLog6[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]
se3mat = MatrixLog6[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]//N
Output:
{{0,0,0,0},{0,0,-1.5708,2.35619},{0,1.5708,0,2.35619},{0,0,0,0}}"
@ -1289,10 +1289,11 @@ MatrixExp3[so3mat_]:=Module[
MatrixLog3[R_]:=Module[
{acosinput,theta,omgmat},
acosinput=(Tr[R]-1)/2;
Which[
NearZero[Norm[R-IdentityMatrix[3]]],
acosinput>=1,
Return[ConstantArray[0,{3,3}]],
NearZero[Tr[R]+1],
acosinput<=-1,
Which[
Not[NearZero[R[[3,3]]+1]],
Return[VecToso3[
@ -1311,8 +1312,6 @@ MatrixLog3[R_]:=Module[
]]
],
True,
acosinput=(Tr[R]-1)/2;
Which[acosinput>1,acosinput=1,acosinput<-1,acosinput=-1];
theta=ArcCos[acosinput];
omgmat=(R-R\[Transpose])/(2*Sin[theta]);
Return[theta*omgmat]
@ -1408,15 +1407,13 @@ MatrixExp6[se3mat_]:=Module[
MatrixLog6[T_]:=Module[
{R,p,acosinput,theta,omgmat},
{R,p}=TransToRp[T];
omgmat=MatrixLog3[R];
If[
NearZero[Norm[R-IdentityMatrix[3]]],
omgmat==ConstantArray[0,{3,3}],
ArrayFlatten[
{{ConstantArray[0,{3,3}],{T[[1;;3,4]]}\[Transpose]},{0,0}}
],
acosinput=(Tr[R]-1)/2;
Which[acosinput>1,acosinput=1,acosinput<-1,acosinput=-1];
theta=ArcCos[acosinput];
omgmat=MatrixLog3[R];
theta=ArcCos[(Tr[R]-1)/2];
Return[ArrayFlatten[{
{omgmat,(IdentityMatrix[3]-omgmat/2+
(1/theta-Cot[theta/2]/2)*omgmat.omgmat/theta).p},

24
packages/Matlab/mr/MatrixLog3.m
View File

@ -15,25 +15,23 @@ function so3mat = MatrixLog3(R)
% 1.2092 0 -1.2092
% -1.2092 1.2092 0
if NearZero(norm(R - eye(3)))
so3mat = zeros(3);
elseif NearZero(trace(R) + 1)
acosinput = (trace(R) - 1) / 2;
if acosinput >= 1
so3mat = zeros(3);
elseif acosinput <= -1
if ~NearZero(1 + R(3, 3))
omg = (1 / sqrt(2 * (1 + R(3, 3)))) * [R(1, 3); R(2, 3); 1 + R(3, 3)];
omg = (1 / sqrt(2 * (1 + R(3, 3)))) ...
* [R(1, 3); R(2, 3); 1 + R(3, 3)];
elseif ~NearZero(1 + R(2, 2))
omg = (1 / sqrt(2 * (1 + R(2, 2)))) * [R(1, 2); 1 + R(2, 2); R(3, 2)];
omg = (1 / sqrt(2 * (1 + R(2, 2)))) ...
* [R(1, 2); 1 + R(2, 2); R(3, 2)];
else
omg = (1 / sqrt(2 * (1 + R(1, 1)))) * [1 + R(1, 1); R(2, 1); R(3, 1)];
omg = (1 / sqrt(2 * (1 + R(1, 1)))) ...
* [1 + R(1, 1); R(2, 1); R(3, 1)];
end
so3mat = VecToso3(pi * omg);
else
acosinput = (trace(R) - 1) / 2;
if acosinput > 1
acosinput = 1;
elseif acosinput < -1
acosinput = -1;
end
theta = acos(acosinput);
theta = acos(acosinput);
so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
end
end

18
packages/Matlab/mr/MatrixLog6.m
View File

@ -17,20 +17,14 @@ function expmat = MatrixLog6(T)
% 0 0 0 0
[R, p] = TransToRp(T);
if NearZero(norm(R - eye(3)))
omgmat = MatrixLog3(R);
if isequal(omgmat, zeros(3))
expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
else
acosinput = (trace(R) - 1) / 2;
if acosinput > 1
acosinput = 1;
elseif acosinput < -1
acosinput = -1;
end
theta = acos(acosinput);
omgmat = MatrixLog3(R);
expmat = [ omgmat, (eye(3) - omgmat / 2 ...
+ (1 / theta - cot(theta / 2) / 2) ...
* omgmat * omgmat / theta) * p;
theta = acos((trace(R) - 1) / 2);
expmat = [omgmat, (eye(3) - omgmat / 2 ...
+ (1 / theta - cot(theta / 2) / 2) ...
* omgmat * omgmat / theta) * p;
0, 0, 0, 0];
end
end