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)
