another go at angvelxform_dot, including notebook for derivation

Author: petercorke

spatialmath/base/transforms3d.py

@@ -2256,20 +2256,24 @@ def angvelxform_dot(𝚪, 𝚪d, full=True, representation="rpy/xyz"):
         skd = base.skew(vd)
         theta_dot = np.inner(𝚪, 𝚪d) / base.norm(𝚪)
         theta = base.norm(𝚪)
-        Theta = 1 - theta / 2 * np.sin(theta) / (1 - np.cos(theta))
+        Theta = (1.0 - theta / 2.0 * np.sin(theta) / (1.0 - np.cos(theta))) / theta**2
+
+        # hand optimized version of code from notebook
+        # TODO:
+        #   results are close but different to numerical cross check
+        #   something wrong in the derivation
         Theta_dot = (
-            -0.5 * theta * theta_dot * math.cos(theta) / (1 - math.cos(theta))
-            + 0.5
-            * theta
-            * theta_dot
-            * math.sin(theta) ** 2
-            / (1 - math.cos(theta)) ** 2
-            - 0.5 * theta_dot * math.sin(theta) / (1 - math.cos(theta))
-        ) / theta ** 2 - 2 * theta_dot * (
-            -1 / 2 * theta * math.sin(theta) / (1 - math.cos(theta)) + 1
-        ) / theta ** 3
-
-        Ad = -0.5 * skd + 2 * sk @ skd * Theta + sk @ sk * Theta_dot
+                (
+                -theta * math.cos(theta) 
+                -math.sin(theta) +
+                 theta * math.sin(theta)**2 / (1 - math.cos(theta)) 
+                ) * theta_dot / 2  / (1 - math.cos(theta)) / theta**2
+                - (
+                    2 - theta * math.sin(theta) / (1 - math.cos(theta))
+                  ) * theta_dot / theta**3
+                ) 
+
+        Ad = -0.5 * skd + 2.0 * sk @ skd * Theta + sk @ sk * Theta_dot
     else:
         raise ValueError("bad representation specified")
 

symbolic/angvelxform_dot.ipynb

@@ -43,7 +43,7 @@
     "$\n",
     "\\mathbf{A} = I_{3 \\times 3} - \\frac{1}{2} [v]_\\times + [v]^2_\\times \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right)\n",
     "$\n",
-    "where $\\theta = \\| \\varphi \\|$.\n",
+    "where $\\theta = \\| \\varphi \\|$ and $v = \\hat{\\varphi}$\n",
     "\n",
     "We simplify the equation as\n",
     "\n",
@@ -56,7 +56,7 @@
     "\\Theta = \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right)\n",
     "$\n",
     "\n",
-    "We want to find the deriviative, which we can compute using the chain rule\n",
+    "We can find the derivative using the chain rule\n",
     "\n",
     "$\n",
     "\\dot{\\mathbf{A}} = - \\frac{1}{2} [\\dot{v}]_\\times + 2 [v]_\\times [\\dot{v}]_\\times \\Theta + [v]^2_\\times \\dot{\\Theta}\n",