def delta(self, X2): r""" Infinitesimal difference of SE(3) values :return: differential motion vector :rtype: numpy.ndarray, shape=(6,) ``X1.delta(X2)`` is the differential motion (6x1) corresponding to infinitesimal motion (in the ``X1`` frame) from pose ``X1`` to ``X2``. The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]` represents infinitesimal translation and rotation. Example:: >>> x1 = SE3.Rx(0.3) >>> x2 = SE3.Rx(0.3001) >>> x1.delta(x2) array([0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 9.99999998e-05, 0.00000000e+00, 0.00000000e+00]) .. note:: - the displacement is only an approximation to the motion, and assumes that ``X1`` ~ ``X2``. - can be considered as an approximation to the effect of spatial velocity over a a time interval, ie. the average spatial velocity multiplied by time. :Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67. :seealso: :func:`~spatialmath.base.transforms3d.tr2delta` """ return base.tr2delta(self.A, X2.A)
def delta(self, X2): r""" Infinitesimal difference of SE(3) :param X2: :type X2: SE3 :return: differential motion vector :rtype: numpy.ndarray, shape=(6,) - ``X1.delta(X2)`` is the differential motion (6x1) corresponding to infinitesimal motion (in the X1 frame) from pose X1 to X2. The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z` represents infinitesimal translation and rotation. Notes: - the displacement is only an approximation to the motion T, and assumes that X1 ~ X2. - Can be considered as an approximation to the effect of spatial velocity over a a time interval, average spatial velocity multiplied by time. Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67. :seealso: :func:`~spatialmath.base.transform3d.tr2delta` """ return tr.tr2delta(self.A, X2.A)
def Delta(cls, d): r""" Create SE(3) from differential motion :param d: differential motion :type d: 6-element array_like :return: 4x4 homogeneous transformation matrix :rtype: SE3 instance - ``T = delta2tr(d)`` is an SE(3) representing differential motion :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`. Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67. :seealso: :func:`~delta`, :func:`~spatialmath.base.transform3d.delta2tr` """ return tr.tr2delta(d)
def output(self, t=None): return [base.tr2delta(self.inputs[0].A, self.inputs[1].A)]
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