Changed the function order

packages/Mathematica/ModernRobotics.m

@@ -121,6 +121,51 @@ Example:
 		{{0,-1.2092,1.2092},{1.2092,0,-1.2092},{-1.2092,1.2092,0}}"
 
 
+DistanceToSO3::usage=
+"DistanceToSO3[mat]:
+Takes mat as a 3x3 matrix.
+Returns the Frobenius norm to describe the distance of mat from the SO(3) 
+manifold.
+Computes the distance from mat to the SO(3) manifold using the following 
+method: If det(mat) <= 0, return a large number. If det(mat) > 0, return 
+norm(mat'.mat - I).
+
+Example:
+	Input: 
+		d = DistanceToSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}]
+	Output: 
+		0.088353"
+
+
+TestIfSO3::usage=
+"TestIfSO3[mat]:
+Takes mat as a 3x3 matrix.
+Check if mat is close to or on the manifold SO(3).
+
+Example:
+	Input: 
+		judge = TestIfSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}]
+	Output: 
+		False"
+
+
+ProjectToSO3::usage=
+"ProjectToSO3[mat]:
+Takes a matrix near SO(3) to project to SO(3).
+Returns the closest rotation matrix that is in SO(3).
+This function uses singular-value decomposition (see
+http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
+and is only appropriate for matrices close to SO(3).
+
+Example:
+	Input: 
+		R = ProjectToSO3[{{0.675,0.150,0.720},{0.370,0.771,-0.511},
+						  {-0.630,0.619,0.472}}]//N
+	Output: 
+		{{0.679011,0.148945,0.718859},{0.373207,0.773196,-0.512723}, 
+		 {-0.632187,0.616428,0.469421}}"
+
+
 RpToTrans::usage=
 "RpToTrans[R,p]:
 Takes rotation matrix R and position p.
@@ -260,21 +305,35 @@ Example:
 		{{0,0,0,0},{0,0,-1.5708,2.35619},{0,1.5708,0,2.35619},{0,0,0,0}}"
 
 
-ProjectToSO3::usage=
+DistanceToSE3::usage=
-"ProjectToSO3[mat]:
+"DistanceToSE3[mat]:
-Takes a matrix near SO(3) to project to SO(3).
+Takes mat as a 4x4 matrix.
-Returns the closest rotation matrix that is in SO(3).
+Returns the Frobenius norm to describe the distance of mat from the SE(3) 
-This function uses singular-value decomposition (see
+manifold.
-http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
+Computes the determinant of matR, the top 3x3 submatrix of mat. If 
-and is only appropriate for matrices close to SO(3).
+det (matR) <= 0, return a large number. If det (mat) > 0, replace the top 3x3
+submatrix of mat with matR'.matR, and set the first three entries of the 
+fourth column of mat to zero. Then return norm (mat - I).
+
+Example :
+	Input: 
+		d = DistanceToSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5},
+						   {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}]
+	Output: 
+		0.134931"
+
+
+TestIfSE3::usage=
+"TestIfSE3[mat]:
+Takes mat as a 4X4 matrix.
+Check if mat is close to or on the manifold SE(3).
 
 Example:
 	Input: 
-		R = ProjectToSO3[{{0.675,0.150,0.720},{0.370,0.771,-0.511},
+		judge = TestIfSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5},
-						  {-0.630,0.619,0.472}}]//N
+						   {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}]
 	Output: 
-		{{0.679011,0.148945,0.718859},{0.373207,0.773196,-0.512723}, 
+		False"
-		 {-0.632187,0.616428,0.469421}}"
 
 
 ProjectToSE3::usage=
@@ -296,65 +355,6 @@ Example:
 		 {-0.632187,0.616428,0.469421,3.6},{0,0,0,1}}"
 
 
-DistanceToSO3::usage=
-"DistanceToSO3[mat]:
-Takes mat as a 3x3 matrix.
-Returns the Frobenius norm to describe the distance of mat from the SO(3) 
-manifold.
-Computes the distance from mat to the SO(3) manifold using the following 
-method: If det(mat) <= 0, return a large number. If det(mat) > 0, return 
-norm(mat'.mat - I).
-
-Example:
-	Input: 
-		d = DistanceToSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}]
-	Output: 
-		0.088353"
-
-
-DistanceToSE3::usage=
-"DistanceToSE3[mat]:
-Takes mat as a 4x4 matrix.
-Returns the Frobenius norm to describe the distance of mat from the SE(3) 
-manifold.
-Computes the determinant of matR, the top 3x3 submatrix of mat. If 
-det (matR) <= 0, return a large number. If det (mat) > 0, replace the top 3x3
-submatrix of mat with matR'.matR, and set the first three entries of the 
-fourth column of mat to zero. Then return norm (mat - I).
-
-Example :
-	Input: 
-		d = DistanceToSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5},
-						   {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}]
-	Output: 
-		0.134931"
-
-
-TestIfSO3::usage=
-"TestIfSO3[mat]:
-Takes mat as a 3x3 matrix.
-Check if mat is close to or on the manifold SO(3).
-
-Example:
-	Input: 
-		judge = TestIfSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}]
-	Output: 
-		False"
-
-
-TestIfSE3::usage=
-"TestIfSE3[mat]:
-Takes mat as a 4X4 matrix.
-Check if mat is close to or on the manifold SE(3).
-
-Example:
-	Input: 
-		judge = TestIfSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5},
-						   {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}]
-	Output: 
-		False"
-
-
 (* ::Subsection::Closed:: *)
 (*CHAPTER 4: FORWARD KINEMATICS*)
 
