def payload(self, m, p=np.zeros(3)):
"""
payload(m, p) adds payload mass adds a payload with point mass m at
position p in the end-effector coordinate frame.
payload(m) adds payload mass adds a payload with point mass m at
in the end-effector coordinate frame.
payload(0) removes added payload.
:param m: mass (kg)
:type m: float
:param p: position in end-effector frame
:type p: ndarray(3,1)
"""
p = getvector(p, 3, out='col')
lastlink = self.links[self.n - 1]
lastlink.m = m
lastlink.r = p
def Revolute(cls, q):
"""
Construct a new 2D revolute Twist object
:param q: Point on the line of action
:type q: array_like(2)
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2.Revolute(q)`` is a 2D Twist object representing rotation about the 2D point ``q``.
Example:
.. runblock:: pycon
>>> from spatialmath import Twist2
>>> Twist2.Revolute([0, 1])
"""
q = base.getvector(q, 2)
v = -np.cross(np.r_[0.0, 0.0, 1.0], np.r_[q, 0.0])
return cls(v[:2], 1)
def blackbody(lam, T):
"""
Compute blackbody emission spectrum
:param lam: wavelength 𝜆 [m]
:type lam: float or array_like
:param T: blackbody temperature [K]
:type T: float
``blackbody(𝜆, T)`` is the blackbody radiation power density [W/m^3]
at the wavelength 𝜆 [m] and temperature T [K].
If 𝜆 is a vector (N,), then the result is a vector (N,) of
blackbody radiation power density at the corresponding elements of 𝜆.
Example::
l = np.linspace(380, 700, 10) * 1e-9 # visible spectrum
e = blackbody(l, 6500) # emission of sun
plt.plot(l, e)
:references:
- Robotics, Vision & Control, Section 10.1,
P. Corke, Springer 2011.
"""
# physical constants
c = 2.99792458e8 # m/s (speed of light)
h = 6.626068e-34 # m2 kg / s (Planck's constant)
k = 1.3806503e-23 # J K-1 (Boltzmann's constant)
lam = base.getvector(lam)
e = 2.0 * h * c**2 / (lam**5 * (np.exp(h * c / k / T / lam) - 1))
if len(e) == 1:
return e[0]
else:
return e
def eval(self, q):
"""
Evaluate an ETS with joint coordinate substitution
:param q: joint coordinates
:type q: array-like
:return: product of transformations
:rtype: SE3
Effectively the forward-kinematics of the ET sequence. Compounds the
transforms left to right, and substitutes in joint coordinates as
needed from consecutive elements of ``q``.
"""
T = SE3()
q = getvector(q, out='sequence')
for e in self:
if e.jtype == self.VARIABLE:
T *= e.T(q.pop(0))
else:
T *= e.T()
return T
def unittwist(S, tol=10):
"""
Convert twist to unit twist
:param S: twist vector
:type S: array_like(6)
:param tol: tolerance in units of eps
:type tol: float
:return: unit twist
:rtype: ndarray(6)
A unit twist is a twist where:
- the rotation part has unit magnitude
- if the rotational part is zero, then the translational part has unit magnitude
.. runblock:: pycon
>>> from spatialmath.base import *
>>> unittwist([2, 4, 6, 2, 0, 0])
>>> unittwist([2, 0, 0, 0, 0, 0])
Returns None if the twist has zero magnitude
"""
S = getvector(S, 6)
if iszerovec(S, tol=tol):
return None
v = S[0:3]
w = S[3:6]
if iszerovec(w):
th = norm(v)
else:
th = norm(w)
return S / th
def rodrigues(w, theta=None):
r"""
Rodrigues' formula for rotation
:param w: rotation vector
:type w: array_like(3) or array_like(1)
:param θ: rotation angle
:type θ: float or None
:return: SO(n) matrix
:rtype: ndarray(2,2) or ndarray(3,3)
Compute Rodrigues' formula for a rotation matrix given a rotation axis
and angle.
.. math::
\mat{R} = \mat{I}_{3 \times 3} + \sin \theta \skx{\hat{\vec{v}}} + (1 - \cos \theta) \skx{\hat{\vec{v}}}^2
.. runblock:: pycon
>>> from spatialmath.base import *
>>> rodrigues([1, 0, 0], 0.3)
>>> rodrigues([0.3, 0, 0])
>>> rodrigues(0.3) # 2D version
"""
w = base.getvector(w)
if base.iszerovec(w):
# for a zero so(n) return unit matrix, theta not relevant
if len(w) == 1:
return np.eye(2)
else:
return np.eye(3)
if theta is None:
w, theta = base.unitvec_norm(w)
skw = skew(w)
return np.eye(skw.shape[0]) + math.sin(theta) * skw + (
1.0 - math.cos(theta)) * skw @ skw
def __init__(self, m=None, r=None, I=None):
"""
Create a new spatial inertia
:param m: mass
:type m: float
:param r: centre of mass relative to link frame
:type r: 3-element array_like
:param I: inertia about the centre of mass, axes aligned with link frame
:type I: numpy.array, shape=(6,6)
- ``SpatialInertia(m, r I)`` is a spatial inertia object for a rigid-body
with mass ``m``, centre of mass at ``r`` relative to the link frame, and an
inertia matrix ``I`` (3x3) about the centre of mass.
- ``SpatialInertia(I)`` is a spatial inertia object with a value equal
to ``I`` (6x6).
"""
super().__init__()
if m is None and r is None and I is None:
# no arguments
I = SpatialInertia._identity()
elif m is not None and r is None and I is None and base.ismatrix(
m, (6, 6)):
I = base.getmatrix(m, (6, 6))
elif m is not None and r is not None:
r = base.getvector(r, 3)
if I is None:
I = np.zeros((3, 3))
else:
I = base.getmatrix(I, (3, 3))
C = base.skew(r)
I = np.block([[m * np.eye(3), m * C.T], [m * C, I + m * C @ C.T]])
else:
raise ValueError('bad values')
self.data = [I]
def isunittwist(v, tol=10):
r"""
Test if vector represents a unit twist in SE(2) or SE(3)
:param v: twist vector to test
:type v: array_like(6)
:param tol: tolerance in units of eps
:type tol: float
:return: whether twist has unit length
:rtype: bool
:raises ValueError: for incorrect vector length
Vector is is intepretted as :math:`[v, \omega]` where :math:`v \in \mathbb{R}^n` and
:math:`\omega \in \mathbb{R}^1` for SE(2) and :math:`\omega \in \mathbb{R}^3` for SE(3).
A unit twist can be a:
- unit rotational twist where :math:`|| \omega || = 1`, or
- unit translational twist where :math:`|| \omega || = 0` and :math:`|| v || = 1`.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> isunittwist([1, 2, 3, 1, 0, 0])
>>> isunittwist([0, 0, 0, 2, 0, 0])
:seealso: unit, isunitvec
"""
v = getvector(v)
if len(v) == 6:
# test for SE(3) twist
return isunitvec(v[3:6],
tol=tol) or (np.linalg.norm(v[3:6]) < tol * _eps
and isunitvec(v[0:3], tol=tol))
else:
raise ValueError
def Tx(cls, x):
"""
Create an SE(2) translation along the X-axis
:param x: translation distance along the X-axis
:type x: float
:return: SE(2) matrix
:rtype: SE2 instance
`SE2.Tx(x)` is an SE(2) translation of ``x`` along the x-axis
Example:
.. runblock:: pycon
>>> SE2.Tx(2)
>>> SE2.Tx([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl2(_x, 0) for _x in base.getvector(x)], check=False)
def qnorm(q):
r"""
Norm of a quaternion
:arg q: quaternion
:type v: : array_like(4)
:return: norm of the quaternion
:rtype: float
Returns the norm, length or magnitude of the input quaternion which is
:math:`(s^2 + v_x^2 + v_y^2 + v_z^2}^{1/2}`
.. runblock:: pycon
>>> from spatialmath.base import qnorm
>>> q = qnorm([1, 2, 3, 4])
>>> print(q)
:seealso: unit
"""
q = base.getvector(q, 4)
return np.linalg.norm(q)
def Tz(cls, z):
"""
Create an SE(3) translation along the Z-axis
:param z: translation distance along the Z-axis
:type z: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tz(z)` is an SE(3) translation of ``z`` along the z-axis
Example:
.. runblock:: pycon
>>> SE3.Tz(2)
>>> SE3.Tz([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(0, 0, _z) for _z in base.getvector(z)],
check=False)
def unit(q, tol=10):
"""
Create a unit quaternion
:arg v: quaterion
:type v: array_like(4)
:return: a pure quaternion
:rtype: ndarray(4)
:raises ValueError: quaternion has (near) zero norm
Creates a unit quaternion, with unit norm, by scaling the input quaternion.
.. runblock:: pycon
>>> from spatialmath.base import unit, qprint
>>> q = unit([1, 2, 3, 4])
>>> qprint(q)
.. note:: Scalar part is always positive.
.. note:: If the quaternion norm is less than ``tol * eps`` an exception is
raised.
:seealso: norm
"""
q = base.getvector(q, 4)
nm = np.linalg.norm(q)
if abs(nm) < tol * _eps:
raise ValueError("cannot normalize (near) zero length quaternion")
else:
q /= nm
if q[0] >= 0:
return q
else:
return -q
def Ty(cls, y):
"""
Create an SE(3) translation along the Y-axis
:param y: translation distance along the Y-axis
:type y: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Ty(y) is an SE(3) translation of ``y`` along the y-axis
Example:
.. runblock:: pycon
>>> SE3.Ty(2)
>>> SE3.Tz([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([base.transl(0, _y, 0) for _y in base.getvector(y)],
check=False)
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 skew(v):
r"""
Create skew-symmetric metrix from vector
:param v: vector
:type v: array_like(1) or array_like(3)
:return: skew-symmetric matrix in so(2) or so(3)
:rtype: ndarray(2,2) or ndarray(3,3)
:raises ValueError: bad argument
``skew(V)`` is a skew-symmetric matrix formed from the elements of ``V``.
- ``len(V)`` is 1 then ``S`` = :math:`\left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]`
- ``len(V)`` is 3 then ``S`` = :math:`\left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]`
.. runblock:: pycon
>>> from spatialmath.base import *
>>> skew(2)
>>> skew([1, 2, 3])
.. note::
- This is the inverse of the function ``vex()``.
- These are the generator matrices for the Lie algebras so(2) and so(3).
:seealso: :func:`vex`, :func:`skewa`
:SymPy: supported
"""
v = base.getvector(v, None, 'sequence')
if len(v) == 1:
return np.array([[0, -v[0]], [v[0], 0]])
elif len(v) == 3:
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
else:
raise ValueError("argument must be a 1- or 3-vector")
def edgelist(im, p, direction=1):
"""
Find edge of a region
:param im: binary image
:type im: ndarray(h,w,int)
:param p: initial point
:type p: array_like(2)
:param direction: direction to traverse region, +1 clockwise [default], -1
counter-clockwise
:type direction: int, optional
:raises ValueError: initial point is not on the edge
:raises RuntimeError: not able to find path to the goal
:return: edge list, direction vector list
:rtype: tuple of lists
``edge, dirs = edgelist(im, seed)`` is the boundary/contour/edge of a region
in the binary image ``im``. ``seed=[X,Y]`` is the coordinate of a point on
the edge of the region of interest, but belonging to the region.
``edge`` is a list of coordinates (2) of edge pixels of a region in theThe
elements of the edgelist are NumPy ndarray(2).
``dirs`` is a list of integers representing the direction of the edge from
the corresponding point in ``edge`` to the next point in ``edge``. The
integers in the range 0 to 7 represent directions: W SW S SE E NW N NW
respectively.
- Coordinates are given and returned assuming the matrix is an image, so the
indices are always in the form (x,y) or (column,row).
- ``im` is a binary image where 0 is assumed to be background, non-zero
is an object.
- ``p`` must be a point on the edge of the region.
- The initial point is always the first and last element of the returned edgelist.
- 8-direction chain coding can give incorrect results when used with
blobs founds using 4-way connectivty.
:Reference:
- METHODS TO ESTIMATE AREAS AND PERIMETERS OF BLOB-LIKE OBJECTS: A COMPARISON
Luren Yang, Fritz Albregtsen, Tor Lgnnestad and Per Grgttum
IAPR Workshop on Machine Vision Applications Dec. 13-15, 1994, Kawasaki
"""
if direction > 0:
neighbours = np.arange(start=0, stop=8, step=1)
else:
neighbours = np.arange(start=7, stop=-1, step=-1)
try:
pix0 = im[p[1], p[0]] # color of pixel we start at
except:
raise ValueError('specified coordinate is not within image')
p = base.getvector(p, 2, dtype=np.int)
q = adjacent_point(im, p, pix0)
# find an adjacent point outside the blob
if q is None:
raise ValueError('no neighbour outside the blob')
d = None
e = [p] # initialize the edge list
dir = [] # initialize the direction list
p0 = None
while True:
# find which direction is Q
dq = q - p
for kq in range(0, 8):
# get index of neighbour's direction in range [1,8]
if np.all(dq == _dirs[kq]):
break
# now test for directions relative to Q
for j in neighbours:
# get index of neighbour's direction in range [1,8]
k = (j + kq) % 8
# if k > 7:
# k = k - 7
# compute coordinate of the k'th neighbour
nk = p + _dirs[k]
try:
if im[nk[1], nk[0]] == pix0:
# if this neighbour is in the blob it is the next edge pixel
p = nk
break
except:
raise ValueError("Something went wrong calculating edgelist")
q = nk
dir.append(k)
# check if we are back where we started
if p0 is None:
p0 = p # make a note of where we started
else:
if all(p == p0):
break
# keep going, add this point to the edgelist
e.append(p)
return e, dir
def distancexform(occgrid, goal, metric='cityblock', show=False):
"""
Distance transform for path planning
:param occgrid: Occupancy grid
:type occgrid: NumPy ndarray
:param goal: Goal position (x,y)
:type goal: 2-element array-like
:param metric: distance metric, defaults to 'cityblock'
:type metric: str, optional
:return: Distance transform matrix
:rtype: NumPy ndarray
Returns a matrix the same size as the occupancy grid ``occgrid`` where each
cell contains the distance to the goal according to the chosen ``metric``.
- Obstacle cells will be set to ``nan``.
- Unreachable cells, ie. free cells _inside obstacles_ will be set
to ``inf``.
The cells of the passed occupancy grid are:
- zero, cell is free or driveable
- one, cell is an obstacle, or not driveable
"""
# build the matrix for performing distance transform:
# - obstacles are nan
# - other cells are inf
# - goal is zero
goal = base.getvector(goal, 2, dtype=np.int)
occgrid = occgrid.astype(np.float32)
occgrid[occgrid > 0] = np.nan # assign nan to obstacle cells
nans = np.isnan(occgrid) # keep record of where the NaNs are
occgrid[occgrid==0] = np.inf # assign inf to other cells
occgrid[goal[1], goal[0]] = 0 # assign zero to goal
# create the appropriate distance matrix D
if metric.lower() in ('manhattan', 'cityblock'):
D = np.array([
[ np.inf, 1, np.inf],
[ 1, 0, 1],
[ np.inf, 1, np.inf]
])
elif metric.lower() == 'euclidean':
r2 = np.sqrt(2)
D = np.array([
[ r2, 1, r2],
[ 1, 0, 1],
[ r2, 1, r2]
])
# get ready to iterate
count = 0
ninf = np.inf # number of infinities in the map
while True:
occgrid = dxstep(occgrid, D)
occgrid[nans] = np.nan
count += 1
# if opt.animate
# cmap = [1 0 0; gray(count)];
# colormap(cmap)
# image(occgrid+1, 'CDataMapping', 'direct');
# set(gca, 'Ydir', 'normal');
# xlabel('x');
# ylabel('y');
# if opt.animate
# anim.add();
# else
# pause(opt.delay);
ninfnow = sum(np.isinf(occgrid.flatten())) # current number of Infs
if ninfnow == ninf:
# stop if the number of Infs left in the map had stopped reducing
# it may never get to zero if there are unreachable cells in the map
break
ninf = ninfnow
print(f"{count:d} iterations, {ninf:d} unreachable cells")
return occgrid
def ikine_min(
self, T, q0=None, pweight=1.0, stiffness=0.0,
qlimits=True, ilimit=1000):
"""
Inverse kinematics by optimization with joint limits (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 pweight: weighting on position error norm compared to rotation
error (default 1)
:type pweight: float
:param stiffness: Stiffness used to impose a smoothness contraint on
joint angles, useful when n is large (default 0)
:type stiffness: float
:param qlimits: Enforce joint limits (default True)
:type qlimits: bool
:param ilimit: Iteration limit (default 1000)
:type ilimit: bool
:return: inverse kinematic solution
:rtype: named tuple
``sol = robot.ikine_unc(T)`` are the joint coordinates (n) corresponding
to the robot end-effector pose T which is an SE3 object. 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.
.. note::
- PROTOTYPE CODE UNDER DEVELOPMENT, intended to do numerical
inverse kinematics with joint limits
- Uses ``SciPy.minimize`` with/without constraints.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
- This norm is computed from distances and angles and ``pweight``
can be used to scale the position error norm to be congruent
with rotation error norm.
- For a highly redundant robot ``stiffness`` can be used to impose
a smoothness contraint on joint angles, tending toward solutions
with are smooth curves.
- 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 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 become explicit bounds if 'qlimits' is set.
.. warning::
- The objective function is rather uncommon.
- Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
uses a scalar cost-function and does not provide a Jacobian.
:seealso: :func:`ikine_LM`, :func:`ikine_LMS`, :func:`ikine_unc`,
:func:`ikine_con`
""" # noqa
if not isinstance(T, SE3):
T = SE3(T)
if q0 is None:
q0 = np.zeros((self.n,))
else:
q0 = base.getvector(q0, self.n)
col = 2
solutions = []
# Define the cost function to minimise
def cost(q, *args):
T, pweight, col, stiffness = args
Tq = self.fkine(q).A
# translation error
dT = base.transl(T) - base.transl(Tq)
E = np.linalg.norm(dT) * pweight
# Rotation error
# Find dot product of two columns
dd = np.dot(T[0:3, col], Tq[0:3, col])
E += np.arccos(dd)**2 * 1000
if stiffness > 0:
# Enforce a continuity constraint on joints, minimum bend
E += np.sum(np.diff(q)**2) * stiffness
return E
for Tk in T:
if qlimits:
bounds = Bounds(self.qlim[0, :], self.qlim[1, :])
res = minimize(
cost,
q0,
args=(Tk.A, pweight, col, stiffness),
bounds=bounds,
options={'gtol': 1e-6, 'maxiter': ilimit})
else:
# No joint limits, unconstrained optimization
res = minimize(
cost,
q0,
args=(Tk.A, pweight, col, stiffness),
options={'gtol': 1e-6, 'maxiter': ilimit})
solution = iksol(res.x, res.success, res.message, res.nit, res.fun)
solutions.append(solution)
q0 = res.x # use this solution as initial estimate for next time
if len(T) == 1:
return solutions[0]
else:
return solutions
def __init__(self,
covar=None,
speed_max=np.inf,
accel_max=np.inf,
x0=None,
dt=0.1,
control=None,
animation=None,
verbose=False,
dim=10):
r"""
Superclass for vehicle kinematic models
:param covar: odometry covariance, defaults to zero
:type covar: ndarray(2,2), optional
:param speed_max: maximum speed, defaults to :math:`\infty`
:type speed_max: float, optional
:param accel_max: maximum acceleration, defaults to :math:`\infty`
:type accel_max: float, optional
:param x0: Initial state, defaults to (0,0,0)
:type x0: array_like(3), optional
:param dt: sample update interval, defaults to 0.1
:type dt: float, optional
:param control: vehicle control inputs, defaults to None
:type control: array_like(2), interp1d, VehicleDriver
:param animation: Graphical animation of vehicle, defaults to None
:type animation: VehicleAnimation subclass, optional
:param verbose: print lots of info, defaults to False
:type verbose: bool, optional
:param dim: dimensions of 2D plot area, defaults to (-10:10) x (-10:10),
see :func:`~spatialmath.base.animate.plotvol2`
:type dims: float, array_like(2), , array_like(4)
This is an abstract superclass that simulates the motion of a mobile
robot under the action of a controller. The controller provides
control inputs to the vehicle, and the output odometry is returned.
The true state, effectively unknowable in practice, is computed
and accessible.
:seealso: :func:`Bicycle`, :func:`Unicycle`
"""
if covar is None:
covar = np.zeros((2, 2))
self._V = covar
self._dt = dt
if x0 is None:
x0 = np.zeros((3, ), dtype=float)
else:
x0 = base.getvector(x0)
if len(x0) not in (2, 3):
raise ValueError('x0 must be length 2 or 3')
self._x0 = x0
self._x = x0
self._speed_max = speed_max
self._accel_max = accel_max
self._v_prev = 0
self._vehicle_plot = None
if control is not None:
self.add_driver(control)
self._animation = animation
self._dt = dt
self._t = 0
self._stopsim = False
self._verbose = verbose
self._plot = False
self._dim = dim
self._x_hist = np.empty((0, len(x0)))
def ikine_con(self, T, q0=None, **kwargs):
r"""
Inverse kinematics by optimization with joint limits (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)
:return: inverse kinematic solution
:rtype: named tuple
``sol = robot.ikine_unc(T)`` are the joint coordinates (n)
corresponding to the robot end-effector pose T which is an SE3 object.
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.
.. note::
- Uses ``SciPy.minimize`` with bounds.
- Joint limits are considered in this solution.
- Can be used for robots with arbitrary degrees of freedom.
- 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.
- The objective function (error) is
:math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2`
where :math:`\mat{\Omega}` is a diagonal matrix.
- Joint offsets, if defined, are accounted for in the solution.
.. warning::
- The objective function is rather uncommon.
- Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
uses a scalar cost-function and does not provide a Jacobian.
:author: Bryan Moutrie, for RTB-MATLAB
:seealso: :func:`ikine_LM`, :func:`ikine_LMS`, :func:`ikine_unc`,
:func:`ikine_min`
""" # noqa
if not isinstance(T, SE3):
T = SE3(T)
if q0 is None:
q0 = np.zeros((self.n))
else:
q0 = base.getvector(q0, self.n)
solutions = []
omega = np.diag([1, 1, 1, 3 / self.reach])
def cost(q, *args):
T, omega = args
E = (base.trinv(T) @ self.fkine(q).A - np.eye(4)) @ omega
return (E**2).sum() # quicker than np.sum(E**2)
bnds = Bounds(self.qlim[0, :], self.qlim[1, :])
for Tk in T:
res = minimize(
cost,
q0,
args=(Tk.A, omega),
bounds=bnds,
tol=1e-8,
options={'ftol': 1e-10})
solution = iksol(res.x, res.success, res.message, res.nit, res.fun)
solutions.append(solution)
q0 = res.x # use this solution as initial estimate for next time
if len(T) == 1:
return solutions[0]
else:
return solutions
def ikine_LMS(
self, T,
q0=None,
mask=None,
ilimit=500,
tol=1e-10,
wN=1e-3,
Lmin=0
):
"""
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 tol: final error tolerance (default 1e-10)
:type tol: float
:param ωN: damping coefficient
:type ωN: float (default 1e-3)
: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 should equal the number of
manipulator DOF.
For example when using a 3 DOF manipulator rotation orientation might
be unimportant in which case use the option: mask = [1 1 1 0 0 0].
.. note::
- Implements a modified Levenberg-Marquadt variable-step-size
solver which is quite robust in practice.
- 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.
:references:
- "Solvability-Unconcerned Inverse Kinematics by the
Levenberg–Marquardt Method", T. Sugihara, IEEE T-RO, 27(5),
October 2011, pp. 984-991.
:seealso: :func:`ikine_LM`, :func:`ikine_unc`, :func:`ikine_con`,
:func:`ikine_min`
""" # noqa
if not isinstance(T, SE3):
T = SE3(T)
solutions = []
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]] # noqa
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)
else:
# non-diagonal matrix case
ln = base.norm(l)
a = math.atan2(ln, np.trace(R) - 1) * l / ln
return np.r_[d, a]
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
# bool vector indicating revolute joints
revolutes = np.array([link.isrevolute() for link in self])
q = q0
for Tk in T:
iterations = 0
failure = None
nm = None
while True:
# Update the count and test against iteration limit
iterations += 1
if iterations > ilimit:
failure = f"iteration limit {ilimit} exceeded"
break
e = angle_axis(self.fkine(q).A, Tk.A)
# Are we there yet?
E = 0.5 * e.T @ W @ e
if E < tol:
break
# Compute the Jacobian
J = self.jacob0(q)
JtJ = J.T @ W @ J
# Do the damped inverse Gauss-Newton with
# Levenberg-Marquadt
dq = np.linalg.inv(
JtJ + (E + wN) * np.eye(self.n)
) @ J.T @ W @ e
# Compute possible new value of
q += dq
# 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"|e|={E: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
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
def qlim(self, qlim_new):
if qlim_new is None:
self._qlim = None
else:
self._qlim = getvector(qlim_new, 2)
def __init__(self,
y0=0,
yf=1,
T=None,
time=False,
traj='lspb',
*inputs,
**kwargs):
"""
:param y0: initial value
:type y0: array_like(m)
:param yf: final value
:type yf: array_like(m)
:param time: x is simulation time, defaults to False
:type time: bool
:param traj: trajectory type: 'lspb' [default] or 'tpoly'
:type traj: str
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param ``**kwargs``: common Block options
:return: TRAJ block
:rtype: Traj instance
Create a trajectory block.
A block that generates a trajectory using a trapezoidal or quintic
polynomial profile.
A simple triangle function with domain [0,10] and range [0,1] can be
defined by::
INTERPOLATE(x=(0,5,10), y=(0,1,0))
We might also express this as::
INTERPOLATE(xy=[(0,0), (5,1), (10,0)])
The distance along the trajectory comes from:
- Input port 0
- Simulation time, if ``time=True``. In this case the block has no
input ports and is a ``Source`` not a ``Function``.
"""
self.time = time
if time:
nin = 0
self.blockclass = 'source'
else:
nin = 1
super().__init__(nin=nin, nout=3, inputs=inputs, **kwargs)
self.type = 'function'
y0 = base.getvector(y0)
yf = base.getvector(yf)
assert len(y0) == len(yf), 'y0 and yf must have same length'
self.y0 = y0
self.yf = yf
self.time = time
self.T = T
self.traj = traj
self.outport_names(('y', 'yd', 'ydd'))
def init(self, x0=None, animation=None, plot=False, control=None):
"""
Initialize for simulation (superclass method)
:param x0: Initial state, defaults to value given to Vehicle constructor
:type x0: array_like(3) or array_like(2)
:param animation: vehicle animation object, defaults to None
:type animation: VehicleAnimation subclass, optional
:param plot: Enable plotting, defaults to False
:type plot: bool, optional
Performs the following initializations:
#. Clears the state history
#. Sets state :math:`x = x_0`
#. If a driver is attached, initialize it
#. If plotting is enabled, initialize that
If ``plot`` is set and no animation object is given, use a default
``VehiclePolygon('car')``
:seealso: :func:`VehicleAnimation`
"""
if x0 is not None:
self._x = base.getvector(x0, 3)
else:
self._x = self._x0
self._x_hist = np.empty((0, 3))
if control is not None:
self._control = control
self._t = 0
# initialize the graphics
self._plot = plot
if plot:
if animation is None:
animation = self._animation # get default animation if set
else:
# use default animation
animation = VehiclePolygon("car")
self._animation = animation
# setu[ the plot]
plt.clf()
self._ax = base.plotvol2(self._dim)
plt.xlabel('x')
plt.ylabel('y')
self._ax.set_aspect('equal')
self._ax.figure.canvas.set_window_title(
f"Robotics Toolbox for Python (Figure {self._ax.figure.number})"
)
animation.add() # add vehicle animation to axis
self._timer = plt.figtext(0.85, 0.95, '') # display time counter
# initialize the driver
if isinstance(self._control, VehicleDriver):
self._control.init(ax=self._ax)
def dot(self, value):
return np.dot(self.A, base.getvector(value, 6))
def r(self, r_new):
self._r = getvector(r_new, 3)
def transl2(x, y=None):
"""
Create SE(2) pure translation, or extract translation from SE(2) matrix
**Create a translational SE(2) matrix**
:param x: translation along X-axis
:type x: float
:param y: translation along Y-axis
:type y: float
:return: SE(2) transform matrix or the translation elements of a homogeneous transform
:rtype: ndarray(3,3)
- ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a
pure translation.
- ``T = transl2( V )`` as above but the translation is given by a 2-element
list, dict, or a numpy array, row or column vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> transl2(3, 4)
>>> transl2([3, 4])
>>> transl2(np.array([3, 4]))
**Extract the translational part of an SE(2) matrix**
:param x: SE(2) transform matrix
:type x: ndarray(3,3)
:return: translation elements of SE(2) matrix
:rtype: ndarray(2)
- ``t = transl2(T)`` is the translational part of the SE(3) matrix ``T`` as a
2-element NumPy array.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
>>> transl2(T)
.. note:: This function is compatible with the MATLAB version of the Toolbox. It
is unusual/weird in doing two completely different things inside the one
function.
"""
if np.isscalar(x):
T = np.identity(3)
T[:2, 2] = [x, y]
return T
elif base.isvector(x, 2):
T = np.identity(3)
T[:2, 2] = base.getvector(x, 2)
return T
elif base.ismatrix(x, (3, 3)):
return x[:2, 2]
else:
ValueError('bad argument')
def __init__(self, c):
self.plane = base.getvector(c, 4)
def trexp2(S, theta=None, check=True):
"""
Exponential of so(2) or se(2) matrix
:param S: se(2), so(2) matrix or equivalent velctor
:type T: ndarray(3,3) or ndarray(2,2)
:param theta: motion
:type theta: float
:return: matrix exponential in SE(2) or SO(2)
:rtype: ndarray(3,3) or ndarray(2,2)
:raises ValueError: bad argument
An efficient closed-form solution of the matrix exponential for arguments
that are se(2) or so(2).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(Σ)`` is the matrix exponential of the se(2) element ``Σ`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(Σ, θ)`` as above but for an se(3) motion of Σθ, where ``Σ``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
- ``trexp2(S)`` is the matrix exponential of the se(3) element ``S`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(S, θ)`` as above but for an se(2) motion of Sθ, where ``S``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skew(1))
>>> trexp2(skew(1), 2) # revolute unit twist
>>> trexp2(1)
>>> trexp2(1, 2) # revolute unit twist
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(Ω, θ)`` as above but for an so(3) motion of Ωθ, where ``Ω`` is
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
given by ``θ``.
- ``trexp2(ω)`` is the matrix exponential of the so(2) element ``ω`` expressed as
a 1-vector.
- ``trexp2(ω, θ)`` as above but for an so(3) motion of ωθ where ``ω`` is a
unit-norm vector representing a rotation axis and a rotation magnitude
given by ``θ``. ``ω`` is expressed as a 1-vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skewa([1, 2, 3]))
>>> trexp2(skewa([1, 0, 0]), 2) # prismatic unit twist
>>> trexp2([1, 2, 3])
>>> trexp2([1, 0, 0], 2)
:seealso: trlog, trexp2
"""
if base.ismatrix(S, (3, 3)) or base.isvector(S, 3):
# se(2) case
if base.ismatrix(S, (3, 3)):
# augmentented skew matrix
if check and not base.isskewa(S):
raise ValueError("argument must be a valid se(2) element")
tw = base.vexa(S)
else:
# 3 vector
tw = base.getvector(S)
if base.iszerovec(tw):
return np.eye(3)
if theta is None:
(tw, theta) = base.unittwist2_norm(tw)
elif not base.isunittwist2(tw):
raise ValueError("If theta is specified S must be a unit twist")
t = tw[0:2]
w = tw[2]
R = base.rodrigues(w, theta)
skw = base.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return base.rt2tr(R, V @ t)
elif base.ismatrix(S, (2, 2)) or base.isvector(S, 1):
# so(2) case
if base.ismatrix(S, (2, 2)):
# skew symmetric matrix
if check and not base.isskew(S):
raise ValueError("argument must be a valid so(2) element")
w = base.vex(S)
else:
# 1 vector
w = base.getvector(S)
if theta is not None and not base.isunitvec(w):
raise ValueError("If theta is specified S must be a unit twist")
# do Rodrigues' formula for rotation
return base.rodrigues(w, theta)
else:
raise ValueError(
" First argument must be SO(2), 1-vector, SE(2) or 3-vector")