Merge pull request #15 from NxRLab/dev

1.1.0 ready to release

packages/Matlab/README.md → packages/MATLAB/README.md

@@ -49,7 +49,7 @@ Author: Huan Weng, Bill Hunt, Mikhail Todes, Jarvis Schultz
 
 Contact: [email protected]
 
-Package Version: 1.0.1
+Package Version: 1.1.0
 
 Matlab Version: R2017b
 

packages/Matlab/mr/MatrixLog3.m → packages/MATLAB/mr/MatrixLog3.m

@@ -15,25 +15,23 @@
 %    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

packages/Matlab/mr/MatrixLog6.m → packages/MATLAB/mr/MatrixLog6.m

@@ -17,20 +17,14 @@
 %         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;
-              0, 0, 0, 0];
+    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

packages/Mathematica/ModernRobotics.m

@@ -11,7 +11,7 @@
 (* :Author: Huan Weng, Jarvis Schultz, Mikhail Todes*)
 (* :Contact: [email protected]*)
 (* :Summary: This package is the code library accompanying the book. *)
-(* :Package Version: 1.0.1 *)
+(* :Package Version: 1.1.0*)
 (* :Mathematica Version: 11.3 *)
 (* Tested in Mathematica 11.3 *)
 
@@ -53,7 +53,7 @@ Returns the inverse (transpose).
 		invR = RotInv[{{0,0,1},{1,0,0},{0,1,0}}]
 	Output: 
 		{{0,1,0},{0,0,1},{1,0,0}}"
-	
+
 
 VecToso3::usage=
 "VecToso3[omg]:
@@ -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 @@ Returns the corresponding space Jacobian (6xn real numbers).
 
 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,11 +1312,9 @@ Returns the corresponding space Jacobian (6xn real numbers).
 			]]
 		],
 		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]
+		Return[theta*omgmat]					
 	]
 ]
 
@@ -1408,20 +1407,18 @@ Returns the corresponding space Jacobian (6xn real numbers).
 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},
 			{0,0}
-		}]]
+		}]]	
 	]
 ]
 

packages/Python/modern_robotics/__version__.py

@@ -1,2 +1,2 @@
 
-__version__ = '1.0.1'
+__version__ = '1.1.0'

packages/Python/modern_robotics/core.py

@@ -6,7 +6,7 @@
 ***************************************************************************
 Author: Huan Weng, Bill Hunt, Jarvis Schultz, Mikhail Todes,
 Email: [email protected]
-Date: July 2018
+Date: January 2018
 ***************************************************************************
 Language: Python
 Also available in: MATLAB, Mathematica
@@ -158,9 +158,10 @@ def MatrixLog3(R):
                   [ 1.20919958,           0, -1.20919958],
                   [-1.20919958,  1.20919958,           0]])
     """
-    if NearZero(np.linalg.norm(R - np.eye(3))):
+    acosinput = (np.trace(R) - 1) / 2.0
+    if acosinput >= 1:
         return np.zeros((3, 3))
-    elif NearZero(np.trace(R) + 1):
+    elif acosinput <= -1:
         if not NearZero(1 + R[2][2]):
             omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \
                   * np.array([R[0][2], R[1][2], 1 + R[2][2]])
@@ -172,11 +173,6 @@ def MatrixLog3(R):
                   * np.array([1 + R[0][0], R[1][0], R[2][0]])
         return VecToso3(np.pi * omg)
     else:
-        acosinput = (np.trace(R) - 1) / 2.0
-        if acosinput > 1:
-            acosinput = 1
-        elif acosinput < -1:
-            acosinput = -1
         theta = np.arccos(acosinput)
         return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
 
@@ -392,27 +388,21 @@ def MatrixLog6(T):
                   [0,          0,           0,           0]])
     """
     R, p = TransToRp(T)
-    if NearZero(np.linalg.norm(R - np.eye(3))):
+    omgmat = MatrixLog3(R)
+    if np.array_equal(omgmat, np.zeros((3, 3))):
         return np.r_[np.c_[np.zeros((3, 3)),
                            [T[0][3], T[1][3], T[2][3]]],
                      [[0, 0, 0, 0]]]
     else:
-        acosinput = (np.trace(R) - 1) / 2.0
-        if acosinput > 1:
-            acosinput = 1
-        elif acosinput < -1:
-            acosinput = -1
-        theta = np.arccos(acosinput)
-        omgmat = MatrixLog3(R)
+        theta = np.arccos((np.trace(R) - 1) / 2.0)
         return np.r_[np.c_[omgmat,
                            np.dot(np.eye(3) - omgmat / 2.0 \
                            + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
-                             * np.dot(omgmat,omgmat) / theta,[T[0][3],
-                                                              T[1][3],
-                                                              T[2][3]])],
+                              * np.dot(omgmat,omgmat) / theta,[T[0][3],
+                                                               T[1][3],
+                                                               T[2][3]])],
                      [[0, 0, 0, 0]]]
 
-
 def ProjectToSO3(mat):
     """Returns a projection of mat into SO(3)