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 tr2jac2(T):
r"""
SE(2) Jacobian matrix
:param T: SE(2) matrix
:type T: ndarray(3,3)
:return: Jacobian matrix
:rtype: ndarray(3,3)
Computes an Jacobian matrix that maps spatial velocity between two frames defined by
an SE(2) matrix.
``tr2jac2(T)`` is a Jacobian matrix (3x3) that maps spatial velocity or
differential motion from frame {B} to frame {A} where the pose of {B}
elative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> T = trot2(0.3, t=[4,5])
>>> tr2jac2(T)
:Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p65.
:SymPy: supported
"""
if not ishom2(T):
raise ValueError("expecting an SE(2) matrix")
J = np.eye(3, dtype=T.dtype)
J[:2, :2] = base.t2r(T)
return J
def __init__(self, arg=None, *, check=True):
"""
Construct new SO(3) object
:rtype: SO3 instance
There are multiple call signatures:
- ``SO3()`` is an ``SO3`` instance with one value -- a 3x3 identity
matrix which corresponds to a null rotation
- ``SO3(R)`` is an ``SO3`` instance with with the value ``R`` which is a
3x3 numpy array representing an SO(3) rotation matrix. If ``check``
is ``True`` check the matrix belongs to SO(3).
- ``SO3([R1, R2, ... RN])`` is an ``SO3`` instance wwith ``N`` values
given by the elements ``Ri`` each of which is a 3x3 NumPy array
representing an SO(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SO(3).
- ``SO3([X1, X2, ... XN])`` is an ``SO3`` instance with ``N`` values
given by the elements ``Xi`` each of which is an SO3 instance.
:SymPy: supported
"""
super().__init__()
if isinstance(arg, SE3):
self.data = [base.t2r(x) for x in arg.data]
elif not super().arghandler(arg, check=check):
raise ValueError('bad argument to constructor')
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 __init__(self, arg=None, *, unit='rad', check=True):
"""
Construct new SO(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:param check: check for valid SO(2) elements if applicable, default to True
:type check: bool
:return: SO(2) rotation
:rtype: SO2 instance
- ``SO2()`` is an SO2 instance representing a null rotation -- the identity matrix.
- ``SO2(θ)`` is an SO2 instance representing a rotation by ``θ`` radians. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(θ, unit='deg')`` is an SO2 instance representing a rotation by ``θ`` degrees. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(R)`` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. If ``check``
is ``True`` check the matrix belongs to SO(2).
- ``SO2([R1, R2, ... RN])`` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix
Ri which is a 2x2 numpy array. If ``check`` is ``True`` then check each matrix belongs to SO(2).
- ``SO2([X1, X2, ... XN])`` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.
"""
super().__init__()
if isinstance(arg, SE2):
self.data = [base.t2r(x) for x in arg.data]
elif super().arghandler(arg, check=check):
return
elif argcheck.isscalar(arg):
self.data = [base.rot2(arg, unit=unit)]
elif argcheck.isvector(arg):
self.data = [base.rot2(x, unit=unit) for x in argcheck.getvector(arg)]
else:
raise ValueError('bad argument to constructor')
def __init__(self, s=None, v=None, norm=True, check=True):
"""
Construct a UnitQuaternion object
:arg norm: explicitly normalize the quaternion [default True]
:type norm: bool
:arg check: explicitly check dimension of passed lists [default True]
:type check: bool
:return: new unit uaternion
:rtype: UnitQuaternion
:raises: ValueError
Single element quaternion:
- ``UnitQuaternion()`` constructs the identity quaternion 1<0,0,0>
- ``UnitQuaternion(s, v)`` constructs a unit quaternion with specified
real ``s`` and ``v`` vector parts. ``v`` is a 3-vector given as a
list, tuple, numpy.ndarray
- ``UnitQuaternion(v)`` constructs a unit quaternion with specified
elements from ``v`` which is a 4-vector given as a list, tuple, numpy.ndarray
- ``UnitQuaternion(R)`` constructs a unit quaternion from an orthonormal
rotation matrix given as a 3x3 numpy.ndarray. If ``check`` is True
test the matrix for orthogonality.
Multi-element quaternion:
- ``UnitQuaternion(V)`` constructs a unit quaternion list with specified
elements from ``V`` which is an Nx4 numpy.ndarray, each row is a
quaternion. If ``norm`` is True explicitly normalize each row.
- ``UnitQuaternion(L)`` constructs a unit quaternion list from a list
of 4-element numpy.ndarrays. If ``check`` is True test each element
of the list is a 4-vector. If ``norm`` is True explicitly normalize
each vector.
"""
if s is None and v is None:
self.data = [quat.eye()]
elif argcheck.isscalar(s) and argcheck.isvector(v, 3):
q = np.r_[s, argcheck.getvector(v)]
if norm:
q = quat.unit(q)
self.data = [q]
elif argcheck.isvector(s, 4):
#print('uq constructor 4vec')
q = argcheck.getvector(s)
# if norm:
# q = quat.unit(q)
# print(q)
self.data = [quat.unit(s)]
elif isinstance(s, list):
if isinstance(s[0], np.ndarray):
if check:
assert argcheck.isvectorlist(
s, 4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s[0], p3d.SO3):
self.data = [quat.r2q(x.R) for x in s]
elif isinstance(s[0], self.__class__):
# possibly a list of objects of same type
assert all(map(lambda x: isinstance(x, type(self)),
s)), 'all elements of list must have same type'
self.data = [x._A for x in s]
else:
raise ValueError('incorrect list')
elif isinstance(s, p3d.SO3):
self.data = [quat.r2q(s.R)]
elif isinstance(s, np.ndarray) and tr.isrot(s, check=check):
self.data = [quat.r2q(s)]
elif isinstance(s, np.ndarray) and tr.ishom(s, check=check):
self.data = [quat.r2q(tr.t2r(s))]
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
if norm:
self.data = [quat.qnorm(x) for x in s]
else:
self.data = [x for x in s]
elif isinstance(s, UnitQuaternion):
self.data = s.data
else:
raise ValueError('bad argument to UnitQuaternion constructor')
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
