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 __str__(self): """ Pretty string representation of 3D twist :return: readable representation of the twist :rtype: str Convert the twist's value to an array of numbers. Example:: >>> x = Twist3.R([1,2,3], [4,5,6]) >>> print(x) (0.80178 -1.6036 0.80178; 0.26726 0.53452 0.80178) """ return '\n'.join([ "({:.5g} {:.5g} {:.5g}; {:.5g} {:.5g} {:.5g})".format( *list(base.removesmall(tw.S))) for tw in self ])
def __str__(self): """ Convert to a string :return: String representation of line parameters :rtype: str ``str(line)`` is a string showing Plucker parameters in a compact single line format like:: { 0 0 0; -1 -2 -3} where the first three numbers are the moment, and the last three are the direction vector. """ return '\n'.join([ '{{ {:.5g} {:.5g} {:.5g}; {:.5g} {:.5g} {:.5g}}}'.format( *list(base.removesmall(x.vec))) for x in self ])
def __str__(self): """ Pretty string representation of 3D twist :return: readable representation of the twist :rtype: str Convert the twist's value to an array of numbers. Example: .. runblock: pycon >>> from spatialmath import Twist3 >>> x = Twist3.R([1,2,3], [4,5,6]) >>> print(x) """ return '\n'.join([ "({:.5g} {:.5g} {:.5g}; {:.5g} {:.5g} {:.5g})".format( *list(base.removesmall(tw.S))) for tw in self ])
def rne_dh(self, Q, QD=None, QDD=None, grav=None, fext=None, debug=False, basewrench=False): """ Compute inverse dynamics via recursive Newton-Euler formulation :param Q: Joint coordinates :param QD: Joint velocity :param QDD: Joint acceleration :param grav: [description], defaults to None :type grav: [type], optional :param fext: end-effector wrench, defaults to None :type fext: 6-element array-like, optional :param debug: print debug information to console, defaults to False :type debug: bool, optional :param basewrench: compute the base wrench, defaults to False :type basewrench: bool, optional :raises ValueError: for misshaped inputs :return: Joint force/torques :rtype: NumPy array Recursive Newton-Euler for standard Denavit-Hartenberg notation. - ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is the number of robot joints. The result has shape (n,). - ``rne_dh(q, qd, qdd)`` where the arguments have shape (m,n) where n is the number of robot joints and where m is the number of steps in the joint trajectory. The result has shape (m,n). - ``rne_dh(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] with shape (3n,), and the result has shape (n,). - ``rne_dh(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] with shape (m,3n) and the result has shape (m,n). .. notes:: - this is a pure Python implementation and slower than the .rne() which is written in C. - this version supports symbolic model parameters - verified against MATLAB code """ def removesmall(x): return x n = self.n if self.symbolic: dtype = 'O' else: dtype = None z0 = np.array([0, 0, 1], dtype=dtype) if grav is None: grav = self.gravity # default gravity from the object else: grav = getvector(grav, 3) if fext is None: fext = np.zeros((6, ), dtype=dtype) else: fext = getvector(fext, 6) if debug: print('grav', grav) print('fext', fext) # unpack the joint coordinates and derivatives if Q is not None and QD is None and QDD is None: # single argument case Q = getmatrix(Q, (None, self.n * 3)) q = Q[:, 0:n] qd = Q[:, n:2 * n] qdd = Q[:, 2 * n:] else: # 3 argument case q = getmatrix(Q, (None, self.n)) qd = getmatrix(QD, (None, self.n)) qdd = getmatrix(QDD, (None, self.n)) nk = q.shape[0] tau = np.zeros((nk, n), dtype=dtype) if basewrench: wbase = np.zeros((nk, n), dtype=dtype) for k in range(nk): # take the k'th row of data q_k = q[k, :] qd_k = qd[k, :] qdd_k = qdd[k, :] if debug: print('q_k', q_k) print('qd_k', qd_k) print('qdd_k', qdd_k) print() # joint vector quantities stored columwise in matrix # m suffix for matrix Fm = np.zeros((3, n), dtype=dtype) Nm = np.zeros((3, n), dtype=dtype) # if robot.issym # pstarm = sym([]); # else # pstarm = []; pstarm = np.zeros((3, n), dtype=dtype) Rm = [] # rotate base velocity and acceleration into L1 frame Rb = t2r(self.base.A).T w = Rb @ np.zeros( (3, ), dtype=dtype) # base has zero angular velocity wd = Rb @ np.zeros( (3, ), dtype=dtype) # base has zero angular acceleration vd = Rb @ grav # ---------------- initialize some variables -------------------- # for j in range(n): link = self.links[j] # compute the link rotation matrix if link.sigma == 0: # revolute axis Tj = link.A(q_k[j]).A d = link.d else: # prismatic Tj = link.A(link.theta).A d = q_k[j] Rm.append(t2r(Tj)) # compute pstar: # O_{j-1} to O_j in {j}, negative inverse of link xform alpha = link.alpha pstarm[:, j] = np.r_[link.a, d * sym.sin(alpha), d * sym.cos(alpha)] # ----------------- the forward recursion ----------------------- # for j, link in enumerate(self.links): Rt = Rm[j].T # transpose!! pstar = pstarm[:, j] r = link.r # statement order is important here if link.isrevolute() == 0: # revolute axis wd = Rt @ (wd + z0 * qdd_k[j] \ + _cross(w, z0 * qd_k[j])) w = Rt @ (w + z0 * qd_k[j]) vd = _cross(wd, pstar) \ + _cross(w, _cross(w, pstar)) \ + Rt @ vd else: # prismatic axis w = Rt @ w wd = Rt @ wd vd = Rt @ (z0 * qdd_k[j] + vd) \ + _cross(wd, pstar) \ + 2 * _cross(w, Rt @ z0 * qd_k[j]) \ + _cross(w, _cross(w, pstar)) vhat = _cross(wd, r) \ + _cross(w, _cross(w, r)) \ + vd Fm[:, j] = link.m * vhat Nm[:, j] = link.I @ wd + _cross(w, link.I @ w) if debug: print('w: ', removesmall(w)) print('wd: ', removesmall(wd)) print('vd: ', removesmall(vd)) print('vdbar: ', removesmall(vhat)) print() if debug: print('Fm\n', Fm) print('Nm\n', Nm) # ----------------- the backward recursion ----------------------- # f = fext[:3] # force/moments on end of arm nn = fext[3:] for j in range(n - 1, -1, -1): link = self.links[j] pstar = pstarm[:, j] # # order of these statements is important, since both # nn and f are functions of previous f. # if j == (n - 1): R = np.eye(3, dtype=dtype) else: R = Rm[j + 1] r = link.r nn = R @ (nn + _cross(R.T @ pstar, f)) \ + _cross(pstar + r, Fm[:,j]) \ + Nm[:,j] f = R @ f + Fm[:, j] if debug: print('f: ', removesmall(f)) print('n: ', removesmall(nn)) R = Rm[j] if link.isrevolute(): # revolute axis t = nn @ (R.T @ z0) else: # prismatic t = f @ (R.T @ z0) # add joint inertia and friction # no Coulomb friction if model is symbolic tau[k, j] = t \ + link.G ** 2 * link.Jm * qdd_k[j] \ - link.friction(qd_k[j], coulomb=not self.symbolic) if debug: print( f'j={j:}, G={link.G:}, Jm={link.Jm:}, friction={link.friction(qd_k[j], coulomb=False):}' ) print() # compute the base wrench and save it if basewrench: R = Rm[0] nn = R @ nn f = R @ f wbase[k, :] = np.r_[f, nn] if self.symbolic: # simplify symbolic expressions print('start simplification') t0 = time() from sympy import trigsimp tau = trigsimp(tau) # consider using multiprocessing to spread over cores # https://stackoverflow.com/questions/33844085/using-multiprocessing-with-sympy print(f'done simplification after {time() - t0:} sec') if tau.shape[0] == 1: return tau.flatten() else: return tau if tau.shape[0] == 1: return tau #.flatten() else: return tau
def trim(x): if x.dtype == 'O': return x else: return tr.removesmall(x)