def pop(self, i=-1): """ Pop value :param i: item in the list to pop, default is last :type i: int :return: the popped value :rtype: instance of same type :raises IndexError: if there are no values to pop Removes a value from the value list and returns it. The original instance is modified. Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1) >>> tail = e.pop() >>> tail >>> e """ item = ETS() item.data = [super().pop(i)] return item
def __getitem__(self, i): """ Index or slice an ETS :param i: the index or slince :type i: int or slice :return: Elementary transform (sequence) :rtype: ETS Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1) >>> e[0] >>> e[1] >>> e[1:3] """ item = ETS() data = self.data[i] # can be [2] or slice, eg. [3:5] # ensure that data is always a list if isinstance(data, list): item.data = data else: item.data = [data] return item
def __mul__(self, rest): """ Overloaded ``*`` operator :return: [description] :rtype: [type] Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> e1 = ETS.rz() >>> len(e1) >>> e2= ETS.tx(2) >>> len(e2) >>> e = e1 * e2 >>> len(e) .. note:: The ``*`` operator implies composition, but actually the result is a new ETS instance that contains the concatenation of the left and right operands in an internal list. In this example we see the length of the product is 2. """ prod = ETS() prod.data = self.data + rest.data return prod
def eliminate(prev, this): if this.isjoint or prev.isjoint: return None new = None if prev.axis == 'Rx' and this.axis == 'ty': # RX.TY -> TZ.RX new = ETS.tx(prev.eta) * prev elif prev.axis == 'Rx' and this.axis == 'tz': # RX.TZ -> TY.RX new = ETS.ty(-prev.eta) * prev elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TX-> TZ.RY new = ETS.tz(-prev.eta) * prev elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TZ-> TX.RY new = ETS.tx(prev.eta) * prev elif prev.axis == 'ty' and this.axis == 'Rx': # TY.RX -> RX.TZ new = this * ETS.tz(-this.eta) elif prev.axis == 'tx' and this.axis == 'Rz': # TX.RZ -> RZ.TY new = this * ETS.tz(this.eta) elif prev.axis == 'Ry' and this.axis == 'Rx': # RY(Q).RX -> RX.RZ(-Q) new = this * ETS.Rz(-prev.eta) elif prev.axis == 'Rx' and this.axis == 'Ry': # RX.RY -> RZ.RX new = ETS.Rz(this.eta) * prev elif prev.axis == 'Rz' and this.axis == 'Rx': # RZ.RX -> RX.RY new = this * ETS.Ry(this.eta) return new
def test_init_elink_branched(self): robot = ERobot([ ELink(ETS.rz(), name='link1'), ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(), name='link2', parent='link1'), ELink(ETS.tx(1), name='ee_1', parent='link2'), ELink(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(), name='link3', parent='link1'), ELink(ETS.tx(1), name='ee_2', parent='link3') ]) self.assertEqual(robot.n, 3) for i in range(5): self.assertIsInstance(robot[i], ELink) self.assertTrue(robot[0].isrevolute) self.assertTrue(robot[1].isrevolute) self.assertTrue(robot[3].isrevolute) self.assertIs(robot[0].parent, None) self.assertIs(robot[1].parent, robot[0]) self.assertIs(robot[2].parent, robot[1]) self.assertIs(robot[3].parent, robot[0]) self.assertIs(robot[4].parent, robot[3]) self.assertEqual(robot[0].children, [robot[1], robot[3]]) self.assertEqual(robot[1].children, [robot[2]]) self.assertEqual(robot[2].children, []) self.assertEqual(robot[3].children, [robot[4]]) self.assertEqual(robot[2].children, [])
def inv(self): r""" Inverse of ETS :return: [description] :rtype: ETS instance The inverse of a given ETS. It is computed as the inverse of the individual ETs in the reverse order. .. math:: (\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} ) Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> e = ETS.rz(j=2) * ETS.tx(1) * ETS.rx(j=3,flip=True) * ETS.tx(1) >>> print(e) >>> print(e.inv()) >>> q = [1,2,3,4] >>> print(e.eval(q) * e.inv().eval(q)) .. note:: It is essential to use explicit joint indices to account for the reversed order of the transforms. """ inv = ETS() for e in reversed(self.data): # get the named tuple from the list, and convert to a dict etdict = e._asdict() # update the dict to make this an inverse if etdict['joint']: etdict['flip'] ^= True # toggle flip status elif etdict['axis'][0] == 'C': etdict['T'] = trinv(etdict['T']) elif etdict['eta'] is not None: etdict['T'] = trinv(etdict['T']) etdict['eta'] = -etdict['eta'] et = ETS() # create a new ETS instance et.data = [self.ets_tuple(**etdict)] # set it's data from the dict inv *= et return inv
def inv(self): r""" Inverse of ETS :return: [description] :rtype: ETS instance The inverse of a given ETS. It is computed as the inverse of the individual ETs in the reverse order. .. math:: (\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} ) Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> e = ETS.rz(j=2) * ETS.tx(1) * ETS.rx(j=3,flip=True) * ETS.tx(1) >>> print(e) >>> print(e.inv()) >>> q = [1,2,3,4] >>> print(e.eval(q) * e.inv().eval(q)) .. note:: It is essential to use explicit joint indices to account for the reversed order of the transforms. """ # noqa inv = ETS() for ns in reversed(self.data): # get the namespace from the list # clone it, and invert the elements to create an inverse nsi = copy.copy(ns) if nsi.joint: nsi.flip ^= True # toggle flip status elif nsi.axis[0] == 'C': nsi.T = trinv(nsi.T) elif nsi.eta is not None: nsi.T = trinv(nsi.T) nsi.eta = -nsi.eta et = ETS() # create a new ETS instance et.data = [nsi] # set it's data from the dict inv *= et return inv
def SE3(cls, t, rpy=None, tol=100): """ Convert an SE3 to an ETS :param t: Translation vector, or an SE(3) matrix :type t: array_like(3) or SE3 instance :param rpy: roll-pitch-yaw angles in XYZ order :type rpy: array_like(3) :param tol: Elements small than this many eps are considered as being zero, defaults to 100 :type tol: int, optional :return: ET sequence :rtype: ETS instance Create an ETS from the non-zero translational and rotational components. - ``SE3(t, rpy)`` convert translation ``t`` and rotation given by XYZ roll-pitch-yaw angles ``rpy`` into an ETS. - ``SE3(X)`` as above but convert from an SE3 instance ``X``. Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> ETS.SE3(SE3(1,2,3)) >>> ETS.SE3(SE3.Rx(90, 'deg')) .. warning:: ``SE3.rpy()`` is used to determine rotation about the x-, y- and z-axes. For a y-axis rotation with magnitude greater than 180° this will result in a non-minimal representation with non-zero amounts of x- and z-axis rotation. :seealso: :func:`~SE3.rpy` """ if isinstance(t, SE3): T = t t = removesmall(T.t, tol) rpy = removesmall(T.rpy(order='zyx')) ets = ETS() if t[0] != 0: ets *= ETS.tx(t[0]) if t[1] != 0: ets *= ETS.ty(t[1]) if t[2] != 0: ets *= ETS.tz(t[2]) if rpy is not None: if rpy[2] != 0: ets *= ETS.rz(rpy[2]) if rpy[1] != 0: ets *= ETS.ry(rpy[1]) if rpy[0] != 0: ets *= ETS.rx(rpy[0]) return ets
def test_init_elink(self): link1 = ELink(ETS.rx(), name='link1') link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2', parent=link1) link3 = ELink(ETS.tx(1), name='ee_1', parent=link2) robot = ERobot([link1, link2, link3]) self.assertEqual(robot.n, 2) self.assertIsInstance(robot[0], ELink) self.assertIsInstance(robot[1], ELink) self.assertIsInstance(robot[2], ELink) self.assertTrue(robot[0].isrevolute) self.assertTrue(robot[1].isprismatic) self.assertFalse(robot[2].isrevolute) self.assertFalse(robot[2].isprismatic) self.assertIs(robot[0].parent, None) self.assertIs(robot[1].parent, robot[0]) self.assertIs(robot[2].parent, robot[1]) self.assertEqual(robot[0].children, [robot[1]]) self.assertEqual(robot[1].children, [robot[2]]) self.assertEqual(robot[2].children, []) link1 = ELink(ETS.rx(), name='link1') link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2', parent='link1') link3 = ELink(ETS.tx(1), name='ee_1', parent='link2') robot = ERobot([link1, link2, link3]) self.assertEqual(robot.n, 2) self.assertIsInstance(robot[0], ELink) self.assertIsInstance(robot[1], ELink) self.assertIsInstance(robot[2], ELink) self.assertTrue(robot[0].isrevolute) self.assertTrue(robot[1].isprismatic) self.assertIs(robot[0].parent, None) self.assertIs(robot[1].parent, robot[0]) self.assertIs(robot[2].parent, robot[1]) self.assertEqual(robot[0].children, [robot[1]]) self.assertEqual(robot[1].children, [robot[2]]) self.assertEqual(robot[2].children, [])
def test_init_elink_autoparent(self): links = [ ELink(ETS.rx(), name='link1'), ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2'), ELink(ETS.tx(1), name='ee_1') ] robot = ERobot(links) self.assertEqual(robot.n, 2) self.assertIsInstance(robot[0], ELink) self.assertIsInstance(robot[1], ELink) self.assertIsInstance(robot[2], ELink) self.assertTrue(robot[0].isrevolute) self.assertTrue(robot[1].isprismatic) self.assertIs(robot[0].parent, None) self.assertIs(robot[1].parent, robot[0]) self.assertIs(robot[2].parent, robot[1]) self.assertEqual(robot[0].children, [robot[1]]) self.assertEqual(robot[1].children, [robot[2]]) self.assertEqual(robot[2].children, [])
def compile(self): """ Compile an ETS :return: optimised ETS :rtype: ETS Perform constant folding for faster evaluation. Consecutive constant ETs are compounded, leading to a constant ET which is denoted by ``Ci`` when displayed. Example: .. runblock:: pycon >>> from roboticstoolbox import ETS >>> robot = rtb.models.ETS.Panda() >>> ets = robot.ets() >>> ets >>> ets.compile() :seealso: :func:`isconstant` """ const = None ets = ETS() for et in self: if et.isjoint: # a joint if const is not None: # flush the constant if not iseye(const): ets *= ETS._CONST(const) const = None ets *= et # emit the joint ET else: # not a joint if const is None: const = et.T() else: const = const @ et.T() if const is not None: # flush the constant, tool transform if not iseye(const): ets *= ETS._CONST(const) return ets
def eliminate_y(self): nchanges = 0 out = ETS() jointyet = False def eliminate(prev, this): if this.isjoint or prev.isjoint: return None new = None if prev.axis == 'Rx' and this.axis == 'ty': # RX.TY -> TZ.RX new = ETS.tx(prev.eta) * prev elif prev.axis == 'Rx' and this.axis == 'tz': # RX.TZ -> TY.RX new = ETS.ty(-prev.eta) * prev elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TX-> TZ.RY new = ETS.tz(-prev.eta) * prev elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TZ-> TX.RY new = ETS.tx(prev.eta) * prev elif prev.axis == 'ty' and this.axis == 'Rx': # TY.RX -> RX.TZ new = this * ETS.tz(-this.eta) elif prev.axis == 'tx' and this.axis == 'Rz': # TX.RZ -> RZ.TY new = this * ETS.tz(this.eta) elif prev.axis == 'Ry' and this.axis == 'Rx': # RY(Q).RX -> RX.RZ(-Q) new = this * ETS.Rz(-prev.eta) elif prev.axis == 'Rx' and this.axis == 'Ry': # RX.RY -> RZ.RX new = ETS.Rz(this.eta) * prev elif prev.axis == 'Rz' and this.axis == 'Rx': # RZ.RX -> RX.RY new = this * ETS.Ry(this.eta) return new for i in range(len(self)): this = self[i] jointyet = this.isjoint if i == 0 or not jointyet: continue prev = self[i - 1] new = eliminate(prev, this) if new is not None: self[i - 1:i] = new nchanges += 1 return nchanges
def merge(self): def canmerge(prev, this): return prev.axis == this.axis and not (prev.isjoint and this.isjoint) out = ETS() while len(self.data) > 0: this = self.pop(0) if len(self.data) > 0: next = self[0] if canmerge(this, next): new = DHFactor.add(this, next) out *= new self.pop(0) # remove next from the queue print(f"Merging {this * next} to {new}") else: out *= this self.data = out.data
def add(this, that): if this.isjoint and not that.isjoint: out = ETS(this) if out.eta is None: out.eta = that.eta else: out.eta += that.eta elif not this.isjoint and that.isjoint: out = ETS(that) if out.eta is None: out.eta = this.eta else: out.eta += this.eta else: raise ValueError('both ET cannot be joint variables') return out
def __init__(self, s): et_re = re.compile(r"([RT][xyz])\(([^)]*)\)") super().__init__() # self.data = [] for axis, eta in et_re.findall(s): print(axis, eta) if eta[0] == 'q': eta = None unit = None else: # eta can be given as a variable or a number try: # first attempt to create symbolic number eta = sympy.Number(eta) except: # failing that, a symbolic variable eta = sympy.symbols(eta) if axis[0] == 'R': # convert to degrees, assumed unit in input string eta = sympy.simplify(eta * deg) if axis == 'Rx': e = ETS.rx(eta) elif axis == 'Ry': e = ETS.ry(eta) elif axis == 'Rz': e = ETS.rz(eta) elif axis == 'Tx': e = ETS.tx(eta) elif axis == 'Ty': e = ETS.ty(eta) elif axis == 'Tz': e = ETS.tz(eta) self.data.append(e.data[0])
sol = puma.ikine_LM(T) print(sol) puma.plot(sol.q, block=False) puma.ikine_a(T, config="lun") # Puma dimensions (m), see RVC2 Fig. 7.4 for details l1 = 0.672 l2 = -0.2337 l3 = 0.4318 l4 = 0.0203 l5 = 0.0837 l6 = 0.4318 e = (ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.ry() * ET.tz(l6) * ET.rz() * ET.ry() * ET.rz()) robot = ERobot(e) print(robot) panda = models.URDF.Panda() print(panda) # ## B. Trajectories traj = jtraj(puma.qz, puma.qr, 100) qplot(traj.q) t = np.arange(0, 2, 0.010) T0 = SE3(0.6, -0.5, 0.3)
# 7.4 - 2 link planar robot velocity ellipse import numpy as np import roboticstoolbox as rtb import matplotlib.pyplot as plt # Create the model p2 = rtb.models.DH.Planar2() # Assign some angles p2.q = [0.1, 0.2] # Plot and include the Velocity Ellipsoid #p2.plot(p2.q, vellipse=True) from roboticstoolbox import ETS as ets # Links a1 = 1 a2 = 1 # Angles q1 = 0 q2 = np.pi / 2 e = ets.rz(q1) * ets.tx(a1) * ets.rz(q2) * ets.tx(a2) print(e.eval([q1, q2])) e.plot([q1, q2], vellipse=True)
:seealso: :func:`ETS` """ return cls(lambda y: transl2(0, y), axis='ty', eta=eta, **kwargs) if __name__ == "__main__": print(ETS.rx(0.2)) print(ETS.rx(45, 'deg')) print(ETS.tz(0.75)) e = ETS.rx(45, 'deg') * ETS.tz(0.75) print(e) print(e.eval()) from roboticstoolbox import ETS e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1) print(e.eval([0, 0])) print(e.eval([90, -90], 'deg')) a = e.pop() print(a) from spatialmath.base import symbol theta, d = symbol('theta, d') e = ETS.rx(theta) * ETS.tx(2) * ETS.rx(45, 'deg') * ETS.ry(0.2) * ETS.ty(d) print(e) e = ETS() e *= ETS.rx() e *= ETS.tz()
def __init__(self): # Puma dimensions (m) l1 = 0.672 l2 = 0.2337 l3 = 0.4318 l4 = -0.0837 l5 = 0.4318 l6 = 0.0203 e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() * ET.tz(l3) * ET.tx(l6) * \ ET.ty(l4) * ET.ry() * ET.tz(l5) * ET.rz() * ET.ry() * ET.rz() * ET.tx(0.2) super().__init__( e, name='Puma560', manufacturer='Unimation', comment='ETS-based model') self.addconfiguration( "qz", [0, 0, 0, 0, 0, 0]) self.addconfiguration( "qbent", [0, -90, 90, 0, 0, 0], 'deg')
def substitute_to_z(self): # substitute all non Z joint transforms according to rules nchanges = 0 out = ETS() for e in self: if e.isjoint: out *= e else: # do a substitution if e.axis == 'Rx': new = ETS.ry(pi2) * ETS.rz(e.eta) * ETS.ry(-pi2) elif e.axis == 'Ry': new = ETS.rx(-pi2) * ETS.rz(e.eta) * ETS.rx(pi2) elif e.axis == 'tx': new = ETS.ry(pi2) * ETS.tz(e.eta) * ETS.ry(-pi2) elif e.axis == 'ty': new = ETS.rx(-pi2) * ETS.tz(e.eta) * ETS.rx(pi2) else: out *= e continue out *= new nchanges += 1 self.data = out.data return nchanges
print(sol) puma.plot(sol.q, block=False) puma.ikine_a(T, config="lun") # Puma dimensions (m), see RVC2 Fig. 7.4 for details l1 = 0.672 l2 = -0.2337 l3 = 0.4318 l4 = 0.0203 l5 = 0.0837 l6 = 0.4318 e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() \ * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.ry() \ * ET.tz(l6) * ET.rz() * ET.ry() * ET.rz() robot = ERobot(e) print(robot) panda = models.URDF.Panda() print(panda) # ## B. Trajectories traj = jtraj(puma.qz, puma.qr, 100) qplot(traj.q)
def __imul__(self, rest): prod = ETS() prod.data = self.data + rest.data return prod