def delta(self, X2):
r"""
Infinitesimal difference of SE(3) values
:return: differential motion vector
:rtype: numpy.ndarray, shape=(6,)
``X1.delta(X2)`` is the differential motion (6x1) corresponding to
infinitesimal motion (in the ``X1`` frame) from pose ``X1`` to ``X2``.
The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]`
represents infinitesimal translation and rotation.
Example::
>>> x1 = SE3.Rx(0.3)
>>> x2 = SE3.Rx(0.3001)
>>> x1.delta(x2)
array([0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 9.99999998e-05,
0.00000000e+00, 0.00000000e+00])
.. note::
- the displacement is only an approximation to the motion, and assumes
that ``X1`` ~ ``X2``.
- can be considered as an approximation to the effect of spatial velocity over a
a time interval, ie. the average spatial velocity multiplied by time.
:Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
:seealso: :func:`~spatialmath.base.transforms3d.tr2delta`
"""
return base.tr2delta(self.A, X2.A)
def delta(self, X2):
r"""
Infinitesimal difference of SE(3)
:param X2:
:type X2: SE3
:return: differential motion vector
:rtype: numpy.ndarray, shape=(6,)
- ``X1.delta(X2)`` is the differential motion (6x1) corresponding to
infinitesimal motion (in the X1 frame) from pose X1 to X2.
The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`
represents infinitesimal translation and rotation.
Notes:
- the displacement is only an approximation to the motion T, and assumes
that X1 ~ X2.
- Can be considered as an approximation to the effect of spatial velocity over a
a time interval, average spatial velocity multiplied by time.
Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
:seealso: :func:`~spatialmath.base.transform3d.tr2delta`
"""
return tr.tr2delta(self.A, X2.A)
def Delta(cls, d):
r"""
Create SE(3) from differential motion
:param d: differential motion
:type d: 6-element array_like
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
- ``T = delta2tr(d)`` is an SE(3) representing differential
motion :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`.
Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
:seealso: :func:`~delta`, :func:`~spatialmath.base.transform3d.delta2tr`
"""
return tr.tr2delta(d)
def output(self, t=None):
return [base.tr2delta(self.inputs[0].A, self.inputs[1].A)]
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, or ``None``
``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
============ ========== ============================================================
**Trajectory operation**:
If ``len(T) > 1`` it is considered to be a trajectory, and the result
is a list of named tuples such that ``sol[k]`` corresponds to ``T[k]``.
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::
- Implements a Levenberg-Marquadt variable-step-size 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
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, :]
# 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]
else:
# no solution found, stop now
return iksol()
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])
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 np.linalg.norm(W @ e) < tol:
# print(iterations)
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]
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
# 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 solutions
