def numjac(f, x, dx=1e-8, SO=0, SE=0):
r"""
Numerically compute Jacobian of function
:param f: the function, returns an m-vector
:type f: callable
:param x: function argument
:type x: ndarray(n)
:param dx: the numerical perturbation, defaults to 1e-8
:type dx: float, optional
:param SO: function returns SO(N) matrix, defaults to 0
:type SO: int, optional
:param SE: function returns SE(N) matrix, defaults to 0
:type SE: int, optional
:return: Jacobian matrix
:rtype: ndarray(m,n)
Computes a numerical approximation to the Jacobian for ``f(x)`` where
:math:`f: \mathbb{R}^n \mapsto \mathbb{R}^m`.
Uses first-order difference :math:`J[:,i] = (f(x + dx) - f(x)) / dx`.
If ``SO`` is 2 or 3, then it is assumed that the function returns
an SO(N) matrix and the derivative is converted to a column vector
.. math:
\vex \dmat{R} \mat{R}^T
If ``SE`` is 2 or 3, then it is assumed that the function returns
an SE(N) matrix and the derivative is converted to a colun vector.
"""
x = np.array(x)
Jcol = []
J0 = f(x)
I = np.eye(len(x))
f0 = np.array(f(x))
for i in range(len(x)):
fi = np.array(f(x + I[:, i] * dx))
Ji = (fi - f0) / dx
if SE > 0:
t = Ji[:SE, SE]
r = base.vex(Ji[:SE, :SE] @ J0[:SE, :SE].T)
Jcol.append(np.r_[t, r])
elif SO > 0:
R = Ji[:SO, :SO]
r = base.vex(R @ J0[:SO, :SO].T)
Jcol.append(r)
else:
Jcol.append(Ji)
# print(Ji)
return np.c_[Jcol].T
def numjac(f, x, dx=1e-8, tN=0, rN=0):
r"""
Numerically compute Jacobian of function
:param f: the function, returns an m-vector
:type f: callable
:param x: function argument
:type x: ndarray(n)
:param dx: the numerical perturbation, defaults to 1e-8
:type dx: float, optional
:param N: function returns SE(N) matrix, defaults to 0
:type N: int, optional
:return: Jacobian matrix
:rtype: ndarray(m,n)
Computes a numerical approximation to the Jacobian for ``f(x)`` where
:math:`f: \mathbb{R}^n \mapsto \mathbb{R}^m`.
Uses first-order difference :math:`J[:,i] = (f(x + dx) - f(x)) / dx`.
If ``N`` is 2 or 3, then it is assumed that the function returns
an SE(N) matrix which is converted into a Jacobian column comprising the
translational Jacobian followed by the rotational Jacobian.
"""
x = np.array(x)
Jcol = []
J0 = f(x)
I = np.eye(len(x))
f0 = np.array(f(x))
for i in range(len(x)):
fi = np.array(f(x + I[:, i] * dx))
Ji = (fi - f0) / dx
if tN > 0:
t = Ji[:tN, tN]
r = base.vex(Ji[:tN, :tN] @ J0[:tN, :tN].T)
Jcol.append(np.r_[t, r])
elif rN > 0:
R = Ji[:rN, :rN]
r = base.vex(R @ J0[:rN, :rN].T)
Jcol.append(r)
else:
Jcol.append(Ji)
# print(Ji)
return np.c_[Jcol].T
def trexp2(S, theta=None, check=True):
"""
Exponential of so(2) or se(2) matrix
:param S: se(2), so(2) matrix or equivalent velctor
:type T: ndarray(3,3) or ndarray(2,2)
:param theta: motion
:type theta: float
:return: matrix exponential in SE(2) or SO(2)
:rtype: ndarray(3,3) or ndarray(2,2)
:raises ValueError: bad argument
An efficient closed-form solution of the matrix exponential for arguments
that are se(2) or so(2).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(Σ)`` is the matrix exponential of the se(2) element ``Σ`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(Σ, θ)`` as above but for an se(3) motion of Σθ, where ``Σ``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
- ``trexp2(S)`` is the matrix exponential of the se(3) element ``S`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(S, θ)`` as above but for an se(2) motion of Sθ, where ``S``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skew(1))
>>> trexp2(skew(1), 2) # revolute unit twist
>>> trexp2(1)
>>> trexp2(1, 2) # revolute unit twist
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(Ω, θ)`` as above but for an so(3) motion of Ωθ, where ``Ω`` is
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
given by ``θ``.
- ``trexp2(ω)`` is the matrix exponential of the so(2) element ``ω`` expressed as
a 1-vector.
- ``trexp2(ω, θ)`` as above but for an so(3) motion of ωθ where ``ω`` is a
unit-norm vector representing a rotation axis and a rotation magnitude
given by ``θ``. ``ω`` is expressed as a 1-vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skewa([1, 2, 3]))
>>> trexp2(skewa([1, 0, 0]), 2) # prismatic unit twist
>>> trexp2([1, 2, 3])
>>> trexp2([1, 0, 0], 2)
:seealso: trlog, trexp2
"""
if base.ismatrix(S, (3, 3)) or base.isvector(S, 3):
# se(2) case
if base.ismatrix(S, (3, 3)):
# augmentented skew matrix
if check and not base.isskewa(S):
raise ValueError("argument must be a valid se(2) element")
tw = base.vexa(S)
else:
# 3 vector
tw = base.getvector(S)
if base.iszerovec(tw):
return np.eye(3)
if theta is None:
(tw, theta) = base.unittwist2_norm(tw)
elif not base.isunittwist2(tw):
raise ValueError("If theta is specified S must be a unit twist")
t = tw[0:2]
w = tw[2]
R = base.rodrigues(w, theta)
skw = base.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return base.rt2tr(R, V @ t)
elif base.ismatrix(S, (2, 2)) or base.isvector(S, 1):
# so(2) case
if base.ismatrix(S, (2, 2)):
# skew symmetric matrix
if check and not base.isskew(S):
raise ValueError("argument must be a valid so(2) element")
w = base.vex(S)
else:
# 1 vector
w = base.getvector(S)
if theta is not None and not base.isunitvec(w):
raise ValueError("If theta is specified S must be a unit twist")
# do Rodrigues' formula for rotation
return base.rodrigues(w, theta)
else:
raise ValueError(
" First argument must be SO(2), 1-vector, SE(2) or 3-vector")
def trlog2(T, check=True, twist=False):
"""
Logarithm of SO(2) or SE(2) matrix
:param T: SE(2) or SO(2) matrix
:type T: ndarray(3,3) or ndarray(2,2)
:param check: check that matrix is valid
:type check: bool
:param twist: return a twist vector instead of matrix [default]
:type twist: bool
:return: logarithm
:rtype: ndarray(3,3) or ndarray(3); or ndarray(2,2) or ndarray(1)
:raises ValueError: bad argument
An efficient closed-form solution of the matrix logarithm for arguments that
are SO(2) or SE(2).
- ``trlog2(R)`` is the logarithm of the passed rotation matrix ``R`` which
will be 2x2 skew-symmetric matrix. The equivalent vector from ``vex()``
is parallel to rotation axis and its norm is the amount of rotation about
that axis.
- ``trlog(T)`` is the logarithm of the passed homogeneous transformation
matrix ``T`` which will be 3x3 augumented skew-symmetric matrix. The
equivalent vector from ``vexa()`` is the twist vector (6x1) comprising [v
w].
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trlog2(trot2(0.3))
>>> trlog2(trot2(0.3), twist=True)
>>> trlog2(rot2(0.3))
>>> trlog2(rot2(0.3), twist=True)
:seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`,
:func:`~spatialmath.base.transformsNd.vexa`
"""
if ishom2(T, check=check):
# SE(2) matrix
if base.iseye(T):
# is identity matrix
if twist:
return np.zeros((3, ))
else:
return np.zeros((3, 3))
else:
if twist:
return base.vexa(scipy.linalg.logm(T))
else:
return scipy.linalg.logm(T)
elif isrot2(T, check=check):
# SO(2) rotation matrix
if twist:
return base.vex(scipy.linalg.logm(T))
else:
return scipy.linalg.logm(T)
else:
raise ValueError("Expect SO(2) or SE(2) matrix")
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new 2D Twist object
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
a 2x2 matrix.
- ``Twist2(T)`` is a 2D Twist object representing the SE(2) rigid-body motion expressed as
a 3x3 matrix.
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
moment vector (1 element) and direction vector (2 elements).
"""
super().__init__() # enable UserList superpowers
if arg is None:
self.data = [np.r_[0.0, 0.0, 0.0, ]]
elif isinstance(arg, Twist2):
# clone it
self.data = [np.r_[arg.v, arg.w]]
elif argcheck.isvector(arg, 3):
s = argcheck.getvector(arg)
self.data = [s]
elif argcheck.isvector(arg, 2) and argcheck.isvector(w, 1):
v = argcheck.getvector(arg)
w = argcheck.getvector(w)
self.data = [np.r_[v, w]]
elif isinstance(arg, SE2):
S = tr.trlog2(arg.A) # use closed form for SE(2)
skw, v = tr.tr2rt(S)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif Twist2.isvalid(arg):
# it's an augmented skew matrix, unpack it
skw, v = tr.tr2rt(arg)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif isinstance(arg, list):
# construct from a list
if isinstance(arg[0], np.ndarray):
# possibly a list of numpy arrays
if check:
assert all(
map(lambda x: Twist2.isvalid(x), arg)
), 'all elements of list must have valid shape and value for the class'
self.data = arg
elif type(arg[0]) == type(self):
# possibly a list of objects of same type
assert all(
map(lambda x: type(x) == type(self),
arg)), 'all elements of list must have same type'
self.data = [x.S for x in arg]
elif type(arg[0]) == list:
# possibly a list of 3-lists
assert all(
map(lambda x: isinstance(x, list) and len(x) == 3,
arg)), 'all elements of list must have same type'
self.data = [np.r_[x] for x in arg]
else:
raise ValueError('bad list argument to constructor')
else:
raise ValueError('bad argument to constructor')
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new Twist object
TW = Twist(T) is a Twist object representing the SE(2) or SE(3)
homogeneous transformation matrix T (3x3 or 4x4).
TW = Twist(V) is a twist object where the vector is specified directly.
3D CASE:
TW = Twist('R', A, Q) is a Twist object representing rotation about the
axis of direction A (3x1) and passing through the point Q (3x1).
%
TW = Twist('R', A, Q, P) as above but with a pitch of P (distance/angle).
TW = Twist('T', A) is a Twist object representing translation in the
direction of A (3x1).
Notes:
- The argument 'P' for prismatic is synonymous with 'T'.
"""
super().__init__() # enable UserList superpowers
if arg is None:
self.data = [np.r_[0, 0, 0, 0, 0, 0]]
elif isinstance(arg, Twist):
# clone it
self.data = [np.r_[arg.v, arg.w]]
elif argcheck.isvector(arg, 6):
s = argcheck.getvector(arg)
self.data = [s]
elif argcheck.isvector(arg, 3) and argcheck.isvector(w, 3):
v = argcheck.getvector(arg)
w = argcheck.getvector(w)
self.data = [np.r_[v, w]]
elif isinstance(arg, SE3):
S = tr.trlog(arg.A) # use closed form for SE(3)
skw, v = tr.tr2rt(S)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif Twist.isvalid(arg):
# it's an augmented skew matrix, unpack it
skw, v = tr.tr2rt(arg)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif isinstance(arg, list):
# construct from a list
if isinstance(arg[0], np.ndarray):
# possibly a list of numpy arrays
if check:
assert all(
map(lambda x: Twist.isvalid(x), arg)
), 'all elements of list must have valid shape and value for the class'
self.data = arg
elif type(arg[0]) == type(self):
# possibly a list of objects of same type
assert all(
map(lambda x: type(x) == type(self),
arg)), 'all elements of list must have same type'
self.data = [x.S for x in arg]
elif type(arg[0]) == list:
# possibly a list of 6-lists
assert all(
map(lambda x: isinstance(x, list) and len(x) == 6,
arg)), 'all elements of list must have same type'
self.data = [np.r_[x] for x in arg]
else:
raise ValueError('bad list argument to constructor')
else:
raise ValueError('bad argument to constructor')
def project(self, P, pose=None, objpose=None, visibility=False):
"""
Project world points to image plane
:param P: 3D points to project into camera image plane
:type P: array_like(3), array_like(3,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: SE3, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: SE3, optional
:param visibility: test if points are visible, default False
:type visibility: bool
:raises ValueError: [description]
:return: image plane points
:rtype: ndarray(2,n)
If ``pose`` is specified it is used for the camera pose instead of the
attribute ``pose``. The objects attribute is not updated.
The points ``P`` are by default with respect to the world frame, but
they can be transformed
If points are behind the camera, the image plane points are set to
NaN.
if ``visibility`` is True then check whether the projected point lies in
the bounds of the image plane. Return two values: the image plane
coordinates and an array of booleans indicating if the corresponding
point is visible.
If ``P`` is a Plucker object, then each value is projected into a
2D line in homogeneous form :math:`p[0] u + p[1] v + p[2] = 0`.
"""
P = base.getmatrix(P, (3,None))
if pose is None:
pose = self.pose
C = self.getC(pose)
if isinstance(P, np.ndarray):
# project 3D points
if objpose is not None:
P = objpose * P
x = C @ base.e2h(P)
x[2,x[2,:]<0] = np.nan # points behind the camera are set to NaN
x = base.h2e(x)
# if self._distortion is not None:
# x = self.distort(x)
# if self._noise is not None:
# # add Gaussian noise with specified standard deviation
# x += np.diag(self._noise) * np.random.randn(x.shape)
# do visibility check if required
if visibility:
visible = ~np.isnan(x[0,:]) \
& (x[0,:] >= 0) \
& (x[1,:] >= 0) \
& (x[0,:] < self.nu) \
& (x[1,:] < self.nv)
return x, visibility
else:
return x
elif isinstance(P, Plucker):
# project Plucker lines
x = np.empty(shape=(3, 0))
for p in P:
l = base.vex( C * p.skew * C.T)
x = np.c_[x, l / np.max(np.abs(l))] # normalize by largest element
return x
