def payload(self, m, p=np.zeros(3)): """ payload(m, p) adds payload mass adds a payload with point mass m at position p in the end-effector coordinate frame. payload(m) adds payload mass adds a payload with point mass m at in the end-effector coordinate frame. payload(0) removes added payload. :param m: mass (kg) :type m: float :param p: position in end-effector frame :type p: ndarray(3,1) """ p = getvector(p, 3, out='col') lastlink = self.links[self.n - 1] lastlink.m = m lastlink.r = p
def Revolute(cls, q): """ Construct a new 2D revolute Twist object :param q: Point on the line of action :type q: array_like(2) :return: 2D prismatic twist :rtype: Twist2 instance - ``Twist2.Revolute(q)`` is a 2D Twist object representing rotation about the 2D point ``q``. Example: .. runblock:: pycon >>> from spatialmath import Twist2 >>> Twist2.Revolute([0, 1]) """ q = base.getvector(q, 2) v = -np.cross(np.r_[0.0, 0.0, 1.0], np.r_[q, 0.0]) return cls(v[:2], 1)
def blackbody(lam, T): """ Compute blackbody emission spectrum :param lam: wavelength 𝜆 [m] :type lam: float or array_like :param T: blackbody temperature [K] :type T: float ``blackbody(𝜆, T)`` is the blackbody radiation power density [W/m^3] at the wavelength 𝜆 [m] and temperature T [K]. If 𝜆 is a vector (N,), then the result is a vector (N,) of blackbody radiation power density at the corresponding elements of 𝜆. Example:: l = np.linspace(380, 700, 10) * 1e-9 # visible spectrum e = blackbody(l, 6500) # emission of sun plt.plot(l, e) :references: - Robotics, Vision & Control, Section 10.1, P. Corke, Springer 2011. """ # physical constants c = 2.99792458e8 # m/s (speed of light) h = 6.626068e-34 # m2 kg / s (Planck's constant) k = 1.3806503e-23 # J K-1 (Boltzmann's constant) lam = base.getvector(lam) e = 2.0 * h * c**2 / (lam**5 * (np.exp(h * c / k / T / lam) - 1)) if len(e) == 1: return e[0] else: return e
def eval(self, q): """ Evaluate an ETS with joint coordinate substitution :param q: joint coordinates :type q: array-like :return: product of transformations :rtype: SE3 Effectively the forward-kinematics of the ET sequence. Compounds the transforms left to right, and substitutes in joint coordinates as needed from consecutive elements of ``q``. """ T = SE3() q = getvector(q, out='sequence') for e in self: if e.jtype == self.VARIABLE: T *= e.T(q.pop(0)) else: T *= e.T() return T
def unittwist(S, tol=10): """ Convert twist to unit twist :param S: twist vector :type S: array_like(6) :param tol: tolerance in units of eps :type tol: float :return: unit twist :rtype: ndarray(6) A unit twist is a twist where: - the rotation part has unit magnitude - if the rotational part is zero, then the translational part has unit magnitude .. runblock:: pycon >>> from spatialmath.base import * >>> unittwist([2, 4, 6, 2, 0, 0]) >>> unittwist([2, 0, 0, 0, 0, 0]) Returns None if the twist has zero magnitude """ S = getvector(S, 6) if iszerovec(S, tol=tol): return None v = S[0:3] w = S[3:6] if iszerovec(w): th = norm(v) else: th = norm(w) return S / th
def rodrigues(w, theta=None): r""" Rodrigues' formula for rotation :param w: rotation vector :type w: array_like(3) or array_like(1) :param θ: rotation angle :type θ: float or None :return: SO(n) matrix :rtype: ndarray(2,2) or ndarray(3,3) Compute Rodrigues' formula for a rotation matrix given a rotation axis and angle. .. math:: \mat{R} = \mat{I}_{3 \times 3} + \sin \theta \skx{\hat{\vec{v}}} + (1 - \cos \theta) \skx{\hat{\vec{v}}}^2 .. runblock:: pycon >>> from spatialmath.base import * >>> rodrigues([1, 0, 0], 0.3) >>> rodrigues([0.3, 0, 0]) >>> rodrigues(0.3) # 2D version """ w = base.getvector(w) if base.iszerovec(w): # for a zero so(n) return unit matrix, theta not relevant if len(w) == 1: return np.eye(2) else: return np.eye(3) if theta is None: w, theta = base.unitvec_norm(w) skw = skew(w) return np.eye(skw.shape[0]) + math.sin(theta) * skw + ( 1.0 - math.cos(theta)) * skw @ skw
def __init__(self, m=None, r=None, I=None): """ Create a new spatial inertia :param m: mass :type m: float :param r: centre of mass relative to link frame :type r: 3-element array_like :param I: inertia about the centre of mass, axes aligned with link frame :type I: numpy.array, shape=(6,6) - ``SpatialInertia(m, r I)`` is a spatial inertia object for a rigid-body with mass ``m``, centre of mass at ``r`` relative to the link frame, and an inertia matrix ``I`` (3x3) about the centre of mass. - ``SpatialInertia(I)`` is a spatial inertia object with a value equal to ``I`` (6x6). """ super().__init__() if m is None and r is None and I is None: # no arguments I = SpatialInertia._identity() elif m is not None and r is None and I is None and base.ismatrix( m, (6, 6)): I = base.getmatrix(m, (6, 6)) elif m is not None and r is not None: r = base.getvector(r, 3) if I is None: I = np.zeros((3, 3)) else: I = base.getmatrix(I, (3, 3)) C = base.skew(r) I = np.block([[m * np.eye(3), m * C.T], [m * C, I + m * C @ C.T]]) else: raise ValueError('bad values') self.data = [I]
def isunittwist(v, tol=10): r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: twist vector to test :type v: array_like(6) :param tol: tolerance in units of eps :type tol: float :return: whether twist has unit length :rtype: bool :raises ValueError: for incorrect vector length Vector is is intepretted as :math:`[v, \omega]` where :math:`v \in \mathbb{R}^n` and :math:`\omega \in \mathbb{R}^1` for SE(2) and :math:`\omega \in \mathbb{R}^3` for SE(3). A unit twist can be a: - unit rotational twist where :math:`|| \omega || = 1`, or - unit translational twist where :math:`|| \omega || = 0` and :math:`|| v || = 1`. .. runblock:: pycon >>> from spatialmath.base import * >>> isunittwist([1, 2, 3, 1, 0, 0]) >>> isunittwist([0, 0, 0, 2, 0, 0]) :seealso: unit, isunitvec """ v = getvector(v) if len(v) == 6: # test for SE(3) twist return isunitvec(v[3:6], tol=tol) or (np.linalg.norm(v[3:6]) < tol * _eps and isunitvec(v[0:3], tol=tol)) else: raise ValueError
def Tx(cls, x): """ Create an SE(2) translation along the X-axis :param x: translation distance along the X-axis :type x: float :return: SE(2) matrix :rtype: SE2 instance `SE2.Tx(x)` is an SE(2) translation of ``x`` along the x-axis Example: .. runblock:: pycon >>> SE2.Tx(2) >>> SE2.Tx([2,3]) :seealso: :func:`~spatialmath.base.transforms3d.transl` :SymPy: supported """ return cls([base.transl2(_x, 0) for _x in base.getvector(x)], check=False)
def qnorm(q): r""" Norm of a quaternion :arg q: quaternion :type v: : array_like(4) :return: norm of the quaternion :rtype: float Returns the norm, length or magnitude of the input quaternion which is :math:`(s^2 + v_x^2 + v_y^2 + v_z^2}^{1/2}` .. runblock:: pycon >>> from spatialmath.base import qnorm >>> q = qnorm([1, 2, 3, 4]) >>> print(q) :seealso: unit """ q = base.getvector(q, 4) return np.linalg.norm(q)
def Tz(cls, z): """ Create an SE(3) translation along the Z-axis :param z: translation distance along the Z-axis :type z: float :return: SE(3) matrix :rtype: SE3 instance `SE3.Tz(z)` is an SE(3) translation of ``z`` along the z-axis Example: .. runblock:: pycon >>> SE3.Tz(2) >>> SE3.Tz([2,3]) :seealso: :func:`~spatialmath.base.transforms3d.transl` :SymPy: supported """ return cls([base.transl(0, 0, _z) for _z in base.getvector(z)], check=False)
def unit(q, tol=10): """ Create a unit quaternion :arg v: quaterion :type v: array_like(4) :return: a pure quaternion :rtype: ndarray(4) :raises ValueError: quaternion has (near) zero norm Creates a unit quaternion, with unit norm, by scaling the input quaternion. .. runblock:: pycon >>> from spatialmath.base import unit, qprint >>> q = unit([1, 2, 3, 4]) >>> qprint(q) .. note:: Scalar part is always positive. .. note:: If the quaternion norm is less than ``tol * eps`` an exception is raised. :seealso: norm """ q = base.getvector(q, 4) nm = np.linalg.norm(q) if abs(nm) < tol * _eps: raise ValueError("cannot normalize (near) zero length quaternion") else: q /= nm if q[0] >= 0: return q else: return -q
def Ty(cls, y): """ Create an SE(3) translation along the Y-axis :param y: translation distance along the Y-axis :type y: float :return: SE(3) matrix :rtype: SE3 instance `SE3.Ty(y) is an SE(3) translation of ``y`` along the y-axis Example: .. runblock:: pycon >>> SE3.Ty(2) >>> SE3.Tz([2,3]) :seealso: :func:`~spatialmath.base.transforms3d.transl` :SymPy: supported """ return cls([base.transl(0, _y, 0) for _y in base.getvector(y)], check=False)
def Hx(self, xv, jf): """ Jacobian dh/dx % # J = self.Hx(X, K) returns the Jacobian dh/dx (2x3) at the vehicle # state X (3x1) for map landmark K. % # J = self.Hx(X, P) as above but for a landmark at coordinate P. % # See also RangeBearingSensor.h. """ if base.isinteger(jf): # landmark index provided xf = self.sensor.landmark(jf) else: # assume it is a coordinate xf = base.getvector(jf, 2) Delta = xf - xv[0:2] r = base.norm(Delta) return np.array([ [-Delta[0] / r, -Delta[1] / r, 0], [Delta[1] / r**2, -Delta[0] / r**2, -1], ])
def skew(v): r""" Create skew-symmetric metrix from vector :param v: vector :type v: array_like(1) or array_like(3) :return: skew-symmetric matrix in so(2) or so(3) :rtype: ndarray(2,2) or ndarray(3,3) :raises ValueError: bad argument ``skew(V)`` is a skew-symmetric matrix formed from the elements of ``V``. - ``len(V)`` is 1 then ``S`` = :math:`\left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]` - ``len(V)`` is 3 then ``S`` = :math:`\left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]` .. runblock:: pycon >>> from spatialmath.base import * >>> skew(2) >>> skew([1, 2, 3]) .. note:: - This is the inverse of the function ``vex()``. - These are the generator matrices for the Lie algebras so(2) and so(3). :seealso: :func:`vex`, :func:`skewa` :SymPy: supported """ v = base.getvector(v, None, 'sequence') if len(v) == 1: return np.array([[0, -v[0]], [v[0], 0]]) elif len(v) == 3: return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) else: raise ValueError("argument must be a 1- or 3-vector")
def edgelist(im, p, direction=1): """ Find edge of a region :param im: binary image :type im: ndarray(h,w,int) :param p: initial point :type p: array_like(2) :param direction: direction to traverse region, +1 clockwise [default], -1 counter-clockwise :type direction: int, optional :raises ValueError: initial point is not on the edge :raises RuntimeError: not able to find path to the goal :return: edge list, direction vector list :rtype: tuple of lists ``edge, dirs = edgelist(im, seed)`` is the boundary/contour/edge of a region in the binary image ``im``. ``seed=[X,Y]`` is the coordinate of a point on the edge of the region of interest, but belonging to the region. ``edge`` is a list of coordinates (2) of edge pixels of a region in theThe elements of the edgelist are NumPy ndarray(2). ``dirs`` is a list of integers representing the direction of the edge from the corresponding point in ``edge`` to the next point in ``edge``. The integers in the range 0 to 7 represent directions: W SW S SE E NW N NW respectively. - Coordinates are given and returned assuming the matrix is an image, so the indices are always in the form (x,y) or (column,row). - ``im` is a binary image where 0 is assumed to be background, non-zero is an object. - ``p`` must be a point on the edge of the region. - The initial point is always the first and last element of the returned edgelist. - 8-direction chain coding can give incorrect results when used with blobs founds using 4-way connectivty. :Reference: - METHODS TO ESTIMATE AREAS AND PERIMETERS OF BLOB-LIKE OBJECTS: A COMPARISON Luren Yang, Fritz Albregtsen, Tor Lgnnestad and Per Grgttum IAPR Workshop on Machine Vision Applications Dec. 13-15, 1994, Kawasaki """ if direction > 0: neighbours = np.arange(start=0, stop=8, step=1) else: neighbours = np.arange(start=7, stop=-1, step=-1) try: pix0 = im[p[1], p[0]] # color of pixel we start at except: raise ValueError('specified coordinate is not within image') p = base.getvector(p, 2, dtype=np.int) q = adjacent_point(im, p, pix0) # find an adjacent point outside the blob if q is None: raise ValueError('no neighbour outside the blob') d = None e = [p] # initialize the edge list dir = [] # initialize the direction list p0 = None while True: # find which direction is Q dq = q - p for kq in range(0, 8): # get index of neighbour's direction in range [1,8] if np.all(dq == _dirs[kq]): break # now test for directions relative to Q for j in neighbours: # get index of neighbour's direction in range [1,8] k = (j + kq) % 8 # if k > 7: # k = k - 7 # compute coordinate of the k'th neighbour nk = p + _dirs[k] try: if im[nk[1], nk[0]] == pix0: # if this neighbour is in the blob it is the next edge pixel p = nk break except: raise ValueError("Something went wrong calculating edgelist") q = nk dir.append(k) # check if we are back where we started if p0 is None: p0 = p # make a note of where we started else: if all(p == p0): break # keep going, add this point to the edgelist e.append(p) return e, dir
def distancexform(occgrid, goal, metric='cityblock', show=False): """ Distance transform for path planning :param occgrid: Occupancy grid :type occgrid: NumPy ndarray :param goal: Goal position (x,y) :type goal: 2-element array-like :param metric: distance metric, defaults to 'cityblock' :type metric: str, optional :return: Distance transform matrix :rtype: NumPy ndarray Returns a matrix the same size as the occupancy grid ``occgrid`` where each cell contains the distance to the goal according to the chosen ``metric``. - Obstacle cells will be set to ``nan``. - Unreachable cells, ie. free cells _inside obstacles_ will be set to ``inf``. The cells of the passed occupancy grid are: - zero, cell is free or driveable - one, cell is an obstacle, or not driveable """ # build the matrix for performing distance transform: # - obstacles are nan # - other cells are inf # - goal is zero goal = base.getvector(goal, 2, dtype=np.int) occgrid = occgrid.astype(np.float32) occgrid[occgrid > 0] = np.nan # assign nan to obstacle cells nans = np.isnan(occgrid) # keep record of where the NaNs are occgrid[occgrid==0] = np.inf # assign inf to other cells occgrid[goal[1], goal[0]] = 0 # assign zero to goal # create the appropriate distance matrix D if metric.lower() in ('manhattan', 'cityblock'): D = np.array([ [ np.inf, 1, np.inf], [ 1, 0, 1], [ np.inf, 1, np.inf] ]) elif metric.lower() == 'euclidean': r2 = np.sqrt(2) D = np.array([ [ r2, 1, r2], [ 1, 0, 1], [ r2, 1, r2] ]) # get ready to iterate count = 0 ninf = np.inf # number of infinities in the map while True: occgrid = dxstep(occgrid, D) occgrid[nans] = np.nan count += 1 # if opt.animate # cmap = [1 0 0; gray(count)]; # colormap(cmap) # image(occgrid+1, 'CDataMapping', 'direct'); # set(gca, 'Ydir', 'normal'); # xlabel('x'); # ylabel('y'); # if opt.animate # anim.add(); # else # pause(opt.delay); ninfnow = sum(np.isinf(occgrid.flatten())) # current number of Infs if ninfnow == ninf: # stop if the number of Infs left in the map had stopped reducing # it may never get to zero if there are unreachable cells in the map break ninf = ninfnow print(f"{count:d} iterations, {ninf:d} unreachable cells") return occgrid
def ikine_min( self, T, q0=None, pweight=1.0, stiffness=0.0, qlimits=True, ilimit=1000): """ Inverse kinematics by optimization with joint limits (Robot superclass) :param T: The desired end-effector pose or pose trajectory :type T: SE3 :param q0: initial joint configuration (default all zeros) :type q0: ndarray(n) :param pweight: weighting on position error norm compared to rotation error (default 1) :type pweight: float :param stiffness: Stiffness used to impose a smoothness contraint on joint angles, useful when n is large (default 0) :type stiffness: float :param qlimits: Enforce joint limits (default True) :type qlimits: bool :param ilimit: Iteration limit (default 1000) :type ilimit: bool :return: inverse kinematic solution :rtype: named tuple ``sol = robot.ikine_unc(T)`` are the joint coordinates (n) corresponding to the robot end-effector pose T which is an SE3 object. The return value ``sol`` is a named tuple with elements: ============ ========== ============================================================ Element Type Description ============ ========== ============================================================ ``q`` ndarray(n) joint coordinates in units of radians or metres, or ``None`` ``success`` bool whether a solution was found ``reason`` str reason for the failure ``iterations`` int number of iterations ``residual`` float final value of cost function ============ ========== ============================================================ **Trajectory operation**: If ``len(T) > 1`` it is considered to be a trajectory, and the result is a list of named tuples such that ``sol[k]`` corresponds to ``T[k]``. The initial estimate of q for each time step is taken as the solution from the previous time step. .. note:: - PROTOTYPE CODE UNDER DEVELOPMENT, intended to do numerical inverse kinematics with joint limits - Uses ``SciPy.minimize`` with/without constraints. - The inverse kinematic solution is generally not unique, and depends on the initial guess ``q0``. - This norm is computed from distances and angles and ``pweight`` can be used to scale the position error norm to be congruent with rotation error norm. - For a highly redundant robot ``stiffness`` can be used to impose a smoothness contraint on joint angles, tending toward solutions with are smooth curves. - The default value of ``q0`` is zero which is a poor choice for most manipulators since it often corresponds to a kinematic singularity. - Such a solution is completely general, though much less efficient than analytic inverse kinematic solutions derived symbolically. - This approach allows a solution to obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. - Joint offsets, if defined, are accounted for in the solution. - Joint limits become explicit bounds if 'qlimits' is set. .. warning:: - The objective function is rather uncommon. - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it uses a scalar cost-function and does not provide a Jacobian. :seealso: :func:`ikine_LM`, :func:`ikine_LMS`, :func:`ikine_unc`, :func:`ikine_con` """ # noqa if not isinstance(T, SE3): T = SE3(T) if q0 is None: q0 = np.zeros((self.n,)) else: q0 = base.getvector(q0, self.n) col = 2 solutions = [] # Define the cost function to minimise def cost(q, *args): T, pweight, col, stiffness = args Tq = self.fkine(q).A # translation error dT = base.transl(T) - base.transl(Tq) E = np.linalg.norm(dT) * pweight # Rotation error # Find dot product of two columns dd = np.dot(T[0:3, col], Tq[0:3, col]) E += np.arccos(dd)**2 * 1000 if stiffness > 0: # Enforce a continuity constraint on joints, minimum bend E += np.sum(np.diff(q)**2) * stiffness return E for Tk in T: if qlimits: bounds = Bounds(self.qlim[0, :], self.qlim[1, :]) res = minimize( cost, q0, args=(Tk.A, pweight, col, stiffness), bounds=bounds, options={'gtol': 1e-6, 'maxiter': ilimit}) else: # No joint limits, unconstrained optimization res = minimize( cost, q0, args=(Tk.A, pweight, col, stiffness), options={'gtol': 1e-6, 'maxiter': ilimit}) solution = iksol(res.x, res.success, res.message, res.nit, res.fun) solutions.append(solution) q0 = res.x # use this solution as initial estimate for next time if len(T) == 1: return solutions[0] else: return solutions
def __init__(self, covar=None, speed_max=np.inf, accel_max=np.inf, x0=None, dt=0.1, control=None, animation=None, verbose=False, dim=10): r""" Superclass for vehicle kinematic models :param covar: odometry covariance, defaults to zero :type covar: ndarray(2,2), optional :param speed_max: maximum speed, defaults to :math:`\infty` :type speed_max: float, optional :param accel_max: maximum acceleration, defaults to :math:`\infty` :type accel_max: float, optional :param x0: Initial state, defaults to (0,0,0) :type x0: array_like(3), optional :param dt: sample update interval, defaults to 0.1 :type dt: float, optional :param control: vehicle control inputs, defaults to None :type control: array_like(2), interp1d, VehicleDriver :param animation: Graphical animation of vehicle, defaults to None :type animation: VehicleAnimation subclass, optional :param verbose: print lots of info, defaults to False :type verbose: bool, optional :param dim: dimensions of 2D plot area, defaults to (-10:10) x (-10:10), see :func:`~spatialmath.base.animate.plotvol2` :type dims: float, array_like(2), , array_like(4) This is an abstract superclass that simulates the motion of a mobile robot under the action of a controller. The controller provides control inputs to the vehicle, and the output odometry is returned. The true state, effectively unknowable in practice, is computed and accessible. :seealso: :func:`Bicycle`, :func:`Unicycle` """ if covar is None: covar = np.zeros((2, 2)) self._V = covar self._dt = dt if x0 is None: x0 = np.zeros((3, ), dtype=float) else: x0 = base.getvector(x0) if len(x0) not in (2, 3): raise ValueError('x0 must be length 2 or 3') self._x0 = x0 self._x = x0 self._speed_max = speed_max self._accel_max = accel_max self._v_prev = 0 self._vehicle_plot = None if control is not None: self.add_driver(control) self._animation = animation self._dt = dt self._t = 0 self._stopsim = False self._verbose = verbose self._plot = False self._dim = dim self._x_hist = np.empty((0, len(x0)))
def ikine_con(self, T, q0=None, **kwargs): r""" Inverse kinematics by optimization with joint limits (Robot superclass) :param T: The desired end-effector pose or pose trajectory :type T: SE3 :param q0: initial joint configuration (default all zeros) :type q0: ndarray(n) :return: inverse kinematic solution :rtype: named tuple ``sol = robot.ikine_unc(T)`` are the joint coordinates (n) corresponding to the robot end-effector pose T which is an SE3 object. The return value ``sol`` is a named tuple with elements: ============ ========== ============================================================ Element Type Description ============ ========== ============================================================ ``q`` ndarray(n) joint coordinates in units of radians or metres, or ``None`` ``success`` bool whether a solution was found ``reason`` str reason for the failure ``iterations`` int number of iterations ``residual`` float final value of cost function ============ ========== ============================================================ **Trajectory operation**: If ``len(T) > 1`` it is considered to be a trajectory, and the result is a list of named tuples such that ``sol[k]`` corresponds to ``T[k]``. The initial estimate of q for each time step is taken as the solution from the previous time step. .. note:: - Uses ``SciPy.minimize`` with bounds. - Joint limits are considered in this solution. - Can be used for robots with arbitrary degrees of freedom. - The inverse kinematic solution is generally not unique, and depends on the initial guess ``q0``. - The default value of ``q0`` is zero which is a poor choice for most manipulators since it often corresponds to a kinematic singularity. - Such a solution is completely general, though much less efficient than analytic inverse kinematic solutions derived symbolically. - The objective function (error) is :math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2` where :math:`\mat{\Omega}` is a diagonal matrix. - Joint offsets, if defined, are accounted for in the solution. .. warning:: - The objective function is rather uncommon. - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it uses a scalar cost-function and does not provide a Jacobian. :author: Bryan Moutrie, for RTB-MATLAB :seealso: :func:`ikine_LM`, :func:`ikine_LMS`, :func:`ikine_unc`, :func:`ikine_min` """ # noqa if not isinstance(T, SE3): T = SE3(T) if q0 is None: q0 = np.zeros((self.n)) else: q0 = base.getvector(q0, self.n) solutions = [] omega = np.diag([1, 1, 1, 3 / self.reach]) def cost(q, *args): T, omega = args E = (base.trinv(T) @ self.fkine(q).A - np.eye(4)) @ omega return (E**2).sum() # quicker than np.sum(E**2) bnds = Bounds(self.qlim[0, :], self.qlim[1, :]) for Tk in T: res = minimize( cost, q0, args=(Tk.A, omega), bounds=bnds, tol=1e-8, options={'ftol': 1e-10}) solution = iksol(res.x, res.success, res.message, res.nit, res.fun) solutions.append(solution) q0 = res.x # use this solution as initial estimate for next time if len(T) == 1: return solutions[0] else: return solutions
def ikine_LMS( self, T, q0=None, mask=None, ilimit=500, tol=1e-10, wN=1e-3, Lmin=0 ): """ Numerical inverse kinematics by Levenberg-Marquadt optimization (Robot superclass) :param T: The desired end-effector pose or pose trajectory :type T: SE3 :param q0: initial joint configuration (default all zeros) :type q0: ndarray(n) :param mask: mask vector that correspond to translation in X, Y and Z and rotation about X, Y and Z respectively. :type mask: ndarray(6) :param ilimit: maximum number of iterations (default 500) :type ilimit: int :param tol: final error tolerance (default 1e-10) :type tol: float :param ωN: damping coefficient :type ωN: float (default 1e-3) :return: inverse kinematic solution :rtype: named tuple ``sol = robot.ikine_LM(T)`` are the joint coordinates (n) corresponding to the robot end-effector pose ``T`` which is an ``SE3`` object. This method can be used for robots with any number of degrees of freedom. The return value ``sol`` is a named tuple with elements: ============ ========== ============================================================ Element Type Description ============ ========== ============================================================ ``q`` ndarray(n) joint coordinates in units of radians or metres, or ``None`` ``success`` bool whether a solution was found ``reason`` str reason for the failure ``iterations`` int number of iterations ``residual`` float final value of cost function ============ ========== ============================================================ **Trajectory operation**: If ``len(T) > 1`` it is considered to be a trajectory, and the result is a list of named tuples such that ``sol[k]`` corresponds to ``T[k]``. The initial estimate of q for each time step is taken as the solution from the previous time step. **Underactuated robots:** For the case where the manipulator has fewer than 6 DOF the solution space has more dimensions than can be spanned by the manipulator joint coordinates. In this case we specify the ``mask`` option where the ``mask`` vector (6) specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask vector has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value should be 0 (for ignore) or 1. The number of non-zero elements should equal the number of manipulator DOF. For example when using a 3 DOF manipulator rotation orientation might be unimportant in which case use the option: mask = [1 1 1 0 0 0]. .. note:: - Implements a modified Levenberg-Marquadt variable-step-size solver which is quite robust in practice. - The tolerance is computed on the norm of the error between current and desired tool pose. This norm is computed from distances and angles without any kind of weighting. - The inverse kinematic solution is generally not unique, and depends on the initial guess ``q0``. - The default value of ``q0`` is zero which is a poor choice for most manipulators since it often corresponds to a kinematic singularity. - Such a solution is completely general, though much less efficient than analytic inverse kinematic solutions derived symbolically. - This approach allows a solution to be obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. - Joint offsets, if defined, are accounted for in the solution. - Joint limits are not considered in this solution. :references: - "Solvability-Unconcerned Inverse Kinematics by the Levenberg–Marquardt Method", T. Sugihara, IEEE T-RO, 27(5), October 2011, pp. 984-991. :seealso: :func:`ikine_LM`, :func:`ikine_unc`, :func:`ikine_con`, :func:`ikine_min` """ # noqa if not isinstance(T, SE3): T = SE3(T) solutions = [] def angle_axis(T, Td): d = base.transl(Td) - base.transl(T) R = base.t2r(Td) @ base.t2r(T).T l = np.r_[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]] # noqa if base.iszerovec(l): # diagonal matrix case if np.trace(R) > 0: # (1,1,1) case a = np.zeros((3,)) else: # (1, -1, -1), (-1, 1, -1) or (-1, -1, 1) case a = np.pi / 2 * (np.diag(R) + 1) else: # non-diagonal matrix case ln = base.norm(l) a = math.atan2(ln, np.trace(R) - 1) * l / ln return np.r_[d, a] if q0 is None: q0 = np.zeros((self.n,)) else: q0 = base.getvector(q0, self.n) if mask is not None: mask = base.getvector(mask, 6) if not self.n >= np.sum(mask): raise ValueError( 'Number of robot DOF must be >= the number ' 'of 1s in the mask matrix') else: mask = np.ones(6) W = np.diag(mask) tcount = 0 # Total iteration count # bool vector indicating revolute joints revolutes = np.array([link.isrevolute() for link in self]) q = q0 for Tk in T: iterations = 0 failure = None nm = None while True: # Update the count and test against iteration limit iterations += 1 if iterations > ilimit: failure = f"iteration limit {ilimit} exceeded" break e = angle_axis(self.fkine(q).A, Tk.A) # Are we there yet? E = 0.5 * e.T @ W @ e if E < tol: break # Compute the Jacobian J = self.jacob0(q) JtJ = J.T @ W @ J # Do the damped inverse Gauss-Newton with # Levenberg-Marquadt dq = np.linalg.inv( JtJ + (E + wN) * np.eye(self.n) ) @ J.T @ W @ e # Compute possible new value of q += dq # Wrap angles for revolute joints k = np.logical_and(q > np.pi, revolutes) q[k] -= 2 * np.pi k = np.logical_and(q < -np.pi, revolutes) q[k] += + 2 * np.pi nm = np.linalg.norm(W @ e) # qs = ", ".join(["{:8.3f}".format(qi) for qi in q]) # print(f"|e|={E:8.2g}: q={qs}") # LM process finished, for better or worse # failure will be None or an error message solution = iksol(q, failure is None, failure, iterations, nm) solutions.append(solution) tcount += iterations if len(T) == 1: return solutions[0] else: return solutions
def ikine_LM( self, T, q0=None, mask=None, ilimit=500, rlimit=100, tol=1e-10, L=0.1, Lmin=0, search=False, slimit=100, transpose=None): """ Numerical inverse kinematics by Levenberg-Marquadt optimization (Robot superclass) :param T: The desired end-effector pose or pose trajectory :type T: SE3 :param q0: initial joint configuration (default all zeros) :type q0: ndarray(n) :param mask: mask vector that correspond to translation in X, Y and Z and rotation about X, Y and Z respectively. :type mask: ndarray(6) :param ilimit: maximum number of iterations (default 500) :type ilimit: int :param rlimit: maximum number of consecutive step rejections (default 100) :type rlimit: int :param tol: final error tolerance (default 1e-10) :type tol: float :param L: initial value of lambda :type L: float (default 0.1) :param Lmin: minimum allowable value of lambda :type Lmin: float (default 0) :param search: search over all configurations :type search: bool :param slimit: maximum number of search attempts :type slimit: int (default 100) :param transpose: use Jacobian transpose with step size A, rather than Levenberg-Marquadt :type transpose: float :return: inverse kinematic solution :rtype: named tuple ``sol = robot.ikine_LM(T)`` are the joint coordinates (n) corresponding to the robot end-effector pose ``T`` which is an ``SE3`` object. This method can be used for robots with any number of degrees of freedom. The return value ``sol`` is a named tuple with elements: ============ ========== ============================================================ Element Type Description ============ ========== ============================================================ ``q`` ndarray(n) joint coordinates in units of radians or metres, or ``None`` ``success`` bool whether a solution was found ``reason`` str reason for the failure ``iterations`` int number of iterations ``residual`` float final value of cost function ============ ========== ============================================================ **Trajectory operation**: If ``len(T) > 1`` it is considered to be a trajectory, and the result is a list of named tuples such that ``sol[k]`` corresponds to ``T[k]``. The initial estimate of q for each time step is taken as the solution from the previous time step. **Underactuated robots:** For the case where the manipulator has fewer than 6 DOF the solution space has more dimensions than can be spanned by the manipulator joint coordinates. In this case we specify the ``mask`` option where the ``mask`` vector (6) specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask vector has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value should be 0 (for ignore) or 1. The number of non-zero elements must equal the number of manipulator DOF. For example when using a 3 DOF manipulator tool orientation might be unimportant, in which case use the option ``mask=[1 1 1 0 0 0]``. **Global search**: ``sol = robot.ikine_LM(T, search=True)`` as above but peforms a brute-force search with initial conditions chosen randomly from the entire configuration space. If a numerical solution is found from that initial condition, it is returned, otherwise another initial condition is chosen. .. note:: - Implements a Levenberg-Marquadt variable-step-size solver. - The tolerance is computed on the norm of the error between current and desired tool pose. This norm is computed from distances and angles without any kind of weighting. - The inverse kinematic solution is generally not unique, and depends on the initial guess ``q0``. - The default value of ``q0`` is zero which is a poor choice for most manipulators since it often corresponds to a kinematic singularity. - Such a solution is completely general, though much less efficient than analytic inverse kinematic solutions derived symbolically. - This approach allows a solution to be obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. - Joint offsets, if defined, are accounted for in the solution. - Joint limits are not considered in this solution. - If the search option is used any prismatic joint must have joint limits defined. :references: - Robotics, Vision & Control, P. Corke, Springer 2011, Section 8.4. :seealso: :func:`ikine_LMS`, :func:`ikine_unc`, :func:`ikine_con`, :func:`ikine_min` """ # noqa if not isinstance(T, SE3): T = SE3(T) solutions = [] if search: # Randomised search for a starting point # quiet = True qlim = self.qlim qspan = qlim[1] - qlim[0] # range of joint motion for k in range(slimit): # choose a random joint coordinate q0_k = np.random.rand(self.n) * qspan + qlim[0, :] # recurse into the solver solution = self.ikine_LM( T[0], q0_k, mask, ilimit, rlimit, tol, L, Lmin, False, slimit, transpose) if solution.success: q0 = solution.q if len(T) == 1: # we're done return solution else: # more to do on the trajectory solutions.append(solution) del T[0] else: # no solution found, stop now return iksol() if q0 is None: q0 = np.zeros((self.n,)) else: q0 = base.getvector(q0, self.n) if mask is not None: mask = base.getvector(mask, 6) if not self.n >= np.sum(mask): raise ValueError( 'Number of robot DOF must be >= the number ' 'of 1s in the mask matrix') else: mask = np.ones(6) W = np.diag(mask) tcount = 0 # Total iteration count rejcount = 0 # Rejected step count nm = 0 # bool vector indicating revolute joints revolutes = np.array([link.isrevolute() for link in self]) q = q0 for Tk in T: iterations = 0 Li = L # lambda failure = None while True: # Update the count and test against iteration limit iterations += 1 if iterations > ilimit: failure = f"iteration limit {ilimit} exceeded" break e = base.tr2delta(self.fkine(q).A, Tk.A) # Are we there yet? if np.linalg.norm(W @ e) < tol: # print(iterations) break # Compute the Jacobian J = self.jacobe(q) JtJ = J.T @ W @ J if transpose is not None: # Do the simple Jacobian transpose with constant gain dq = transpose * J.T @ e # lgtm [py/multiple-definition] else: # Do the damped inverse Gauss-Newton with # Levenberg-Marquadt dq = np.linalg.inv( JtJ + ((Li + Lmin) * np.eye(self.n)) ) @ J.T @ W @ e # Compute possible new value of qnew = q + dq # And figure out the new error enew = base.tr2delta(self.fkine(qnew).A, Tk.A) # Was it a good update? if np.linalg.norm(W @ enew) < np.linalg.norm(W @ e): # Step is accepted q = qnew e = enew Li /= 2 rejcount = 0 else: # Step is rejected, increase the damping and retry Li *= 2 rejcount += 1 if rejcount > rlimit: failure = f"rejected-step limit {rlimit} exceeded" break # Wrap angles for revolute joints k = np.logical_and(q > np.pi, revolutes) q[k] -= 2 * np.pi k = np.logical_and(q < -np.pi, revolutes) q[k] += + 2 * np.pi nm = np.linalg.norm(W @ e) # qs = ", ".join(["{:8.3f}".format(qi) for qi in q]) # print(f"λ={Li:8.2g}, |e|={nm:8.2g}: q={qs}") # LM process finished, for better or worse # failure will be None or an error message solution = iksol(q, failure is None, failure, iterations, nm) solutions.append(solution) tcount += iterations if len(T) == 1: return solutions[0] else: return solutions
def qlim(self, qlim_new): if qlim_new is None: self._qlim = None else: self._qlim = getvector(qlim_new, 2)
def __init__(self, y0=0, yf=1, T=None, time=False, traj='lspb', *inputs, **kwargs): """ :param y0: initial value :type y0: array_like(m) :param yf: final value :type yf: array_like(m) :param time: x is simulation time, defaults to False :type time: bool :param traj: trajectory type: 'lspb' [default] or 'tpoly' :type traj: str :param ``*inputs``: Optional incoming connections :type ``*inputs``: Block or Plug :param ``**kwargs``: common Block options :return: TRAJ block :rtype: Traj instance Create a trajectory block. A block that generates a trajectory using a trapezoidal or quintic polynomial profile. A simple triangle function with domain [0,10] and range [0,1] can be defined by:: INTERPOLATE(x=(0,5,10), y=(0,1,0)) We might also express this as:: INTERPOLATE(xy=[(0,0), (5,1), (10,0)]) The distance along the trajectory comes from: - Input port 0 - Simulation time, if ``time=True``. In this case the block has no input ports and is a ``Source`` not a ``Function``. """ self.time = time if time: nin = 0 self.blockclass = 'source' else: nin = 1 super().__init__(nin=nin, nout=3, inputs=inputs, **kwargs) self.type = 'function' y0 = base.getvector(y0) yf = base.getvector(yf) assert len(y0) == len(yf), 'y0 and yf must have same length' self.y0 = y0 self.yf = yf self.time = time self.T = T self.traj = traj self.outport_names(('y', 'yd', 'ydd'))
def init(self, x0=None, animation=None, plot=False, control=None): """ Initialize for simulation (superclass method) :param x0: Initial state, defaults to value given to Vehicle constructor :type x0: array_like(3) or array_like(2) :param animation: vehicle animation object, defaults to None :type animation: VehicleAnimation subclass, optional :param plot: Enable plotting, defaults to False :type plot: bool, optional Performs the following initializations: #. Clears the state history #. Sets state :math:`x = x_0` #. If a driver is attached, initialize it #. If plotting is enabled, initialize that If ``plot`` is set and no animation object is given, use a default ``VehiclePolygon('car')`` :seealso: :func:`VehicleAnimation` """ if x0 is not None: self._x = base.getvector(x0, 3) else: self._x = self._x0 self._x_hist = np.empty((0, 3)) if control is not None: self._control = control self._t = 0 # initialize the graphics self._plot = plot if plot: if animation is None: animation = self._animation # get default animation if set else: # use default animation animation = VehiclePolygon("car") self._animation = animation # setu[ the plot] plt.clf() self._ax = base.plotvol2(self._dim) plt.xlabel('x') plt.ylabel('y') self._ax.set_aspect('equal') self._ax.figure.canvas.set_window_title( f"Robotics Toolbox for Python (Figure {self._ax.figure.number})" ) animation.add() # add vehicle animation to axis self._timer = plt.figtext(0.85, 0.95, '') # display time counter # initialize the driver if isinstance(self._control, VehicleDriver): self._control.init(ax=self._ax)
def dot(self, value): return np.dot(self.A, base.getvector(value, 6))
def r(self, r_new): self._r = getvector(r_new, 3)
def transl2(x, y=None): """ Create SE(2) pure translation, or extract translation from SE(2) matrix **Create a translational SE(2) matrix** :param x: translation along X-axis :type x: float :param y: translation along Y-axis :type y: float :return: SE(2) transform matrix or the translation elements of a homogeneous transform :rtype: ndarray(3,3) - ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a pure translation. - ``T = transl2( V )`` as above but the translation is given by a 2-element list, dict, or a numpy array, row or column vector. .. runblock:: pycon >>> from spatialmath.base import * >>> import numpy as np >>> transl2(3, 4) >>> transl2([3, 4]) >>> transl2(np.array([3, 4])) **Extract the translational part of an SE(2) matrix** :param x: SE(2) transform matrix :type x: ndarray(3,3) :return: translation elements of SE(2) matrix :rtype: ndarray(2) - ``t = transl2(T)`` is the translational part of the SE(3) matrix ``T`` as a 2-element NumPy array. .. runblock:: pycon >>> from spatialmath.base import * >>> import numpy as np >>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]]) >>> transl2(T) .. note:: This function is compatible with the MATLAB version of the Toolbox. It is unusual/weird in doing two completely different things inside the one function. """ if np.isscalar(x): T = np.identity(3) T[:2, 2] = [x, y] return T elif base.isvector(x, 2): T = np.identity(3) T[:2, 2] = base.getvector(x, 2) return T elif base.ismatrix(x, (3, 3)): return x[:2, 2] else: ValueError('bad argument')
def __init__(self, c): self.plane = base.getvector(c, 4)
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")