def ad(self):
"""
Logarithm of adjoint of 3D twist
:return: logarithm of adjoint matrix
:rtype: ndarray(6,6)
``S.ad()`` is the 6x6 logarithm of the adjoint matrix of the
corresponding homogeneous transformation.
For a twist representing motion from frame {B} to {A}, the adjoint will
transform a twist relative to frame {A} to one relative to frame {B}.
Example:
.. runblock:: pycon
>>> from spatialmath import Twist3
>>> S = Twist3.Rx(0.3)
>>> S.ad()
.. note:: An alternative approach to computing the adjoint is to exponentiate this 6x6
matrix.
:seealso: :func:`Twist3.Ad`
"""
return np.block([[base.skew(self.w),
base.skew(self.v)],
[np.zeros((3, 3)),
base.skew(self.w)]])
def ad(self):
"""
Twist3.ad Logarithm of adjoint
TW.ad is the logarithm of the adjoint matrix of the corresponding
homogeneous transformation.
See also SE3.Ad.
"""
return np.array([
base.skew(self.w),
base.skew(self.v), [np.zeros((3, 3)),
base.skew(self.w)]
])
def ad(self):
"""
Logarithm of adjoint
:return: logarithm of adjoint matrix
:rtype: numpy.ndarray, shape=(6,6)
- ``X.ad()`` is the 6x6 logarithm of the adjoint matrix of the corresponding
homogeneous transformation.
"""
return np.array([
base.skew(self.w),
base.skew(self.v), [np.zeros((3, 3)),
base.skew(self.w)]
])
def dotb(q, w):
"""
Rate of change of unit-quaternion
:arg q0: unit-quaternion
:type q0: array_like(4)
:arg w: 3D angular velocity in body frame
:type w: array_like(3)
:return: rate of change of unit quaternion
:rtype: ndarray(4)
``dotb(q, w)`` is the rate of change of the elements of the unit quaternion ``q``
which represents the orientation of a body frame with angular velocity ``w`` in
the body frame.
.. runblock:: pycon
>>> from spatialmath.base import dotb, qprint
>>> from math import sqrt
>>> q = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis
>>> dotb(q, [1, 2, 3])
.. warning:: There is no check that the passed values are unit-quaternions.
"""
q = base.getvector(q, 4)
w = base.getvector(w, 3)
E = q[0] * (np.eye(3, 3)) + base.skew(q[1:4])
return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
def __init__(self, m=None, c=None, I=None):
"""
Create a new spatial inertia
:param m: mass
:type m: float
:param c: centre of mass relative to link frame
:type c: 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, C, I)`` is a spatial inertia object for a rigid-body
with mass ``M``, centre of mass at ``C`` 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).
"""
if m is not None and c is not None:
assert arg.isvector(c, 3), 'c must be 3-vector'
if I is None:
I = np.zeros((3,3))
else:
assert arg.ismatrix(I, (3,3)), 'I must be 3x3 matrix'
C = tr.skew(c)
self.I = np.array([
[m * np.eye(3), m @ C.T],
[m @ C, I + m * C @ C.T]
])
elif m is None and c is None and I is not None:
assert arg.ismatrix(I, (6, 6)), 'I must be 6x6 matrix'
def Ad(self):
"""
Adjoint of SO(3)
:return: adjoint matrix
:rtype: numpy.ndarray, shape=(3,3)
- ``SEO.Ad`` is the 6x6 adjoint matrix
:seealso: Twist.ad.
"""
return np.array([[self.R, tr.skew(self.t) @ self.R],
[np.zeros((3, 3)), self.R]])
def ad(self):
"""
Logarithm of adjoint of 3D twist
:return: logarithm of adjoint matrix
:rtype: numpy.ndarray, shape=(6,6)
``t.ad()`` is the 6x6 logarithm of the adjoint matrix of the
corresponding homogeneous transformation.
For a twist representing motion from frame {B} to {A}, the adjoint will
transform a twist relative to frame {A} to one relative to frame {B}.
.. notes::
- An alternative path to the adjoint is to exponentiate this 6x6
matrix.
:seealso: :func:`Twist3.Ad`
"""
return np.array([
base.skew(self.w),
base.skew(self.v), [np.zeros((3, 3)),
base.skew(self.w)]
])
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).
:SymPy: supported
"""
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)
M = np.diag((m, ) * 3) # sym friendly
I = np.block([[M, m * C.T], [m * C, I + m * C @ C.T]])
else:
raise ValueError('bad values')
self.data = [I]
def __rmul__(right, left):
"""
Line transformation
:param left: Rigid-body transform
:type left: SE3
:param right: Right operand
:type right: Plucker
:return: transformed line
:rtype: Plucker
``T * line`` is the line transformed by the rigid body transformation ``T``.
:seealso: Plucker.__mul__
"""
if isinstance(left, SE3):
A = np.r_[np.c_[left.R, sm.skew(-left.t) @ left.R], np.c_[np.zeros(
(3, 3)), left.R]]
return Plucker(A @ right.vec) # premultiply by SE3
else:
raise ValueError('bad arguments')
def dotb(q, w):
"""
Rate of change of unit-quaternion
:arg q0: unit-quaternion as a 4-vector
:type q0: array_like
:arg w: angular velocity in body frame as a 3-vector
:type w: array_like
:return: rate of change of unit quaternion
:rtype: numpy.ndarray, shape=(4,)
``dotb(q, w)`` is the rate of change of the elements of the unit quaternion ``q``
which represents the orientation of a body frame with angular velocity ``w`` in
the body frame.
.. warning:: There is no check that the passed values are unit-quaternions.
"""
q = argcheck.getvector(q, 4)
w = argcheck.getvector(w, 3)
E = q[0] * (np.eye(3, 3)) + tr.skew(q[1:4])
return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
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")
