Skip to content

Commit 820fcf0

Browse files
committed
make options more meaningful
1 parent 9604839 commit 820fcf0

1 file changed

Lines changed: 23 additions & 12 deletions

File tree

‎spatialmath/base/numeric.py‎

Lines changed: 23 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
import numpy as np
22
from 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

Comments
 (0)