#!/usr/bin/env python3
"""
@author: Jesse Haviland
@author: Peter Corke
"""
from __future__ import annotations
from copy import deepcopy
from typing import Any
import numpy as np
from spatialmath import SE2
from spatialmath.base import (
simplify,
tr2jac2,
)
from roboticstoolbox.ets._ETS import BaseETS
from roboticstoolbox.ets.ET2 import ET2
from roboticstoolbox.tools.types import ArrayLike, NDArray
[docs]
class ETS2(BaseETS[ET2]):
"""
This class implements an elementary transform sequence (ETS) for 2D
:param arg: Function to compute ET value
An instance can contain an elementary transform (ET) or an elementary
transform sequence (ETS). It has list-like properties by subclassing
UserList, which means we can perform indexing, slicing pop, insert, as well
as using it as an iterator over its values.
- ``ETS()`` an empty ETS list
- ``ET2.XY(η)`` is a constant elementary transform
- ``ET2.XY(η, 'deg')`` as above but the angle is expressed in degrees
- ``ET2.XY()`` is a joint variable, the value is left free until evaluation
time
- ``ET2.XY(jindex=J)`` as above but the joint index is explicitly given, this
might correspond to the joint number of a multi-joint robot.
- ``ET2.XY(flip=True)`` as above but the joint moves in the opposite sense
where ``XY`` is one of ``R``, ``tx``, ``ty``.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ET2
>>> e = ET2.R(0.3) * ET2.tx(2) # an ETS2
>>> len(e) # of length 2
>>> e[1] # an ET2 indexed from the ETS2
:references:
- Kinematic Derivatives using the Elementary Transform Sequence,
J. Haviland and P. Corke
:seealso: ``r``, ``tx``, ``ty``
"""
def __init__(
self, arg: list[ETS2 | ET2] | list[ET2] | list[ETS2] | ET2 | ETS2 | None = None
) -> None:
super().__init__()
if isinstance(arg, list):
for item in arg:
if isinstance(item, ET2):
self._data.append(deepcopy(item))
elif isinstance(item, ETS2):
for ets_item in item:
self._data.append(deepcopy(ets_item))
else:
raise TypeError("bad arg")
elif isinstance(arg, ET2):
self._data.append(deepcopy(arg))
elif isinstance(arg, ETS2):
for ets_item in arg:
self._data.append(deepcopy(ets_item))
elif arg is not None:
raise TypeError("bad arg")
self._ndims = 2
self._auto_jindex = False
# Check if jindices are set
joints = self.joints()
# Number of joints with a jindex
jindices = 0
# Number of joints with a sequential jindex (j[2] -> jindex = 2)
seq_jindex = 0
# Count them up
for j, joint in enumerate(joints):
if joint.jindex is not None:
jindices += 1
if joint.jindex == j:
seq_jindex += 1
if (
jindices == self.n - 1
and seq_jindex == self.n - 1
and joints[-1].jindex is None
):
# ets has sequential jindicies, except for the last.
joints[-1].jindex = self.n - 1
self._auto_jindex = True
elif jindices > 0 and not jindices == self.n:
raise ValueError(
"You can not have some jindices set for the ET's in arg. It must be all"
" or none"
) # pragma: nocover
elif jindices == 0 and self.n > 0:
# Set them ourself
for j, joint in enumerate(joints):
joint.jindex = j
self._auto_jindex = True
def __mul__(self, other: ET2 | ETS2) -> ETS2:
if isinstance(other, ET2):
return ETS2([*self._data, other])
else:
return ETS2([*self._data, *other._data]) # pragma: nocover
def __rmul__(self, other: ET2 | ETS2) -> ETS2:
return ETS2([other, *self._data]) # pragma: nocover
def __imul__(self, rest: ET2 | ETS2) -> ETS2:
return self + rest # pragma: nocover
def __add__(self, rest: ET2 | ETS2) -> ETS2:
return self.__mul__(rest) # pragma: nocover
[docs]
def fixjoint(self, j: int, value: float, unit: str = "rad") -> ETS2:
"""Return an independent ETS2 with the specified joint held constant.
j is the zero-based position among joint ETs, independent of their
jindex values. Remaining joints retain explicit global jindices;
automatically indexed joints are renumbered to fill the gap. Joint
limits and flip flags are preserved. The original is not modified.
:param j: index of the joint to fix
:param value: fixed joint coordinate, in radians/degrees or metres
:param unit: angular unit for rotational joints, "rad" [default] or "deg"
:returns: ETS2 with one fewer joint
:raises ValueError: for an invalid joint position or angular unit
"""
if not isinstance(j, (int, np.integer)) or not 0 <= j < self.n:
raise ValueError(f"joint index {j} out of range for {self.n} joints")
if unit not in ("rad", "deg"):
raise ValueError("unit must be 'rad' or 'deg'")
parts = []
joint_index = 0
for et in self:
if not et.isjoint:
parts.append(et)
continue
constructor = getattr(ET2, et.kind)
if joint_index == j:
fixed = -value if et.isflip else value
if et.isrotation:
parts.append(constructor(fixed, unit=unit))
else:
if unit != "rad":
raise ValueError("unit='deg' is only valid for rotational joints")
parts.append(constructor(fixed))
else:
if self._auto_jindex:
parts.append(constructor(flip=et.isflip, qlim=et.qlim))
else:
parts.append(
constructor(flip=et.isflip, qlim=et.qlim, jindex=et.jindex)
)
joint_index += 1
return ETS2(parts)
[docs]
def compile(self) -> ETS2:
"""
Compile an ETS2
:return: optimised ETS2
Perform constant folding for faster evaluation. Consecutive constant
ETs are compounded, leading to a constant ET which is denoted by
``SE3`` when displayed.
:seealso: ``isconstant``
"""
const = None
ets = ETS2()
for et in self:
if et.isjoint:
# a joint
if const is not None:
# flush the constant
if not np.array_equal(const, np.eye(3)):
ets *= ET2.SE2(const)
const = None
ets *= et # emit the joint ET
else:
# not a joint
if const is None:
const = et.A()
else:
const = const @ et.A()
if const is not None:
# flush the constant, tool transform
if not np.array_equal(const, np.eye(3)):
ets *= ET2.SE2(const)
return ets
[docs]
def insert(
self,
i: int,
arg: ET2 | ETS2,
) -> None:
"""
Insert value
:param i: position to insert at
:param arg: the elementary transform or sequence to insert
Inserts an ET or ETS into the ET sequence. The inserted value is at position
``i``.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ET2
>>> e = ET2.R() * ET2.tx(1) * ET2.R() * ET2.tx(1)
>>> f = ET2.R()
>>> e.insert(2, f)
>>> e
"""
if isinstance(arg, ET2):
self._data.insert(i, arg)
elif isinstance(arg, ETS2):
for j, et in enumerate(arg):
self._data.insert(i + j, et)
self._fknm_stale = True
[docs]
def fkine(
self,
q: ArrayLike | None = None,
base: NDArray | SE2 | None = None,
tool: NDArray | SE2 | None = None,
include_base: bool = True,
) -> SE2:
"""
Forward kinematics
:param q: joint coordinates, optional for a sequence with no joints
:param base: base transform, optional
:param tool: tool transform, optional
:returns: transformation matrix representing the end-effector pose
:rtype: SE2
``T = ets.fkine(q)`` evaluates forward kinematics for the robot at
joint configuration ``q``.
**Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a
trajectory and the result is an ``SE2`` instance with ``m`` values.
.. note::
- The robot's base tool transform, if set, is incorporated into the result.
- A tool transform, if provided, is incorporated into the result.
- Works from the end-effector link to the base
.. rubric:: References
- Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke
"""
ret = SE2.Empty()
fk = self.eval(q, base, tool, include_base)
if fk.dtype == "O":
# symbolic
fk = np.array(simplify(fk))
if fk.ndim == 3:
for T in fk:
ret.append(SE2(T, check=False))
else:
ret = SE2(fk, check=False)
return ret
[docs]
def eval(
self,
q: ArrayLike | None = None,
base: NDArray | SE2 | None = None,
tool: NDArray | SE2 | None = None,
include_base: bool = True,
) -> NDArray:
"""
Forward kinematics (returns raw ndarray)
:param q: joint coordinates, optional for a sequence with no joints
:param base: base transform, optional
:param tool: tool transform, optional
:returns: transformation matrix representing the end-effector pose
:rtype: ndarray(3,3) or ndarray(m,3,3)
``T = ets.eval(q)`` evaluates forward kinematics for the robot at
joint configuration ``q``, returning the raw ndarray.
**Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a
trajectory and the result is an ndarray of shape (m,3,3).
.. note::
- The robot's base tool transform, if set, is incorporated into the result.
- A tool transform, if provided, is incorporated into the result.
- Works from the end-effector link to the base
.. rubric:: References
- Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke
"""
if q is None:
q = []
q = self._resolve_q(q, allow_trajectory=True)
l, _ = q.shape
end = self[-1]
if base is None:
bases = None
elif isinstance(base, SE2):
bases = np.array(base.A)
elif np.array_equal(base, np.eye(3)): # pragma: nocover
bases = None
else: # pragma: nocover
bases = base
if tool is None:
tools = None
elif isinstance(tool, SE2):
tools = np.array(tool.A)
elif np.array_equal(tool, np.eye(3)): # pragma: nocover
tools = None
else: # pragma: nocover
tools = tool
if l > 1:
T = np.zeros((l, 3, 3), dtype=object)
else:
T = np.zeros((3, 3), dtype=object)
for k, qk in enumerate(q):
link = end # start with last link
jindex = 0 if link.jindex is None and link.isjoint else link.jindex
Tk = link.A(qk[jindex])
if tools is not None:
Tk = Tk @ tools
# add remaining links, back toward the base
for i in range(self.m - 2, -1, -1):
link = self._data[i]
jindex = 0 if link.jindex is None and link.isjoint else link.jindex
A = link.A(qk[jindex])
if A is not None:
Tk = A @ Tk
# add base transform if it is set
if include_base is True and bases is not None:
Tk = bases @ Tk
# append
if l > 1:
T[k, :, :] = Tk
# ret.append(SE2(Tk, check=False))
else:
T = Tk
# ret = SE2(Tk, check=False)
return T
[docs]
def jacob0(
self,
q: ArrayLike,
) -> NDArray:
"""
Manipulator Jacobian in the base frame
:param q: the joint coordinates
:returns: the Jacobian, rows are :math:`(v_x, v_y, \\omega)`, one column
per joint
.. note:: This is a simple, not efficient, implementation.
"""
# very inefficient implementation, just put a 1 in last row
# if its a rotation joint
j = 0
J = np.zeros((3, self.n))
etjoints = self.joint_idx()
if any(self[i].jindex is None for i in etjoints):
# not all joints have a jindex it is required, set them
for j in range(self.n):
i = etjoints[j]
self[i].jindex = j
self.__dict__.pop("jindices", None) # invalidate cached_property
q = self._resolve_q(q)
for j in range(self.n):
i = etjoints[j]
if self[i].jindex is not None:
jindex = self[i].jindex
else:
jindex = 0 # pragma: nocover
# jindex = 0 if self[i].jindex is None else self[i].jindex
axis = self[i].kind
if axis == "R":
dTdq = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) @ self[i].A(
q[jindex]
)
elif axis == "tx":
dTdq = np.array([[0, 0, 1], [0, 0, 0], [0, 0, 0]])
elif axis == "ty":
dTdq = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 0]])
else: # pragma: nocover
raise TypeError("Invalid axes")
if self[i].isflip:
dTdq = -dTdq
E0 = ETS2(self[:i])
if len(E0) > 0:
dTdq = E0.fkine(q).A @ dTdq
Ef = ETS2(self[i + 1 :])
if len(Ef) > 0:
dTdq = dTdq @ Ef.fkine(q).A
T = self.fkine(q).A
dRdt = dTdq[:2, :2] @ T[:2, :2].T
J[:2, j] = dTdq[:2, 2]
J[2, j] = dRdt[1, 0]
return J
[docs]
def jacobe(self, q: ArrayLike) -> Any:
r"""
Jacobian in end-effector frame
:param q: joint coordinates
:returns: Jacobian matrix
:rtype: ndarray(3,n)
``jacobe(q)`` is the manipulator Jacobian matrix which maps joint
velocity to end-effector spatial velocity.
End-effector spatial velocity :math:`\nu = (v_x, v_y, \omega)^T`
is related to joint velocity by :math:`{}^{e}\nu = {}^{e}\mathbf{J}_0(q) \dot{q}`.
:seealso: :func:`jacob0`, ``hessian0``
"""
T = self.fkine(q, include_base=False).A
return tr2jac2(T.T) @ self.jacob0(q)