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