Make logR work also when the total rotation is pi rad (#31)

* Add tests for logR that currently fails

* Add special version of logR for the case when the rotation is close to pi rad

Co-authored-by: Andreas Stolt <andreas.stolt@cognibotics.org>

Author: andreasstolt

src/utils.jl

@@ -117,6 +117,9 @@ end
 function logR(R)
     @assert isrot(R)
     phi = acos((min(tr(R),3)-1)/2)
+    if abs(phi-π) < 1e-5
+        return logR_special(R)
+    end
     if abs(phi) < 1e-10
         ω = (R-R')/2
     else
@@ -125,6 +128,51 @@ function logR(R)
     return ω
 end
 
+"""Special variant of logR that handles the case when the rotation is π rad"""
+function logR_special(R)
+    qs = sqrt(abs(tr(R) + 1))/2
+    kx = R[3, 2] - R[2, 3]
+    ky = R[1, 3] - R[3, 1]
+    kz = R[2, 1] - R[1, 2]
+
+    if R[1, 1] >= R[2, 2] && R[1, 1] >= R[3, 3]
+        kx1 = R[1, 1] - R[2, 2] - R[3, 3] + 1
+        ky1 = R[2, 1] + R[1, 2]
+        kz1 = R[3, 1] + R[1, 3]
+        add = kx >= 0
+    elseif R[2, 2] >= R[3, 3]
+        kx1 = R[2, 1] + R[1, 2]
+        ky1 = R[2, 2] - R[1, 1] - R[3, 3] + 1
+        kz1 = R[3, 2] + R[2, 3]
+        add = (ky >= 0)
+    else
+        kx1 = R[3, 1] + R[1, 3]
+        ky1 = R[3, 2] + R[2, 3]
+        kz1 = R[3, 3] - R[1, 1] - R[2, 2] + 1
+        add = (kz >= 0)
+    end
+
+    if add
+        kx += kx1
+        ky += ky1
+        kz += kz1
+    else
+        kx -= kx1
+        ky -= ky1
+        kz -= kz1
+    end
+
+    v_tmp = SA[kx, ky, kz]
+    v = @SVector zeros(eltype(R), 3)
+    divider = norm(v_tmp)
+    if divider > 1e-8
+        v = v_tmp ./ divider
+    end
+    theta = 2*acos(qs)
+    ret = skew(v*theta)
+    return ret
+end
+
 """Calculates the adjoint of a transformation matrix"""
 function ad(T)
     # Computes the adjoint of transformation matrix T

test/runtests.jl

@@ -89,6 +89,23 @@ end
         r,p,y = R2rpy(R, conv="xyz")
         @test rpy2R(r,p,y, "xyz") ≈ R
 
+        r1 = [-1.0 0.0 0.0
+              0.0 0.0 1.0
+              0.0 1.0 0.0]
+        log_r1 = logR(r1)
+        log_r1_true = [0.0 -π/sqrt(2) π/sqrt(2)
+                       π/sqrt(2) 0.0 0.0
+                       -π/sqrt(2) 0.0 0.0]
+        @test isapprox(log_r1, log_r1_true)
+
+        r2 = [-1.0 0.0 0.0
+              0.0 -1.0 0.0
+              0.0 0.0 1.0]
+        log_r2 = logR(r2)
+        log_r2_true = [0.0 -π 0.0
+                       π 0.0 0.0
+                       0.0 0.0 0.0]
+        @test isapprox(log_r2, log_r2_true)
     end