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 test_Ttz(self):
fl = 1.543
nt.assert_array_almost_equal(rp.ET.Ttz(fl).T().A, sm.transl(0, 0, fl))
nt.assert_array_almost_equal(
rp.ET.Ttz(-fl).T().A, sm.transl(0, 0, -fl))
nt.assert_array_almost_equal(rp.ET.Ttz(0).T().A, sm.transl(0, 0, 0))
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures that return an ``SE3`` instance
with one or more values.
- ``SE3()`` null motion, value is the identity matrix.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` where ``T`` is a 4x4 Numpy array representing an SE(3)
matrix. If ``check`` is ``True`` check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` has ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3(X)`` where ``X`` is:
- ``SE3`` is a copy of ``X``
- ``SO3`` is the rotation of ``X`` with zero translation
- ``SE2`` is the z-axis rotation and x- and y-axis translation of
``X``
- ``SE3([X1, X2, ... XN])`` has ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif isinstance(x, SO3):
self.data = [base.r2t(_x) for _x in x.data]
elif type(x).__name__ == 'SE2':
def convert(x):
# convert SE(2) to SE(3)
out = np.identity(4, dtype=x.dtype)
out[:2, :2] = x[:2, :2]
out[:2, 3] = x[:2, 2]
return out
self.data = [convert(_x) for _x in x.data]
elif base.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [base.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [base.transl(T) for T in x]
else:
raise ValueError('bad argument to constructor')
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [base.transl(x, y, z)]
def test_T_real_2(self):
fl = 1.543
rx = rp.ET.rx()
ry = rp.ET.ry()
rz = rp.ET.rz()
tx = rp.ET.tx()
ty = rp.ET.ty()
tz = rp.ET.tz()
nt.assert_array_almost_equal(rx.T(fl).A, sm.trotx(fl))
nt.assert_array_almost_equal(ry.T(fl).A, sm.troty(fl))
nt.assert_array_almost_equal(rz.T(fl).A, sm.trotz(fl))
nt.assert_array_almost_equal(tx.T(fl).A, sm.transl(fl, 0, 0))
nt.assert_array_almost_equal(ty.T(fl).A, sm.transl(0, fl, 0))
nt.assert_array_almost_equal(tz.T(fl).A, sm.transl(0, 0, fl))
def test_T_real(self):
fl = 1.543
rx = rp.ET.TRx(joint=86)
ry = rp.ET.TRy(joint=86)
rz = rp.ET.TRz(joint=86)
tx = rp.ET.Ttx(joint=86)
ty = rp.ET.Tty(joint=86)
tz = rp.ET.Ttz(joint=86)
nt.assert_array_almost_equal(rx.T(fl), sm.trotx(fl))
nt.assert_array_almost_equal(ry.T(fl), sm.troty(fl))
nt.assert_array_almost_equal(rz.T(fl), sm.trotz(fl))
nt.assert_array_almost_equal(tx.T(fl), sm.transl(fl, 0, 0))
nt.assert_array_almost_equal(ty.T(fl), sm.transl(0, fl, 0))
nt.assert_array_almost_equal(tz.T(fl), sm.transl(0, 0, fl))
def Rand(cls, *, xrange=[-1, 1], yrange=[-1, 1], zrange=[-1, 1], N=1):
"""
Create SE(3) with pure random rotation
:param N: number of random rotations
:type N: int
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
- ``SE3.Rand()`` is a random SO(3) rotation.
- ``SE3.Rand(N)`` is an SO3 object containing a sequence of N random
rotations.
:seealso: `~spatialmath.quaternion.UnitQuaternion.Rand`
"""
X = np.random.uniform(low=xrange[0], high=xrange[1],
size=N) # random values in the range
Y = np.random.uniform(low=yrange[0], high=yrange[1],
size=N) # random values in the range
Z = np.random.uniform(low=yrange[0], high=zrange[1],
size=N) # random values in the range
R = SO3.Rand(N=N)
return cls([
tr.transl(x, y, z) @ tr.r2t(r.A)
for (x, y, z, r) in zip(X, Y, Z, R)
])
def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1):
"""
Create a random SE(3)
:param xrange: x-axis range [min,max], defaults to [-1, 1]
:type xrange: 2-element sequence, optional
:param yrange: y-axis range [min,max], defaults to [-1, 1]
:type yrange: 2-element sequence, optional
:param zrange: z-axis range [min,max], defaults to [-1, 1]
:type zrange: 2-element sequence, optional
:param N: number of random transforms
:type N: int
:return: homogeneous transformation matrix
:rtype: SE3 instance
Return an SE3 instance with random rotation and translation.
- ``SE3.Rand()`` is a random SE(3) translation.
- ``SE3.Rand(N)`` is an SE3 object containing a sequence of N random
poses.
:seealso: `~spatialmath.quaternion.UnitQuaternion.Rand`
"""
X = np.random.uniform(low=xrange[0], high=xrange[1],
size=N) # random values in the range
Y = np.random.uniform(low=yrange[0], high=yrange[1],
size=N) # random values in the range
Z = np.random.uniform(low=yrange[0], high=zrange[1],
size=N) # random values in the range
R = SO3.Rand(N=N)
return cls([
tr.transl(x, y, z) @ tr.r2t(r.A)
for (x, y, z, r) in zip(X, Y, Z, R)
],
check=False)
def Ty(cls, y):
"""
Create an SE(3) translation along the Y-axis
:param y: translation distance along the Y-axis
:type y: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Ty(y) is an SE(3) translation of ``y`` along the y-axis
Example::
>>> SE3.Ty(2)
SE3(array([[1., 0., 0., 0.],
[0., 1., 0., 2.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]))
>>> SE3.Ty([2,3])
SE3([
array([[1., 0., 0., 0.],
[0., 1., 0., 2.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]),
array([[1., 0., 0., 0.],
[0., 1., 0., 3.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) ])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(0, _y, 0) for _y in base.getvector(y)],
check=False)
def Tz(cls, z):
"""
Create an SE(3) translation along the Z-axis
:param z: translation distance along the Z-axis
:type z: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tz(z)` is an SE(3) translation of ``z`` along the z-axis
Example::
>>> SE3.Tz(2)
SE3(array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 2.],
[0., 0., 0., 1.]]))
>>> SE3.Tz([2,3])
SE3([
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 2.],
[0., 0., 0., 1.]]),
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 3.],
[0., 0., 0., 1.]]) ])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(0, 0, _z) for _z in base.getvector(z)],
check=False)
def __init__(self):
# deg = np.pi/180
mm = 1e-3
tool_offset = (103) * mm
flange = (107) * mm
# d7 = (58.4)*mm
L1 = Revolute(a=0.0,
d=0.333,
alpha=0.0,
qlim=np.array([-2.8973, 2.8973]),
mdh=1)
L2 = Revolute(a=0.0,
d=0.0,
alpha=-np.pi / 2,
qlim=np.array([-1.7628, 1.7628]),
mdh=1)
L3 = Revolute(a=0.0,
d=0.316,
alpha=np.pi / 2,
qlim=np.array([-2.8973, 2.8973]),
mdh=1)
L4 = Revolute(a=0.0825,
d=0.0,
alpha=np.pi / 2,
qlim=np.array([-3.0718, -0.0698]),
mdh=1)
L5 = Revolute(a=-0.0825,
d=0.384,
alpha=-np.pi / 2,
qlim=np.array([-2.8973, 2.8973]),
mdh=1)
L6 = Revolute(a=0.0,
d=0.0,
alpha=np.pi / 2,
qlim=np.array([-0.0175, 3.7525]),
mdh=1)
L7 = Revolute(a=0.088,
d=flange,
alpha=np.pi / 2,
qlim=np.array([-2.8973, 2.8973]),
mdh=1)
L = [L1, L2, L3, L4, L5, L6, L7]
# super(Panda, self).__init__(L, name = 'Panda', manufacturer = 'Franka Emika', tool = transl(0, 0, d7))
tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4)
super(PandaMDH, self).__init__(L,
name='Panda',
manufacturer='Franka Emika',
tool=tool)
# tool = xyzrpy_to_trans(0, 0, d7, 0, 0, -np.pi/4)
self.qz = np.array([0, 0, 0, 0, 0, 0, 0])
# self.qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg
self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
def Tx(cls, x):
"""
Create an SE(3) translation along the X-axis
:param x: translation distance along the X-axis
:type x: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tx(x)` is an SE(3) translation of ``x`` along the x-axis
Example::
>>> SE3.Tx(2)
SE3(array([[1., 0., 0., 2.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]))
>>> SE3.Tx([2,3])
SE3([
array([[1., 0., 0., 2.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]),
array([[1., 0., 0., 3.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) ])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(_x, 0, 0) for _x in base.getvector(x)],
check=False)
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 cost(q, *args):
T, pweight, col, stiffness = args
Tq = self.fkine(q).A
# translation error
dT = base.transl(T) - base.transl(Tq)
E = np.linalg.norm(dT) * pweight
# Rotation error
# Find dot product of two columns
dd = np.dot(T[0:3, col], Tq[0:3, col])
E += np.arccos(dd)**2 * 1000
if stiffness > 0:
# Enforce a continuity constraint on joints, minimum bend
E += np.sum(np.diff(q)**2) * stiffness
return E
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures:
- ``SE3()`` is an ``SE3`` instance with one value -- a 4x4 identity
matrix which corresponds to a null motion.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` is an ``SE3`` instance with the value ``T`` which is a 4x4
numpy array representing an SE(3) matrix. If ``check`` is ``True``
check the matrix belongs to SE(3).
- ``SE3(X)`` is an ``SE3`` instance with the same value as ``X``, ie.
a copy.
- ``SE3([T1, T2, ... TN])`` is an ``SE3`` instance with ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3([X1, X2, ... XN])`` is an ``SE3`` instance with ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif base.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [base.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [base.transl(T) for T in x]
else:
raise ValueError('bad argument to constructor')
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [base.transl(x, y, z)]
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:param x: translation distance along the X-axis
:type x: float
:param y: translation distance along the Y-axis
:type y: float
:param z: translation distance along the Z-axis
:type z: float
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
- ``SE3()`` is a null motion -- the identity matrix
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` where T is a 4x4 numpy array representing an SE(3) matrix. If ``check``
is ``True`` check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` where each Ti is a 4x4 numpy array representing an SE(3) matrix, is
an SE3 instance containing N rotations. If ``check`` is ``True``
check the matrix belongs to SE(3).
- ``SE3([X1, X2, ... XN])`` where each Xi is an SE3 instance, is an SE3 instance containing N rotations.
"""
super().__init__() # activate the UserList semantics
if x is None:
# SE3()
# empty constructor
self.data = [np.eye(4)]
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [tr.transl(x, y, z)]
elif y is None and z is None:
if argcheck.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [tr.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [tr.transl(T) for T in x]
else:
super()._arghandler(x, check=check)
else:
raise ValueError('bad argument to constructor')
def __init__(self):
# deg = np.pi/180
mm = 1e-3
tool_offset = (103) * mm
flange = (107) * mm
# d7 = (58.4)*mm
# This Panda model is defined using modified
# Denavit-Hartenberg parameters
L = [
RevoluteMDH(a=0.0,
d=0.333,
alpha=0.0,
qlim=np.array([-2.8973, 2.8973])),
RevoluteMDH(a=0.0,
d=0.0,
alpha=-np.pi / 2,
qlim=np.array([-1.7628, 1.7628])),
RevoluteMDH(a=0.0,
d=0.316,
alpha=np.pi / 2,
qlim=np.array([-2.8973, 2.8973])),
RevoluteMDH(a=0.0825,
d=0.0,
alpha=np.pi / 2,
qlim=np.array([-3.0718, -0.0698])),
RevoluteMDH(a=-0.0825,
d=0.384,
alpha=-np.pi / 2,
qlim=np.array([-2.8973, 2.8973])),
RevoluteMDH(a=0.0,
d=0.0,
alpha=np.pi / 2,
qlim=np.array([-0.0175, 3.7525])),
RevoluteMDH(a=0.088,
d=flange,
alpha=np.pi / 2,
qlim=np.array([-2.8973, 2.8973]))
]
tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4)
super().__init__(L,
name='Panda',
manufacturer='Franka Emika',
tool=tool)
# tool = xyzrpy_to_trans(0, 0, d7, 0, 0, -np.pi/4)
self._qz = np.array([0, 0, 0, 0, 0, 0, 0])
# self.qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg
self._qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
def Tz(cls, z):
"""
Create SE(3) translation along the Z-axis
:param z: translation distance along the Z-axis
:type z: float
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
`SE3.Tz(D)`` is an SE(3) translation of D along the z-axis
"""
return cls(tr.transl(0, 0, z), check=False)
def Ty(cls, y):
"""
Create SE(3) translation along the Y-axis
:param y: translation distance along the Y-axis
:type y: float
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
`SE3.Tz(D)`` is an SE(3) translation of D along the y-axis
"""
return cls(tr.transl(0, y, 0), check=False)
def Tx(cls, x):
"""
Create SE(3) translation along the X-axis
:param x: translation distance along the X-axis
:type x: float
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
`SE3.Tz(D)`` is an SE(3) translation of D along the x-axis
"""
return cls(tr.transl(x, 0, 0), check=False)
def test_T_real(self):
fl = 1.543
rx = rp.ET.TRx()
ry = rp.ET.TRy()
rz = rp.ET.TRz()
tx = rp.ET.Ttx()
ty = rp.ET.Tty()
tz = rp.ET.Ttz()
rx.j = 86
ry.j = 86
rz.j = 86
tx.j = 86
ty.j = 86
tz.j = 86
nt.assert_array_almost_equal(rx.T(fl).A, sm.trotx(fl))
nt.assert_array_almost_equal(ry.T(fl).A, sm.troty(fl))
nt.assert_array_almost_equal(rz.T(fl).A, sm.trotz(fl))
nt.assert_array_almost_equal(tx.T(fl).A, sm.transl(fl, 0, 0))
nt.assert_array_almost_equal(ty.T(fl).A, sm.transl(0, fl, 0))
nt.assert_array_almost_equal(tz.T(fl).A, sm.transl(0, 0, fl))
def __init__(self):
# deg = np.pi/180
mm = 1e-3
tool_offset = (103) * mm
flange = 0 * mm
# d7 = (58.4)*mm
# This Kuka model is defined using modified
# Denavit-Hartenberg parameters
L = [
RevoluteDH(a=0.0,
d=0,
alpha=np.pi / 2,
qlim=np.array([-2.8973, 2.8973])),
RevoluteDH(a=0.0,
d=0.0,
alpha=-np.pi / 2,
qlim=np.array([-1.7628, 1.7628])),
RevoluteDH(a=0.0,
d=0.40,
alpha=-np.pi / 2,
qlim=np.array([-2.8973, 2.8973])),
RevoluteDH(a=0.0,
d=0.0,
alpha=np.pi / 2,
qlim=np.array([-3.0718, -0.0698])),
RevoluteDH(a=0.0,
d=0.39,
alpha=np.pi / 2,
qlim=np.array([-2.8973, 2.8973])),
RevoluteDH(a=0.0,
d=0.0,
alpha=-np.pi / 2,
qlim=np.array([-0.0175, 3.7525])),
RevoluteDH(a=0.0,
d=flange,
alpha=0.0,
qlim=np.array([-2.8973, 2.8973]))
]
tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4)
super().__init__(L, name='LWR-IV', manufacturer='Kuka', tool=tool)
# tool = xyzrpy_to_trans(0, 0, d7, 0, 0, -np.pi/4)
self.addconfiguration("qz", [0, 0, 0, 0, 0, 0, 0])
def Rand(cls, *, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1): # pylint: disable=arguments-differ
"""
Create a random SE(3)
:param xrange: x-axis range [min,max], defaults to [-1, 1]
:type xrange: 2-element sequence, optional
:param yrange: y-axis range [min,max], defaults to [-1, 1]
:type yrange: 2-element sequence, optional
:param zrange: z-axis range [min,max], defaults to [-1, 1]
:type zrange: 2-element sequence, optional
:param N: number of random transforms
:type N: int
:return: SE(3) matrix
:rtype: SE3 instance
Return an SE3 instance with random rotation and translation.
- ``SE3.Rand()`` is a random SE(3) translation.
- ``SE3.Rand(N=N)`` is an SE3 object containing a sequence of N random
poses.
Example::
>>> SE3.Rand(N=2)
SE3([
array([[ 0.58076657, 0.64578702, -0.49565041, -0.78585825],
[-0.57373134, -0.10724881, -0.8119914 , 0.72069253],
[-0.57753142, 0.75594763, 0.30822173, 0.12291999],
[ 0. , 0. , 0. , 1. ]]),
array([[ 0.96481299, -0.26267256, -0.01179066, 0.80294729],
[ 0.06421463, 0.19190584, 0.97931028, -0.15021311],
[-0.25497525, -0.94560841, 0.20202067, 0.02684599],
[ 0. , 0. , 0. , 1. ]]) ])
:seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand`
"""
X = np.random.uniform(low=xrange[0], high=xrange[1],
size=N) # random values in the range
Y = np.random.uniform(low=yrange[0], high=yrange[1],
size=N) # random values in the range
Z = np.random.uniform(low=yrange[0], high=zrange[1],
size=N) # random values in the range
R = SO3.Rand(N=N)
return cls([
base.transl(x, y, z) @ base.r2t(r.A)
for (x, y, z, r) in zip(X, Y, Z, R)
],
check=False)
def trans(cls, x=None, y=None, z=None):
"""
Create SE(3) general translation
:param x: translation distance along the X-axis
:type x: float
:param y: translation distance along the Y-axis
:type y: float
:param z: translation distance along the Z-axis
:type z: float
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
- `SE3.Tz(X, Y, Z)`` is an SE(3) translation of X along the x-axis, Y along the
y-axis and Z along the z-axis.
- `SE3.Tz( [X, Y, Z] )`` as above but the translation is a 3-element array_like object.
"""
return cls(tr.transl(x, y, z))
def vexa(Omega, check=False):
r"""
Convert skew-symmetric matrix to vector
:param s: augmented skew-symmetric matrix
:type s: ndarray(3,3) or ndarray(4,4)
:param check: check if matrix is skew symmetric part is valid (default False, no check)
:type check: bool
:return: vector of unique values
:rtype: ndarray(3) or ndarray(6)
:raises ValueError: bad argument
``vexa(S)`` is the vector which has the corresponding augmented skew-symmetric matrix ``S``.
- ``S`` is 3x3 - se(2) case - where ``S`` :math:`= \left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]` then return :math:`[v_1, v_2, v_3]`.
- ``S`` is 4x4 - se(3) case - where ``S`` :math:`= \left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]` then return :math:`[v_1, v_2, v_3, v_4, v_5, v_6]`.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> S = skewa([1, 2, 3])
>>> print(S)
>>> vexa(S)
>>> S = skewa([1, 2, 3, 4, 5, 6])
>>> print(S)
>>> vexa(S)
.. note::
- This is the inverse of the function ``skewa``.
- Only rudimentary checking (zero diagonal) is done to ensure that the matrix
is actually skew-symmetric.
- The function takes the mean of the two elements that correspond to each unique
element of the matrix.
:seealso: :func:`skewa`, :func:`vex`
:SymPy: supported
"""
if Omega.shape == (4, 4):
return np.hstack((base.transl(Omega), vex(t2r(Omega), check=check)))
elif Omega.shape == (3, 3):
return np.hstack((base.transl2(Omega), vex(t2r(Omega), check=check)))
else:
raise ValueError("expecting a 3x3 or 4x4 matrix")
def __init__(self):
deg = np.pi / 180
mm = 1e-3
tool_offset = (103) * mm
et_list = [
ET.Ttz(0.333),
ET.TRz(joint=1),
ET.TRx(-90 * deg),
ET.TRz(joint=2),
ET.TRx(90 * deg),
ET.Ttz(0.316),
ET.TRz(joint=3),
ET.Ttx(0.0825),
ET.TRx(90 * deg),
ET.TRz(joint=4),
ET.Ttx(-0.0825),
ET.TRx(-90 * deg),
ET.Ttz(0.384),
ET.TRz(joint=5),
ET.TRx(90 * deg),
ET.TRz(joint=6),
ET.Ttx(0.088),
ET.TRx(90 * deg),
ET.Ttz(0.107),
ET.TRz(joint=7),
]
q_idx = [1, 3, 6, 9, 13, 15, 19]
tool = transl(0, 0, tool_offset) @ trotz(-np.pi / 4)
super(Panda, self).__init__(et_list,
q_idx,
name='Panda',
manufacturer='Franka Emika',
tool=tool)
self._qz = np.array([0, 0, 0, 0, 0, 0, 0])
self._qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg
def Tz(cls, z):
"""
Create an SE(3) translation along the Z-axis
:param z: translation distance along the Z-axis
:type z: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tz(z)` is an SE(3) translation of ``z`` along the z-axis
Example:
.. runblock:: pycon
>>> SE3.Tz(2)
>>> SE3.Tz([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(0, 0, _z) for _z in base.getvector(z)],
check=False)
def Tx(cls, x):
"""
Create an SE(3) translation along the X-axis
:param x: translation distance along the X-axis
:type x: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tx(x)` is an SE(3) translation of ``x`` along the x-axis
Example:
.. runblock:: pycon
>>> SE3.Tx(2)
>>> SE3.Tx([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(_x, 0, 0) for _x in base.getvector(x)],
check=False)
def Ty(cls, y):
"""
Create an SE(3) translation along the Y-axis
:param y: translation distance along the Y-axis
:type y: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Ty(y) is an SE(3) translation of ``y`` along the y-axis
Example:
.. runblock:: pycon
>>> SE3.Ty(2)
>>> SE3.Tz([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(0, _y, 0) for _y in base.getvector(y)],
check=False)
def test_Ttx(self):
fl = 1.543
nt.assert_array_almost_equal(rp.ET.Ttx(fl).T(), sm.transl(fl, 0, 0))
nt.assert_array_almost_equal(rp.ET.Ttx(-fl).T(), sm.transl(-fl, 0, 0))
nt.assert_array_almost_equal(rp.ET.Ttx(0).T(), sm.transl(0, 0, 0))
def __init__(self):
# deg = np.pi/180
mm = 1e-3
tool_offset = (103)*mm
flange = (107)*mm
# d7 = (58.4)*mm
# This Panda model is defined using modified
# Denavit-Hartenberg parameters
L = [
RevoluteMDH(
a=0.0,
d=0.333,
alpha=0.0,
qlim=np.array([-2.8973, 2.8973])
),
RevoluteMDH(
a=0.0,
d=0.0,
alpha=-np.pi/2,
qlim=np.array([-1.7628, 1.7628])
),
RevoluteMDH(
a=0.0,
d=0.316,
alpha=np.pi/2,
qlim=np.array([-2.8973, 2.8973])
),
RevoluteMDH(
a=0.0825,
d=0.0,
alpha=np.pi/2,
qlim=np.array([-3.0718, -0.0698])
),
RevoluteMDH(
a=-0.0825,
d=0.384,
alpha=-np.pi/2,
qlim=np.array([-2.8973, 2.8973])
),
RevoluteMDH(
a=0.0,
d=0.0,
alpha=np.pi/2,
qlim=np.array([-0.0175, 3.7525])
),
RevoluteMDH(
a=0.088,
d=flange,
alpha=np.pi/2,
qlim=np.array([-2.8973, 2.8973])
)
]
tool = transl(0, 0, tool_offset) @ trotz(-np.pi/4)
super().__init__(
L,
name='Panda',
manufacturer='Franka Emika',
meshdir='meshes/FRANKA-EMIKA/Panda',
tool=tool)
self.addconfiguration("qz", [0, 0, 0, 0, 0, 0, 0])
self.addconfiguration("qr", np.r_[0, -0.3, 0, -2.2, 0, 2.0, np.pi/4])
