def adjoint2(T):
# http://ethaneade.com/lie.pdf
if T.shape == (3, 3):
# SO(2) adjoint
return np.identity(2)
elif T.shape == (3, 3):
# SE(2) adjoint
(R, t) = base.tr2rt(T)
return np.block([[R, np.c_[t[1], -t[0]].T], [0, 0, 1]])
else:
raise ValueError('bad argument')
def draw(self, T):
R, t = base.tr2rt(T)
p = R @ self.p
# specific to a single Quiver
self.h.set_offsets(t) # shift the origin
self.h.set_UVC(p[0], p[1])
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new 2D Twist object
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
a 2x2 matrix.
- ``Twist2(T)`` is a 2D Twist object representing the SE(2) rigid-body motion expressed as
a 3x3 matrix.
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
moment vector (1 element) and direction vector (2 elements).
"""
super().__init__() # enable UserList superpowers
if arg is None:
self.data = [np.r_[0.0, 0.0, 0.0, ]]
elif isinstance(arg, Twist2):
# clone it
self.data = [np.r_[arg.v, arg.w]]
elif argcheck.isvector(arg, 3):
s = argcheck.getvector(arg)
self.data = [s]
elif argcheck.isvector(arg, 2) and argcheck.isvector(w, 1):
v = argcheck.getvector(arg)
w = argcheck.getvector(w)
self.data = [np.r_[v, w]]
elif isinstance(arg, SE2):
S = tr.trlog2(arg.A) # use closed form for SE(2)
skw, v = tr.tr2rt(S)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif Twist2.isvalid(arg):
# it's an augmented skew matrix, unpack it
skw, v = tr.tr2rt(arg)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif isinstance(arg, list):
# construct from a list
if isinstance(arg[0], np.ndarray):
# possibly a list of numpy arrays
if check:
assert all(
map(lambda x: Twist2.isvalid(x), arg)
), 'all elements of list must have valid shape and value for the class'
self.data = arg
elif type(arg[0]) == type(self):
# possibly a list of objects of same type
assert all(
map(lambda x: type(x) == type(self),
arg)), 'all elements of list must have same type'
self.data = [x.S for x in arg]
elif type(arg[0]) == list:
# possibly a list of 3-lists
assert all(
map(lambda x: isinstance(x, list) and len(x) == 3,
arg)), 'all elements of list must have same type'
self.data = [np.r_[x] for x in arg]
else:
raise ValueError('bad list argument to constructor')
else:
raise ValueError('bad argument to constructor')
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new Twist object
TW = Twist(T) is a Twist object representing the SE(2) or SE(3)
homogeneous transformation matrix T (3x3 or 4x4).
TW = Twist(V) is a twist object where the vector is specified directly.
3D CASE:
TW = Twist('R', A, Q) is a Twist object representing rotation about the
axis of direction A (3x1) and passing through the point Q (3x1).
%
TW = Twist('R', A, Q, P) as above but with a pitch of P (distance/angle).
TW = Twist('T', A) is a Twist object representing translation in the
direction of A (3x1).
Notes:
- The argument 'P' for prismatic is synonymous with 'T'.
"""
super().__init__() # enable UserList superpowers
if arg is None:
self.data = [np.r_[0, 0, 0, 0, 0, 0]]
elif isinstance(arg, Twist):
# clone it
self.data = [np.r_[arg.v, arg.w]]
elif argcheck.isvector(arg, 6):
s = argcheck.getvector(arg)
self.data = [s]
elif argcheck.isvector(arg, 3) and argcheck.isvector(w, 3):
v = argcheck.getvector(arg)
w = argcheck.getvector(w)
self.data = [np.r_[v, w]]
elif isinstance(arg, SE3):
S = tr.trlog(arg.A) # use closed form for SE(3)
skw, v = tr.tr2rt(S)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif Twist.isvalid(arg):
# it's an augmented skew matrix, unpack it
skw, v = tr.tr2rt(arg)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif isinstance(arg, list):
# construct from a list
if isinstance(arg[0], np.ndarray):
# possibly a list of numpy arrays
if check:
assert all(
map(lambda x: Twist.isvalid(x), arg)
), 'all elements of list must have valid shape and value for the class'
self.data = arg
elif type(arg[0]) == type(self):
# possibly a list of objects of same type
assert all(
map(lambda x: type(x) == type(self),
arg)), 'all elements of list must have same type'
self.data = [x.S for x in arg]
elif type(arg[0]) == list:
# possibly a list of 6-lists
assert all(
map(lambda x: isinstance(x, list) and len(x) == 6,
arg)), 'all elements of list must have same type'
self.data = [np.r_[x] for x in arg]
else:
raise ValueError('bad list argument to constructor')
else:
raise ValueError('bad argument to constructor')