def angle(q1, q2):
"""
Angle between two unit-quaternions
:arg q0: unit-quaternion as a 4-vector
:type q0: array_like
:arg q1: unit-quaternion as a 4-vector
:type q1: array_like
:return: angle between the rotations [radians]
:rtype: float
If each of the input quaternions is considered a rotated coordinate
frame, then the angle is the smallest rotation required about a fixed
axis, to rotate the first frame into the second.
References: Metrics for 3D rotations: comparison and analysis,
Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2.
.. warning:: There is no check that the passed values are unit-quaternions.
"""
# TODO different methods
q1 = argcheck.getvector(q1, 4)
q2 = argcheck.getvector(q2, 4)
return 2.0 * math.atan2(tr.norm(q1 - q2), tr.norm(q1 + q2))
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 omega(cls, w):
assert argcheck.isvector(w, 3), 'w must be a 3-vector'
w = argcheck.getvector(w)
theta = tr.norm(w)
s = math.cos(theta / 2)
v = math.sin(theta / 2) * tr.unitvec(w)
return cls(s=s, v=v)
def control(v, t, x):
goal = (6, 6)
goal_heading = atan2(goal[1] - x[1], goal[0] - x[0])
d_heading = base.angdiff(goal_heading, x[2])
v.stopif(base.norm(x[0:2] - goal) < 0.1)
return (1, d_heading)
def iseye(S, tol=10):
"""
Test if matrix is identity
:param S: matrix to test
:type S: ndarray(n,n)
:param tol: tolerance in units of eps
:type tol: float
:return: whether matrix is a proper skew-symmetric matrix
:rtype: bool
Check if matrix is an identity matrix. We test that the trace tom row is zero
We check that the norm of the residual is less than ``tol * eps``.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> iseye(np.array([[1,0], [0,1]]))
>>> iseye(np.array([[1,2], [0,1]]))
:seealso: isskew, isskewa
"""
s = S.shape
if len(s) != 2 or s[0] != s[1]:
return False # not a square matrix
return base.norm(S - np.eye(s[0])) < tol * _eps
def EulerVec(cls, w):
r"""
Construct a new SO(3) rotation matrix from an Euler rotation vector
:param ω: rotation axis
:type ω: 3-element array_like
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.EulerVec(ω)`` is a unit quaternion that describes the 3D rotation
defined by a rotation of :math:`\theta = \lVert \omega \rVert` about the
unit 3-vector :math:`\omega / \lVert \omega \rVert`.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.EulerVec([0.5,0,0])
.. note:: :math:`\theta \eq 0` the result in an identity matrix, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`~spatialmath.base.transforms3d.angvec2r`
"""
assert base.isvector(w, 3), 'w must be a 3-vector'
w = base.getvector(w)
theta = base.norm(w)
return cls(base.angvec2r(theta, w), check=False)
def theta(self):
"""
Twist rotation
:return: rotation about the twist axis
:rtype: float
- ``X.theta`` is the rotation about the twist axis in units of radians.
"""
return base.norm(self.w)
def theta(self):
"""
Twist angle (superclass method)
:return: magnitude of rotation (1x1) about the twist axis in radians
:rtype: float
"""
if self.N == 2:
return abs(self.w)
else:
return base.norm(np.array(self.w))
def theta(self):
"""
Twist rotation
:return: rotation about the twist axis
:rtype: float
``X.theta`` is the rotation about the twist axis in units of radians.
If we consider the twist as a screw, this is the rotation about the
screw axis to achieve the rigid-body motion.
"""
return base.norm(self.w)
def unit(self):
"""
Unit twist
TW.unit() is a Twist object representing a unit aligned with the Twist
TW.
"""
if base.iszerovec(self.w):
# rotational twist
return Twist2(self.S / base.norm(S.w))
else:
# prismatic twist
return Twist2(base.unitvec(self.v), [0, 0, 0])
def angle(q1, q2):
"""
Angle between two unit-quaternions
:arg q0: unit-quaternion
:type q0: array_like(4)
:arg q1: unit-quaternion
:type q1: array_like(4)
:return: angle between the rotations [radians]
:rtype: float
If each of the input quaternions is considered a rotated coordinate
frame, then the angle is the smallest rotation required about a fixed
axis, to rotate the first frame into the second.
.. runblock:: pycon
>>> from spatialmath.base import angle
>>> from math import sqrt
>>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis
>>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0] # 90deg rotation about y-axis
>>> angle(q1, q2)
:References:
- Metrics for 3D rotations: comparison and analysis,
Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2.
.. warning:: There is no check that the passed values are unit-quaternions.
"""
# TODO different methods
q1 = base.getvector(q1, 4)
q2 = base.getvector(q2, 4)
return 2.0 * math.atan2(base.norm(q1 - q2), base.norm(q1 + q2))
def unit(self):
"""
Unitize twist (superclass property)
:return: a unit twist
:rtype: Twist3 or Twist2
``twist.unit()`` is a Twist object representing a unit aligned with the
Twist ``twist``.
"""
if base.iszerovec(self.w):
# rotational twist
return Twist3(self.S / base.norm(S.w))
else:
# prismatic twist
return Twist3(base.unitvec(self.v), [0, 0, 0])
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 theta(self):
"""
Twist rotation
:return: rotation about the twist axis
:rtype: float
``X.theta`` is the rotation about the twist axis in units of radians.
If we consider the twist as a screw, this is the rotation about the
screw axis to achieve the rigid-body motion.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3, Twist3
>>> T = SE3(1, 2, 3) * SE3.Rx(0.3)
>>> S = Twist3(T)
>>> S.theta()
"""
return base.norm(self.w)
def Hp(self, xv, jf):
"""
Jacobian dh/dp
%
# J = self.Hp(X, K) is the Jacobian dh/dp (2x2) at the vehicle
# state X (3x1) for map landmark K.
%
# J = self.Hp(X, P) as above but for a landmark at coordinate P (1x2).
%
# See also RangeBearingSensor.h.
"""
if base.isinteger(jf):
xf = self.map.landmark(jf)
else:
xf = jf
Delta = xf - xv[0:2]
r = base.norm(Delta)
return np.array([
[Delta([0]) / r, Delta([1]) / r],
[-Delta([1]) / r**2, Delta([0]) / r**2],
])
def Hx(self, xv, jf):
"""
Jacobian dh/dx
%
# J = self.Hx(X, K) returns the Jacobian dh/dx (2x3) at the vehicle
# state X (3x1) for map landmark K.
%
# J = self.Hx(X, P) as above but for a landmark at coordinate P.
%
# See also RangeBearingSensor.h.
"""
if base.isinteger(jf):
# landmark index provided
xf = self.sensor.landmark(jf)
else:
# assume it is a coordinate
xf = base.getvector(jf, 2)
Delta = xf - xv[0:2]
r = base.norm(Delta)
return np.array([
[-Delta[0] / r, -Delta[1] / r, 0],
[Delta[1] / r**2, -Delta[0] / r**2, -1],
])
def angdist(self, other, metric=6):
r"""
Angular distance metric between rotations
:param other: second rotation
:type other: SO3 instance
:param metric: metric, default is 6
:type metric: int
:raises TypeError: if other is not an SO3
:return: angle in radians
:rtype: float or ndarray
``R1.angdist(R2)`` is the geodesic norm, or geodesic distance between two
rotations.
Several metrics are supported, the first 5 are computed after conversion
to unit quaternions.
====== ===============================================================
Metric Details
====== ===============================================================
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]`
6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]`
====== ===============================================================
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion
>>> R1 = SO3.Rx(0.3)
>>> R2 = SO3.Ry(0.3)
>>> print(R1.angdist(R1))
>>> print(R1.angdist(R2))
.. note::
- metrics 1, 2, 4 can throw ValueError "math domain error" due to
numeric errors which push the argument of ``acos()`` marginally
outside its domain [0, 1].
- metrics 2 and 3 are equivalent, but 3 is more robust
:seealso: :func:`UnitQuaternion.angdist`
"""
if metric < 5:
from spatialmath.quaternion import UnitQuaternion
return UnitQuaternion(self).angdist(UnitQuaternion(other),
metric=metric)
elif metric == 5:
op = lambda R1, R2: np.linalg.norm(np.eye(3) - R1 @ R2.T)
elif metric == 6:
op = lambda R1, R2: base.norm(base.trlog(R1 @ R2.T, twist=True))
else:
raise ValueError('unknown metric')
ad = self._op2(other, op)
if isinstance(ad, list):
return np.array(ad)
else:
return ad
def ikine_LM(self,
T,
q0=None,
mask=None,
ilimit=500,
rlimit=100,
tol=1e-10,
L=0.1,
Lmin=0,
search=False,
slimit=100,
transpose=None):
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization
(Robot superclass)
:param T: The desired end-effector pose or pose trajectory
:type T: SE3
:param q0: initial joint configuration (default all zeros)
:type q0: ndarray(n)
:param mask: mask vector that correspond to translation in X, Y and Z
and rotation about X, Y and Z respectively.
:type mask: ndarray(6)
:param ilimit: maximum number of iterations (default 500)
:type ilimit: int
:param rlimit: maximum number of consecutive step rejections
(default 100)
:type rlimit: int
:param tol: final error tolerance (default 1e-10)
:type tol: float
:param L: initial value of lambda
:type L: float (default 0.1)
:param Lmin: minimum allowable value of lambda
:type Lmin: float (default 0)
:param search: search over all configurations
:type search: bool
:param slimit: maximum number of search attempts
:type slimit: int (default 100)
:param transpose: use Jacobian transpose with step size A, rather
than Levenberg-Marquadt
:type transpose: float
:return: inverse kinematic solution
:rtype: named tuple
``sol = robot.ikine_LM(T)`` are the joint coordinates (n) corresponding
to the robot end-effector pose ``T`` which is an ``SE3`` object. This
method can be used for robots with any number of degrees of freedom.
The return value ``sol`` is a named tuple with elements:
============ ========== ===============================================
Element Type Description
============ ========== ===============================================
``q`` ndarray(n) joint coordinates in units of radians or metres
``success`` bool whether a solution was found
``reason`` str reason for the failure
``iterations`` int number of iterations
``residual`` float final value of cost function
============ ========== ===============================================
If ``success=False`` the ``q`` values will be valid numbers, but the
solution will be in error. The amount of error is indicated by
the ``residual``.
**Trajectory operation**:
If ``len(T) = m > 1`` it is considered to be a trajectory, then the
result is a named tuple whose elements are
============ ============ ===============================================
Element Type Description
============ ============ ===============================================
``q`` ndarray(m,n) joint coordinates in units of radians or metres
``success`` bool(m) whether a solution was found
``reason`` list of str reason for the failure
``iterations`` ndarray(m) number of iterations
``residual`` ndarray(m) final value of cost function
============ ============ ===============================================
The initial estimate of q for each time step is taken as the solution
from the previous time step.
**Underactuated robots:**
For the case where the manipulator has fewer than 6 DOF the
solution space has more dimensions than can be spanned by the
manipulator joint coordinates.
In this case we specify the ``mask`` option where the ``mask`` vector
(6) specifies the Cartesian DOF (in the wrist coordinate frame) that
will be ignored in reaching a solution. The mask vector has six
elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value should be 0 (for ignore)
or 1. The number of non-zero elements must equal the number of
manipulator DOF.
For example when using a 3 DOF manipulator tool orientation might
be unimportant, in which case use the option ``mask=[1 1 1 0 0 0]``.
**Global search**:
``sol = robot.ikine_LM(T, search=True)`` as above but peforms a
brute-force search with initial conditions chosen randomly from the
entire configuration space. If a numerical solution is found from that
initial condition, it is returned, otherwise another initial condition
is chosen.
.. note::
- See `Toolbox kinematics wiki page
<https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
- Implements a Levenberg-Marquadt variable-damping solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
- The default value of ``q0`` is zero which is a poor choice for
most manipulators since it often corresponds to a
kinematic singularity.
- Such a solution is completely general, though much less
efficient than analytic inverse kinematic solutions derived
symbolically.
- This approach allows a solution to be obtained at a singularity,
but the joint angles within the null space are arbitrarily
assigned.
- Joint offsets, if defined, are accounted for in the solution.
- Joint limits are not considered in this solution.
- If the search option is used any prismatic joint must have
joint limits defined.
:references:
- Robotics, Vision & Control, P. Corke, Springer 2011,
Section 8.4.
:seealso: :func:`ikine_LMS`, :func:`ikine_unc`, :func:`ikine_con`,
:func:`ikine_min`
""" # noqa E501
if not isinstance(T, SE3):
T = SE3(T)
solutions = []
if search:
# Randomised search for a starting point
# quiet = True
qlim = self.qlim
qspan = qlim[1] - qlim[0] # range of joint motion
for k in range(slimit):
# choose a random joint coordinate
q0_k = np.random.rand(self.n) * qspan + qlim[0, :]
print('search starts at ', q0_k)
# recurse into the solver
solution = self.ikine_LM(T[0], q0_k, mask, ilimit, rlimit, tol,
L, Lmin, False, slimit, transpose)
if solution.success:
q0 = solution.q
if len(T) == 1:
# we're done
return solution
else:
# more to do on the trajectory
solutions.append(solution)
del T[0]
# no solution found, stop now
return iksol(None, False, None, None, None)
if q0 is None:
q0 = np.zeros((self.n, ))
else:
q0 = base.getvector(q0, self.n)
if mask is not None:
mask = base.getvector(mask, 6)
if not self.n >= np.sum(mask):
raise ValueError('Number of robot DOF must be >= the number '
'of 1s in the mask matrix')
else:
mask = np.ones(6)
W = np.diag(mask)
tcount = 0 # Total iteration count
rejcount = 0 # Rejected step count
nm = 0
# bool vector indicating revolute joints
# revolutes = np.array([link.isrevolute for link in self])
revolutes = []
# This is a mixin class inherited by robot classes
# Therefore it will never be used on its own and the
# LGTM issue can be ignored
for link in self: # lgtm [py/non-iterable-in-for-loop]
if isinstance(self, rtb.DHRobot) or link.isjoint:
revolutes.append(link.isrevolute)
revolutes = np.array(revolutes)
q = q0
for Tk in T:
iterations = 0
Li = L # lambda
failure = None
while True:
# Update the count and test against iteration limit
iterations += 1
if iterations > ilimit:
failure = f"iteration limit {ilimit} exceeded"
break
e = base.tr2delta(self.fkine(q).A, Tk.A)
# Are we there yet?
if base.norm(W @ e) < tol:
break
# Compute the Jacobian
J = self.jacobe(q)
JtJ = J.T @ W @ J
if transpose is not None:
# Do the simple Jacobian transpose with constant gain
dq = transpose * J.T @ e # lgtm [py/multiple-definition]
q += dq
else:
# Do the damped inverse Gauss-Newton with
# Levenberg-Marquadt
# dq = np.linalg.inv(
# JtJ + ((Li + Lmin) * np.eye(self.n))
# ) @ J.T @ W @ e
dq = np.linalg.inv(JtJ + (
(Li + Lmin) * np.diag(np.diag(JtJ)))) @ J.T @ W @ e
# print(J.T @ W @ e)
# Compute possible new value of
qnew = q + dq
# And figure out the new error
enew = base.tr2delta(self.fkine(qnew).A, Tk.A)
# Was it a good update?
if np.linalg.norm(W @ enew) < np.linalg.norm(W @ e):
# Step is accepted
q = qnew
e = enew
Li /= 2
rejcount = 0
else:
# Step is rejected, increase the damping and retry
Li *= 2
rejcount += 1
if rejcount > rlimit:
failure = f"rejected-step limit {rlimit} exceeded"
break
# Wrap angles for revolute joints
k = np.logical_and(q > np.pi, revolutes)
q[k] -= 2 * np.pi
k = np.logical_and(q < -np.pi, revolutes)
q[k] += +2 * np.pi
nm = np.linalg.norm(W @ e)
# qs = ", ".join(["{:8.3f}".format(qi) for qi in q])
# print(f"λ={Li:8.2g}, |e|={nm:8.2g}: q={qs}")
# LM process finished, for better or worse
# failure will be None or an error message
solution = iksol(q, failure is None, failure, iterations, nm)
solutions.append(solution)
tcount += iterations
if len(T) == 1:
return solutions[0]
else:
return iksol(np.vstack([sol.q for sol in solutions]),
np.array([sol.success for sol in solutions]),
[sol.reason for sol in solutions],
np.array([sol.iterations for sol in solutions]),
np.array([sol.residual for sol in solutions]))
def Omega(cls, w):
return cls(quat.r2q(tr.angvec2r(tr.norm(w), tr.unitvec(w))),
check=False)
def next(self, position):
l = None
y = None
if all(self._goal == position):
return None # we have arrived
if self._step == 1:
# Step 1. Move along the M-line toward the goal
self.message(f"{position}: moving along the M-line (step 1)")
# motion on line toward goal
d = self._goal - position
if abs(d[0]) > abs(d[1]):
# line slope less than 45 deg
dx = 1 if d[0] >= 0 else -1 # np.sign(d[0])
l = self._m_line
y = -((position[0] + dx) * l[0] + l[2]) / l[1]
dy = int(round(y - position[1]))
else:
# line slope greater than 45 deg
dy = 1 if d[1] >= 0 else -1 # np.sign(d[1])
l = self._m_line
x = -((position[1] + dy) * l[1] + l[2]) / l[0]
dx = int(round(x - position[0]))
# detect if next step is an obstacle
if self.is_occupied(position + np.r_[dx, dy]):
self.message(f" {position}: obstacle at {position + np.r_[dx, dy]}")
self.h.append(position)
self._step = 2 # transition to step 2
self.message(f" {position}: change to step 2")
# get a list of all the points around the obstacle
self._edge, _ = edgelist(self._occ_grid_nav == 0, position)
self._k = 1
else:
n = position + np.array([dx, dy])
if self._step == 2:
# Step 2. Move around the obstacle until we reach a point
# on the M-line closer than when we started.
self.message(f"{position}: moving around the obstacle (step 2)")
if self._k <= len(self._edge):
n = self._edge[self._k] # next edge point
else:
# we are at the end of the list of edge points, we
# are back where we started. Step 2.c test.
raise RuntimeError('robot is trapped')
# are we on the M-line now ?
if abs(np.inner(np.r_[position, 1], self._m_line)) <= 0.5:
self.message(f" {position}: crossed the M-line")
# are we closer than when we encountered the obstacle?
if base.norm(position - self._goal) < base.norm(self._h[-1] - self._goal):
# self._j += 1
self._step = 1 # transition to step 1
self.message(f" {position}: change to step 1")
return n
# no, keep going around
self._k += 1
return n
