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])