Skip to content

Commit 7382390

Browse files
committed
unitvec_norm now returns (None,None) if vector is zero length
1 parent 9e47d41 commit 7382390

3 files changed

Lines changed: 12 additions & 8 deletions

File tree

‎spatialmath/base/transforms3d.py‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1769,14 +1769,18 @@ def exp2jac(v, theta=None):
17691769
17701770
:seealso: :func:`eul2jac`, :func:`rpy2jac`, :func:`rot2jac`
17711771
"""
1772-
R = trexp(v)
1772+
1773+
1774+
vn, theta = base.unitvec_norm(v)
17731775
if theta is None:
1774-
v, theta = base.unitvec_norm(v)
1776+
return np.eye(3)
1777+
1778+
R = trexp(v)
17751779

17761780
z = np.eye(3,3) - R
17771781
A = []
17781782
for i in range(3):
1779-
dRdvi = v[i] * base.skew(v) + base.skew(np.cross(v, z[:,i])) / theta
1783+
dRdvi = vn[i] * base.skew(vn) + base.skew(np.cross(vn, z[:,i])) / theta
17801784
x = base.vex(dRdvi)
17811785
A.append(x)
17821786
return np.c_[A].T

‎spatialmath/base/vectors.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ def unitvec_norm(v):
104104
if n > 100 * _eps: # if greater than eps
105105
return (v / n, n)
106106
else:
107-
return None
107+
return None, None
108108

109109
def norm(v):
110110
"""

‎spatialmath/quaternion.py‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1931,14 +1931,14 @@ def increment(self, w, normalize=False):
19311931
"""
19321932

19331933
# is (v, theta) or None
1934-
vt = base.unitvec_norm(w)
1934+
v, theta = base.unitvec_norm(w)
19351935

1936-
if vt is None:
1936+
if v is None:
19371937
# zero update
19381938
return
19391939

1940-
ds = math.cos(vt[1] / 2)
1941-
dv = math.sin(vt[1] / 2) * vt[0]
1940+
ds = math.cos(theta / 2)
1941+
dv = math.sin(theta / 2) * v
19421942

19431943
updated = base.qqmul(self.A, np.r_[ds, dv])
19441944
if normalize:

0 commit comments

Comments
 (0)