def angle(q1, q2): """ Angle between two unit-quaternions :arg q0: unit-quaternion as a 4-vector :type q0: array_like :arg q1: unit-quaternion as a 4-vector :type q1: array_like :return: angle between the rotations [radians] :rtype: float If each of the input quaternions is considered a rotated coordinate frame, then the angle is the smallest rotation required about a fixed axis, to rotate the first frame into the second. References: Metrics for 3D rotations: comparison and analysis, Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2. .. warning:: There is no check that the passed values are unit-quaternions. """ # TODO different methods q1 = argcheck.getvector(q1, 4) q2 = argcheck.getvector(q2, 4) return 2.0 * math.atan2(tr.norm(q1 - q2), tr.norm(q1 + q2))
def _angle_axis_sekiguchi(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]] 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) # as per Sekiguchi paper if R[1, 0] > 0 and R[2, 1] > 0 and R[0, 2] > 0: a = np.pi / np.sqrt(2) * np.sqrt(n.diag(R) + 1) elif R[1, 0] > 0: # (2) a = np.pi / np.sqrt(2) * np.sqrt( n.diag(R) @ np.r_[1, 1, -1] + 1) elif R[0, 2] > 0: # (3) a = np.pi / np.sqrt(2) * np.sqrt( n.diag(R) @ np.r_[1, -1, 1] + 1) elif R[2, 1] > 0: # (4) a = np.pi / np.sqrt(2) * np.sqrt( n.diag(R) @ np.r_[-1, 1, 1] + 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]
def omega(cls, w): assert argcheck.isvector(w, 3), 'w must be a 3-vector' w = argcheck.getvector(w) theta = tr.norm(w) s = math.cos(theta / 2) v = math.sin(theta / 2) * tr.unitvec(w) return cls(s=s, v=v)
def control(v, t, x): goal = (6, 6) goal_heading = atan2(goal[1] - x[1], goal[0] - x[0]) d_heading = base.angdiff(goal_heading, x[2]) v.stopif(base.norm(x[0:2] - goal) < 0.1) return (1, d_heading)
def iseye(S, tol=10): """ Test if matrix is identity :param S: matrix to test :type S: ndarray(n,n) :param tol: tolerance in units of eps :type tol: float :return: whether matrix is a proper skew-symmetric matrix :rtype: bool Check if matrix is an identity matrix. We test that the trace tom row is zero We check that the norm of the residual is less than ``tol * eps``. .. runblock:: pycon >>> from spatialmath.base import * >>> import numpy as np >>> iseye(np.array([[1,0], [0,1]])) >>> iseye(np.array([[1,2], [0,1]])) :seealso: isskew, isskewa """ s = S.shape if len(s) != 2 or s[0] != s[1]: return False # not a square matrix return base.norm(S - np.eye(s[0])) < tol * _eps
def EulerVec(cls, w): r""" Construct a new SO(3) rotation matrix from an Euler rotation vector :param ω: rotation axis :type ω: 3-element array_like :return: SO(3) rotation :rtype: SO3 instance ``SO3.EulerVec(ω)`` is a unit quaternion that describes the 3D rotation defined by a rotation of :math:`\theta = \lVert \omega \rVert` about the unit 3-vector :math:`\omega / \lVert \omega \rVert`. Example: .. runblock:: pycon >>> from spatialmath import SO3 >>> SO3.EulerVec([0.5,0,0]) .. note:: :math:`\theta \eq 0` the result in an identity matrix, otherwise ``V`` must have a finite length, ie. :math:`|V| > 0`. :seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`~spatialmath.base.transforms3d.angvec2r` """ assert base.isvector(w, 3), 'w must be a 3-vector' w = base.getvector(w) theta = base.norm(w) return cls(base.angvec2r(theta, w), check=False)
def theta(self): """ Twist rotation :return: rotation about the twist axis :rtype: float - ``X.theta`` is the rotation about the twist axis in units of radians. """ return base.norm(self.w)
def theta(self): """ Twist angle (superclass method) :return: magnitude of rotation (1x1) about the twist axis in radians :rtype: float """ if self.N == 2: return abs(self.w) else: return base.norm(np.array(self.w))
def theta(self): """ Twist rotation :return: rotation about the twist axis :rtype: float ``X.theta`` is the rotation about the twist axis in units of radians. If we consider the twist as a screw, this is the rotation about the screw axis to achieve the rigid-body motion. """ return base.norm(self.w)
def unit(self): """ Unit twist TW.unit() is a Twist object representing a unit aligned with the Twist TW. """ if base.iszerovec(self.w): # rotational twist return Twist2(self.S / base.norm(S.w)) else: # prismatic twist return Twist2(base.unitvec(self.v), [0, 0, 0])
def angle(q1, q2): """ Angle between two unit-quaternions :arg q0: unit-quaternion :type q0: array_like(4) :arg q1: unit-quaternion :type q1: array_like(4) :return: angle between the rotations [radians] :rtype: float If each of the input quaternions is considered a rotated coordinate frame, then the angle is the smallest rotation required about a fixed axis, to rotate the first frame into the second. .. runblock:: pycon >>> from spatialmath.base import angle >>> from math import sqrt >>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0] # 90deg rotation about y-axis >>> angle(q1, q2) :References: - Metrics for 3D rotations: comparison and analysis, Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2. .. warning:: There is no check that the passed values are unit-quaternions. """ # TODO different methods q1 = base.getvector(q1, 4) q2 = base.getvector(q2, 4) return 2.0 * math.atan2(base.norm(q1 - q2), base.norm(q1 + q2))
def unit(self): """ Unitize twist (superclass property) :return: a unit twist :rtype: Twist3 or Twist2 ``twist.unit()`` is a Twist object representing a unit aligned with the Twist ``twist``. """ if base.iszerovec(self.w): # rotational twist return Twist3(self.S / base.norm(S.w)) else: # prismatic twist return Twist3(base.unitvec(self.v), [0, 0, 0])
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]] if base.iszerovec(l): # diagonal matrix case if np.trace(R) > 0: # (1,1,1) case a = np.zeros((3, )) else: 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]
def theta(self): """ Twist rotation :return: rotation about the twist axis :rtype: float ``X.theta`` is the rotation about the twist axis in units of radians. If we consider the twist as a screw, this is the rotation about the screw axis to achieve the rigid-body motion. Example: .. runblock:: pycon >>> from spatialmath import SE3, Twist3 >>> T = SE3(1, 2, 3) * SE3.Rx(0.3) >>> S = Twist3(T) >>> S.theta() """ return base.norm(self.w)
def Hp(self, xv, jf): """ Jacobian dh/dp % # J = self.Hp(X, K) is the Jacobian dh/dp (2x2) at the vehicle # state X (3x1) for map landmark K. % # J = self.Hp(X, P) as above but for a landmark at coordinate P (1x2). % # See also RangeBearingSensor.h. """ if base.isinteger(jf): xf = self.map.landmark(jf) else: xf = jf Delta = xf - xv[0:2] r = base.norm(Delta) return np.array([ [Delta([0]) / r, Delta([1]) / r], [-Delta([1]) / r**2, Delta([0]) / r**2], ])
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 angdist(self, other, metric=6): r""" Angular distance metric between rotations :param other: second rotation :type other: SO3 instance :param metric: metric, default is 6 :type metric: int :raises TypeError: if other is not an SO3 :return: angle in radians :rtype: float or ndarray ``R1.angdist(R2)`` is the geodesic norm, or geodesic distance between two rotations. Several metrics are supported, the first 5 are computed after conversion to unit quaternions. ====== =============================================================== Metric Details ====== =============================================================== 0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]` 1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]` 2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]` 3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]` 4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]` 5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]` 6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]` ====== =============================================================== Example: .. runblock:: pycon >>> from spatialmath import UnitQuaternion >>> R1 = SO3.Rx(0.3) >>> R2 = SO3.Ry(0.3) >>> print(R1.angdist(R1)) >>> print(R1.angdist(R2)) .. note:: - metrics 1, 2, 4 can throw ValueError "math domain error" due to numeric errors which push the argument of ``acos()`` marginally outside its domain [0, 1]. - metrics 2 and 3 are equivalent, but 3 is more robust :seealso: :func:`UnitQuaternion.angdist` """ if metric < 5: from spatialmath.quaternion import UnitQuaternion return UnitQuaternion(self).angdist(UnitQuaternion(other), metric=metric) elif metric == 5: op = lambda R1, R2: np.linalg.norm(np.eye(3) - R1 @ R2.T) elif metric == 6: op = lambda R1, R2: base.norm(base.trlog(R1 @ R2.T, twist=True)) else: raise ValueError('unknown metric') ad = self._op2(other, op) if isinstance(ad, list): return np.array(ad) else: return ad
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 ``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 ============ ========== =============================================== If ``success=False`` the ``q`` values will be valid numbers, but the solution will be in error. The amount of error is indicated by the ``residual``. **Trajectory operation**: If ``len(T) = m > 1`` it is considered to be a trajectory, then the result is a named tuple whose elements are ============ ============ =============================================== Element Type Description ============ ============ =============================================== ``q`` ndarray(m,n) joint coordinates in units of radians or metres ``success`` bool(m) whether a solution was found ``reason`` list of str reason for the failure ``iterations`` ndarray(m) number of iterations ``residual`` ndarray(m) final value of cost function ============ ============ =============================================== 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:: - See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_ - Implements a Levenberg-Marquadt variable-damping 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 E501 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, :] print('search starts at ', q0_k) # 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] # no solution found, stop now return iksol(None, False, None, None, None) 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]) revolutes = [] # This is a mixin class inherited by robot classes # Therefore it will never be used on its own and the # LGTM issue can be ignored for link in self: # lgtm [py/non-iterable-in-for-loop] if isinstance(self, rtb.DHRobot) or link.isjoint: revolutes.append(link.isrevolute) revolutes = np.array(revolutes) 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 base.norm(W @ e) < tol: 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] q += dq 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 dq = np.linalg.inv(JtJ + ( (Li + Lmin) * np.diag(np.diag(JtJ)))) @ J.T @ W @ e # print(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 iksol(np.vstack([sol.q for sol in solutions]), np.array([sol.success for sol in solutions]), [sol.reason for sol in solutions], np.array([sol.iterations for sol in solutions]), np.array([sol.residual for sol in solutions]))
def Omega(cls, w): return cls(quat.r2q(tr.angvec2r(tr.norm(w), tr.unitvec(w))), check=False)
def next(self, position): l = None y = None if all(self._goal == position): return None # we have arrived if self._step == 1: # Step 1. Move along the M-line toward the goal self.message(f"{position}: moving along the M-line (step 1)") # motion on line toward goal d = self._goal - position if abs(d[0]) > abs(d[1]): # line slope less than 45 deg dx = 1 if d[0] >= 0 else -1 # np.sign(d[0]) l = self._m_line y = -((position[0] + dx) * l[0] + l[2]) / l[1] dy = int(round(y - position[1])) else: # line slope greater than 45 deg dy = 1 if d[1] >= 0 else -1 # np.sign(d[1]) l = self._m_line x = -((position[1] + dy) * l[1] + l[2]) / l[0] dx = int(round(x - position[0])) # detect if next step is an obstacle if self.is_occupied(position + np.r_[dx, dy]): self.message(f" {position}: obstacle at {position + np.r_[dx, dy]}") self.h.append(position) self._step = 2 # transition to step 2 self.message(f" {position}: change to step 2") # get a list of all the points around the obstacle self._edge, _ = edgelist(self._occ_grid_nav == 0, position) self._k = 1 else: n = position + np.array([dx, dy]) if self._step == 2: # Step 2. Move around the obstacle until we reach a point # on the M-line closer than when we started. self.message(f"{position}: moving around the obstacle (step 2)") if self._k <= len(self._edge): n = self._edge[self._k] # next edge point else: # we are at the end of the list of edge points, we # are back where we started. Step 2.c test. raise RuntimeError('robot is trapped') # are we on the M-line now ? if abs(np.inner(np.r_[position, 1], self._m_line)) <= 0.5: self.message(f" {position}: crossed the M-line") # are we closer than when we encountered the obstacle? if base.norm(position - self._goal) < base.norm(self._h[-1] - self._goal): # self._j += 1 self._step = 1 # transition to step 1 self.message(f" {position}: change to step 1") return n # no, keep going around self._k += 1 return n