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)
