def __eq__(self, line): """ Plucker.eq Test if two lines are equivalent PL1 == PL2 is true if the Plucker objects describe the same line in space. Note that because of the over parameterization, lines can be equivalent even if they have different parameters. """ return abs( 1 - np.dot(sm.unitvec(self.vec), sm.unitvec(line.vec))) < 10 * _eps
def commonperp(self, l2): # pylint: disable=no-self-argument """ Common perpendicular to two lines :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: Perpendicular line :rtype: Plucker or None ``l1.commonperp(l2)`` is the common perpendicular line between the two lines. Returns ``None`` if the lines are parallel. :seealso: Plucker.intersect """ l1 = self if l1 | l2: # no common perpendicular if lines are parallel return None else: # lines are skew or intersecting w = np.cross(l1.w, l2.w) v = np.cross(l1.v, l2.w) - np.cross(l2.v, l1.w) + \ (l1 * l2) * np.dot(l1.w, l2.w) * base.unitvec(np.cross(l1.w, l2.w)) return Plucker(v, w)
def uw(self): """ Plucker.uw Line direction as a unit vector self.UW is a unit-vector parallel to the line """ return sm.unitvec(self.w)
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 R(cls, a, q, p=None): """ Construct a new rotational 3D twist :param a: Twist axis or line of action :type a: 3-element array_like :param q: Point on the line of action :type q: 3-element array_like :param p: pitch, defaults to None :type p: float, optional :return: a rotational or helical twist :rtype: Twist instance A revolute twist with a line of action in the z-direction and passing through (1, 2, 0) would be:: >>> Twist3.R([0, 0, 1], [1, 2, 0]) Twist3([2, -1, -0, 0, 0, 1]) """ w = base.unitvec(base.getvector(a, 3)) v = -np.cross(w, base.getvector(q, 3)) if p is not None: pitch = base.getvector(p, 3) v = v + pitch * w return cls(v, w)
def Revolute(cls, a, q, pitch=None): """ Construct a new unit rotational 3D twist :param a: Twist axis or line of action :type a: array_like(3) :param q: Point on the line of action :type q: array_like(3) :param p: pitch, defaults to None :type p: float, optional :return: a rotational or helical twist :rtype: Twist instance A revolute twist with a line of action in the z-direction and passing through (1, 2, 0) would be: .. runblock:: pycon >>> from spatialmath import Twist3 >>> Twist3.Revolute([0, 0, 1], [1, 2, 0]) """ w = base.unitvec(base.getvector(a, 3)) v = -np.cross(w, base.getvector(q, 3)) if pitch is not None: v = v + pitch * w return cls(v, w)
def intersects(self, l2): # pylint: disable=no-self-argument """ Intersection point of two lines :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: 3D intersection point :rtype: numpy.ndarray, shape=(3,) or None ``l1.intersects(l2)`` is the point of intersection of the two lines, or ``None`` if the lines do not intersect or are equivalent. :seealso: Plucker.commonperp, Plucker.eq, Plucker.__xor__ """ l1 = self if l1 ^ l2: # lines do intersect return -(np.dot(l1.v, l2.w) * np.eye(3, 3) + \ l1.w.reshape((3,1)) @ l2.v.reshape((1,3)) - \ l2.w.reshape((3,1)) @ l1.v.reshape((1,3))) * base.unitvec(np.cross(l1.w, l2.w)) else: # lines don't intersect return None
def __eq__(l1, l2): """ Test if two lines are equivalent :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: Plucker :return: line equivalence :rtype: bool ``L1 == L2`` is true if the Plucker objects describe the same line in space. Note that because of the over parameterization, lines can be equivalent even if their coordinate vectors are different. """ return abs( 1 - np.dot(sm.unitvec(l1.vec), sm.unitvec(l2.vec))) < 10*_eps
def uw(self): """ Line direction as a unit vector :return: Line direction :rtype: numpy.ndarray, shape=(3,) ``line.uw`` is a unit-vector parallel to the line. """ return base.unitvec(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 P(cls, a): """ Construct a new 2D primsmatic Twist object :param a: displacment :type a: 2-element array-like :return: 2D prismatic twist :rtype: Twist2 instance - ``Twist3.P(q)`` is a 2D Twist object representing 2D-translation in the direction ``a``. """ w = 0 v = base.unitvec(base.getvector(a, 2)) return cls(v, w)
def P(cls, a): """ Construct a new prismatic 3D twist :param a: Twist axis or line of action :type a: 3-element array_like :return: a prismatic twist :rtype: Twist instance """ w = np.r_[0, 0, 0] v = base.unitvec(base.getvector(a, 3)) return cls(v, w)
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 test_constructor1(self): # construct from 6-vector L = Plucker([1, 2, 3, 4, 5, 6]) self.assertIsInstance(L, Plucker) nt.assert_array_almost_equal(L.v, np.r_[1, 2, 3]) nt.assert_array_almost_equal(L.w, np.r_[4, 5, 6]) # construct from object L2 = Plucker(L) self.assertIsInstance(L, Plucker) nt.assert_array_almost_equal(L2.v, np.r_[1, 2, 3]) nt.assert_array_almost_equal(L2.w, np.r_[4, 5, 6]) # construct from point and direction L = Plucker.PointDir([1, 2, 3], [4, 5, 6]) self.assertTrue(L.contains([1, 2, 3])) nt.assert_array_almost_equal(L.uw, base.unitvec([4, 5, 6]))
def unit(self): """ Unitize twist (superclass property) :return: a unit twist :rtype: Twist3 or Twist2 - ``S.unit()`` is a Twist3 object representing a unit twist aligned with the Twist ``S``. Example: .. runblock:: pycon >>> from spatialmath import Twist3 >>> S = Twist3([1,2,3,4,5,6]) >>> S.unit() """ return Twist3(base.unitvec(self.S))
def P(cls, a): """ Construct a new prismatic 3D twist :param a: Twist axis or line of action :type a: 3-element array_like :return: a prismatic twist :rtype: Twist instance A prismatic twist with a line of action in the z-direction would be:: >>> Twist3.P([0, 0, 1]) Twist3([0, 0, 1, 0, 0, 0]) """ w = np.r_[0, 0, 0] v = base.unitvec(base.getvector(a, 3)) return cls(v, w)
def commonperp(p1, p2): """ Plucker.commonperp Common perpendicular to two lines P = PL1.commonperp(self2) is a Plucker object representing the common perpendicular line between the lines represented by the Plucker objects PL1 and PL2. See also Plucker.intersect. """ if isparallel(p1, p2): # no common perpendicular if lines are parallel return None else: w = np.cross(p1.w, p2.w) v = np.cross(p1.v, p2.w) - np.cross(p2.v, p1.w) + \ (p1 * p2) * np.dot(p1.w, p2.w) * sm.unitvec(np.cross(p1.w, p2.w)) return Plucker(np.r_[v, w])
def R(cls, a, q, p=None): """ Construct a new rotational 3D twist :param a: Twist axis or line of action :type a: 3-element array_like :param q: Point on the line of action :type q: 3-element array_like :param p: pitch, defaults to None :type p: float, optional :return: a rotational or helical twist :rtype: Twist instance """ w = base.unitvec(base.getvector(a, 3)) v = -np.cross(w, base.getvector(q, 3)) if p is not None: pitch = base.getvector(p, 3) v = v + pitch * w return cls(v, w)
def Prismatic(cls, a): """ Construct a new 2D primsmatic Twist object :param a: Displacment :type a: array-like(2) :return: 2D prismatic twist :rtype: Twist2 instance - ``Twist2.Prismatic(a)`` is a 2D Twist object representing 2D-translation in the direction ``a``. Example: .. runblock:: pycon >>> from spatialmath import Twist2 >>> Twist2.Prismatic([1, 2]) """ w = 0 v = base.unitvec(base.getvector(a, 2)) return cls(v, w)
def intersects(p1, p2): """ Plucker.intersects Find intersection of two lines P = P1.intersects(P2) is the point of intersection (3x1) of the lines represented by Plucker objects P1 and P2. P = [] if the lines do not intersect, or the lines are equivalent. Notes:: - Can be used in operator form as P1^P2. - Returns [] if the lines are equivalent (P1==P2) since they would intersect at an infinite number of points. See also Plucker.commonperp, Plucker.eq, Plucker.mpower. """ if p1 ^ p2: return -(np.dot(p1.v, p2.w) * np.eye(3, 3) + \ p1.w.reshape((3,1)) @ p2.v.reshape((1,3)) - p2.w.reshape((3,1)) @ p1.v.reshape((1,3))) * sm.unitvec(np.cross(p1.w, p2.w)) else: return None
def Prismatic(cls, a): """ Construct a new unit prismatic 3D twist :param a: Twist axis or line of action :type a: array_like(3) :return: a prismatic twist :rtype: Twist instance A prismatic twist with a line of action in the z-direction would be: .. runblock:: pycon >>> from spatialmath import Twist3 >>> Twist3.Prismatic([0, 0, 1]) """ w = np.r_[0, 0, 0] v = base.unitvec(base.getvector(a, 3)) return cls(v, w)
def Omega(cls, w): return cls(quat.r2q(tr.angvec2r(tr.norm(w), tr.unitvec(w))), check=False)