fix bug in jacobian calculation of fuse_core::quaternionToAngleAxis

ARTI-Robots · Oct 3, 2024 · 1be10fe · 1be10fe
1 parent 8545097
commit 1be10fe
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/fuse_core/include/fuse_core/util.h b/fuse_core/include/fuse_core/util.h
@@ -388,9 +388,9 @@ static inline void quaternionToAngleAxis(const double * q, double * angle_axis,
     else
     {
       jacobian_map.setZero();
-      jacobian_map(1, 1) = 2.0;
-      jacobian_map(2, 2) = 2.0;
-      jacobian_map(3, 3) = 2.0;
+      jacobian_map(0, 1) = 2.0;
+      jacobian_map(1, 2) = 2.0;
+      jacobian_map(2, 3) = 2.0;
     }
   }
 }