@@ -1320,6 +1320,29 @@ MatrixLog3[R_]:=Module[
 ]
 
 
+DistanceToSO3[mat_]:=If[
+	Det[mat]>0, 
+	Return[Norm[mat\[Transpose].mat-IdentityMatrix[3],"Frobenius"]], 
+	Return[10^9]
+]
+
+
+TestIfSO3[mat_]:=Abs[DistanceToSO3[mat]]<10^-3
+
+
+ProjectToSO3[mat_]:=Module[
+	{R,u,w,v}, 
+	{u,w,v}=SingularValueDecomposition[mat]; 
+	R=u.v\[Transpose];
+	(*In the case below, the result may be far from mat.*)
+	If[
+		Det[R] < 0, 
+		Return[ArrayFlatten[{{R[[;;,1;;2]],{-R[[;;,3]]}\[Transpose]}}]],
+		Return[R]
+	]
+]
+
+
 RpToTrans[R_,p_]:=ArrayFlatten[{{R,p},{0,1}}]
 
 
@@ -1403,30 +1426,6 @@ MatrixLog6[T_]:=Module[
 ]
 
 
-ProjectToSO3[mat_]:=Module[
-	{R,u,w,v}, 
-	{u,w,v}=SingularValueDecomposition[mat]; 
-	R=u.v\[Transpose];
-	(*In the case below, the result may be far from mat.*)
-	If[
-		Det[R] < 0, 
-		Return[ArrayFlatten[{{R[[;;,1;;2]],{-R[[;;,3]]}\[Transpose]}}]],
-		Return[R]
-	]
-]
-
-
-ProjectToSE3[mat_]:=RpToTrans[ProjectToSO3[mat[[1;;3,1;;3]]],
-							  {mat[[1;;3,4]]}\[Transpose]]
-
-
-DistanceToSO3[mat_]:=If[
-	Det[mat]>0, 
-	Return[Norm[mat\[Transpose].mat-IdentityMatrix[3],"Frobenius"]], 
-	Return[10^9]
-]
-
-
 DistanceToSE3[mat_]:=Module[
 	{R,p}, 
 	{R,p}=TransToRp[mat]; 
@@ -1442,12 +1441,13 @@ DistanceToSE3[mat_]:=Module[
 ]
 
 
-TestIfSO3[mat_]:=Abs[DistanceToSO3[mat]]<10^-3
-
-
 TestIfSE3[mat_]:=Abs[DistanceToSE3[mat]]<10^-3
 
 
+ProjectToSE3[mat_]:=RpToTrans[ProjectToSO3[mat[[1;;3,1;;3]]],
+							  {mat[[1;;3,4]]}\[Transpose]]
+
+
 (* ::Subsection::Closed:: *)
 (*CHAPTER 4: FORWARD KINEMATICS*)
 
@@ -1815,10 +1815,10 @@ End[];
 
 Protect["NearZero"];
 Protect["RotInv","VecToso3","so3ToVec","AxisAng3","MatrixExp3","MatrixLog3",
-		"RpToTrans","TransToRp","TransInv","VecTose3","se3ToVec","Adjoint",
+		"DistanceToSO3","TestIfSO3","ProjectToSO3","RpToTrans","TransToRp",
-		"ScrewToAxis","AxisAng6","MatrixExp6","MatrixLog6","ProjectToSO3",
+		"TransInv","VecTose3","se3ToVec","Adjoint","ScrewToAxis","AxisAng6",
-		"ProjectToSE3","ProjectToSE3","DistanceToSE3","TestIfSO3",
+		"MatrixExp6","MatrixLog6","DistanceToSE3","TestIfSE3",
-		"TestIfSE3"];
+		"ProjectToSE3"];
 Protect["FKinBody","FKinSpace"];
 Protect["JacobianBody","JacobianSpace"];
 Protect["IKinBody","IKinSpace"];