11import numpy as np
22from spatialmath import base
33
4- def numjac (f , x , dx = 1e-8 , tN = 0 , rN = 0 ):
4+
5+ def numjac (f , x , dx = 1e-8 , SO = 0 , SE = 0 ):
56 r"""
67 Numerically compute Jacobian of function
78
@@ -11,8 +12,11 @@ def numjac(f, x, dx=1e-8, tN=0, rN=0):
1112 :type x: ndarray(n)
1213 :param dx: the numerical perturbation, defaults to 1e-8
1314 :type dx: float, optional
14- :param N: function returns SE(N) matrix, defaults to 0
15- :type N: int, optional
15+ :param SO: function returns SO(N) matrix, defaults to 0
16+ :type SO: int, optional
17+ :param SE: function returns SE(N) matrix, defaults to 0
18+ :type SE: int, optional
19+
1620 :return: Jacobian matrix
1721 :rtype: ndarray(m,n)
1822
@@ -21,9 +25,16 @@ def numjac(f, x, dx=1e-8, tN=0, rN=0):
2125
2226 Uses first-order difference :math:`J[:,i] = (f(x + dx) - f(x)) / dx`.
2327
24- If ``N`` is 2 or 3, then it is assumed that the function returns
25- an SE(N) matrix which is converted into a Jacobian column comprising the
26- translational Jacobian followed by the rotational Jacobian.
28+ If ``SO`` is 2 or 3, then it is assumed that the function returns
29+ an SO(N) matrix and the derivative is converted to a column vector
30+
31+ .. math:
32+
33+ \vex \dmat{R} \mat{R}^T
34+
35+ If ``SE`` is 2 or 3, then it is assumed that the function returns
36+ an SE(N) matrix and the derivative is converted to a colun vector.
37+
2738 """
2839 x = np .array (x )
2940 Jcol = []
@@ -34,13 +45,13 @@ def numjac(f, x, dx=1e-8, tN=0, rN=0):
3445 fi = np .array (f (x + I [:,i ] * dx ))
3546 Ji = (fi - f0 ) / dx
3647
37- if tN > 0 :
38- t = Ji [:tN , tN ]
39- r = base .vex (Ji [:tN ,: tN ] @ J0 [:tN ,: tN ].T )
48+ if SE > 0 :
49+ t = Ji [:SE , SE ]
50+ r = base .vex (Ji [:SE ,: SE ] @ J0 [:SE ,: SE ].T )
4051 Jcol .append (np .r_ [t , r ])
41- elif rN > 0 :
42- R = Ji [:rN ,: rN ]
43- r = base .vex (R @ J0 [:rN ,: rN ].T )
52+ elif SO > 0 :
53+ R = Ji [:SO ,: SO ]
54+ r = base .vex (R @ J0 [:SO ,: SO ].T )
4455 Jcol .append (r )
4556 else :
4657 Jcol .append (Ji )
0 commit comments