type stability in logT

baggepinnen · Feb 13, 2021 · e6b5775 · e6b5775
1 parent 32f2a2d
commit e6b5775
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/Project.toml b/Project.toml
@@ -1,7 +1,7 @@
 name = "Robotlib"
 uuid = "a75dfa10-1339-53f3-8748-4d618c3d76af"
 authors = ["baggepinnen <baggepinnen@gmail.com>"]
-version = "1.1.4"
+version = "1.1.5"
 
 [deps]
 DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"

diff --git a/src/utils.jl b/src/utils.jl
@@ -100,8 +100,8 @@ Calculates the matrix logarithm of a transformation matrix ∈ SE(3)
 Does not seem to be very reliable for very small rotations, use logm instead, which is a bit slower.
 """
 function logT(T) # Verified to work in the vicinity of I, but not for very small errors
-    R = T[1:3,1:3]
-    t = T[1:3,4]
+    R = T2R(T)
+    t = T2t(T)
     ω = logR(R)
     nω = norm(skewcoords(ω))
     if nω < 1e-3
@@ -110,7 +110,7 @@ function logT(T) # Verified to work in the vicinity of I, but not for very small
         fact = (2sin(nω)-nω*(1+cos(nω)))/(2*nω^2*sin(nω))
     end
     Ai = I-0.5ω+fact*ω^2
-    return [ω Ai*t;0 0 0 0]
+    return [[ω Ai*t]; SA[0 0 0 0]]
 end
 
 """Calculates the matrix logarithm of a rotation matrix ∈ SO(3)"""