#!/usr/bin/env python
"""
@author: Jesse Haviland
@author: Peter Corke
"""
from __future__ import annotations
# import sys
from os.path import splitext
from copy import deepcopy
from warnings import warn
from pathlib import PurePosixPath, Path
from typing import TextIO, TypeVar, Literal as L, overload, cast
import numpy as np
import spatialmath.base as smb
from spatialmath.base.argcheck import (
getvector,
getmatrix,
verifymatrix,
)
from spatialgeometry import Shape, CollisionShape, Cylinder
from spatialmath import (
SE3,
SE2,
SpatialAcceleration,
SpatialVelocity,
SpatialInertia,
SpatialForce,
)
import roboticstoolbox as rtb
from roboticstoolbox.robot.BaseRobot import BaseRobot
from roboticstoolbox.robot.RobotKinematics import RobotKinematicsMixin
from roboticstoolbox.robot.Gripper import Gripper
from roboticstoolbox.robot.Link import BaseLink, Link, Link2
from roboticstoolbox.ets.ETS import ETS
from roboticstoolbox.ets.ETS2 import ETS2
from roboticstoolbox.tools import URDF
from roboticstoolbox.tools.types import ArrayLike, NDArray
from roboticstoolbox.tools.data import rtb_path_to_datafile
# A generic type variable representing any subclass of BaseLink
LinkType = TypeVar("LinkType", bound=BaseLink)
# ==================================================================================== #
# ================= Robot Class ====================================================== #
# ==================================================================================== #
[docs]
class Robot(BaseRobot[Link], RobotKinematicsMixin):
_color = True
def __init__(
self,
arg: list[Link] | ETS | Robot,
gripper_links: Link | list[Link] | None = None,
name: str = "",
manufacturer: str = "",
comment: str = "",
base: NDArray | SE3 | None = None,
tool: NDArray | SE3 | None = None,
gravity: ArrayLike = [0, 0, -9.81],
keywords: list[str] | tuple[str, ...] = [],
symbolic: bool = False,
configs: dict[str, NDArray] | None = None,
check_jindex: bool = True,
):
# Process links
if isinstance(arg, Robot):
# We're passed a Robot, clone it
# We need to preserve the parent link as we copy
# Copy each link within the robot
links = [deepcopy(link) for link in arg.links]
gripper_links = []
for gripper in arg.grippers:
glinks = []
for link in gripper.links:
glinks.append(deepcopy(link))
gripper_links.append(glinks[0])
links = links + glinks
# Sever parent connection, but save the string
# The constructor will piece this together for us
for link in links:
link._children = []
if link.parent is not None:
link._parent_name = link.parent.name
link._parent = None
super().__init__(links, gripper_links=gripper_links)
for i, gripper in enumerate(self.grippers):
gripper.tool = arg.grippers[i].tool.copy()
self._urdf_filepath = arg.urdf_filepath
else:
if isinstance(arg, ETS):
# We're passed an ETS string
links = []
# chop it up into segments, a link frame after every joint
# split()'s default "last" method folds any base content into
# the first segment, so `base` is always empty and dropped;
# `gripper` holds trailing constant content, if any, and
# becomes one extra static (non-joint) link.
_, *segs, gripper = arg.split()
if gripper:
segs.append(gripper)
parent = None
for j, ets_j in enumerate(segs):
elink = Link(ETS(ets_j), parent=parent, name=f"link{j:d}")
if (
elink.qlim is None
and elink.v is not None
and elink.v.qlim is not None # type: ignore[union-attr]
):
elink.qlim = elink.v.qlim # type: ignore[union-attr] # pragma nocover
parent = elink
links.append(elink)
elif smb.islistof(arg, Link):
links = arg
else:
raise TypeError("arg was invalid, must be List[Link], ETS, or Robot")
# Initialise Base Robot object
super().__init__(
links=links,
gripper_links=gripper_links,
name=name,
manufacturer=manufacturer,
comment=comment,
base=base,
tool=tool,
gravity=gravity,
keywords=keywords,
symbolic=symbolic,
configs=configs,
check_jindex=check_jindex,
)
# --------------------------------------------------------------------- #
# --------- Swift Methods --------------------------------------------- #
# --------------------------------------------------------------------- #
def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0):
ob = []
for link in self.links:
if robot_alpha > 0:
for gi in link.geometry:
gi.set_alpha(robot_alpha)
ob.append(gi.to_dict())
if collision_alpha > 0:
for gi in link.collision:
gi.set_alpha(collision_alpha)
ob.append(gi.to_dict())
# Do the grippers now
for gripper in self.grippers:
for link in gripper.links:
if robot_alpha > 0:
for gi in link.geometry:
gi.set_alpha(robot_alpha)
ob.append(gi.to_dict())
if collision_alpha > 0:
for gi in link.collision:
gi.set_alpha(collision_alpha)
ob.append(gi.to_dict())
# for o in ob:
# print(o)
return ob
def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0):
ob = []
# Do the robot
for link in self.links:
if robot_alpha > 0:
for gi in link.geometry:
ob.append(gi.fk_dict())
if collision_alpha > 0:
for gi in link.collision:
ob.append(gi.fk_dict())
# Do the grippers now
for gripper in self.grippers:
for link in gripper.links:
if robot_alpha > 0:
for gi in link.geometry:
ob.append(gi.fk_dict())
if collision_alpha > 0:
for gi in link.collision:
ob.append(gi.fk_dict())
return ob
# --------------------------------------------------------------------- #
# --------- URDF Methods ---------------------------------------------- #
# --------------------------------------------------------------------- #
# @staticmethod
# def URDF_read(
# file_path, tld=None, xacro_tld=None
# ) -> Tuple[List[Link], str, str, Union[Path, PurePosixPath]]:
# """
# Read a URDF file as Links
# File should be specified relative to ``RTBDATA/URDF/xacro``
# Parameters
# ----------
# file_path
# File path relative to the xacro folder
# tld
# A custom top-level directory which holds the xacro data,
# defaults to None
# xacro_tld
# A custom top-level within the xacro data,
# defaults to None
# Returns
# -------
# links
# a list of links
# name
# the name of the robot
# urdf
# a string representing the URDF
# file_path
# a path to the original file
# Notes
# -----
# If ``tld`` is not supplied, filepath pointing to xacro data should
# be directly under ``RTBDATA/URDF/xacro`` OR under ``./xacro`` relative
# to the model file calling this method. If ``tld`` is supplied, then
# ```file_path``` needs to be relative to ``tld``
# """
# # Get the path to the class that defines the robot
# if tld is None:
# base_path = rtb_path_to_datafile("xacro")
# else:
# base_path = PurePosixPath(tld)
# # Add on relative path to get to the URDF or xacro file
# # base_path = PurePath(classpath).parent.parent / 'URDF' / 'xacro'
# file_path = base_path / PurePosixPath(file_path)
# _, ext = splitext(file_path)
# if ext == ".xacro":
# # it's a xacro file, preprocess it
# if xacro_tld is not None:
# xacro_tld = base_path / PurePosixPath(xacro_tld)
# urdf_string = xacro.main(file_path, xacro_tld)
# try:
# urdf = URDF.loadstr(urdf_string, file_path, base_path)
# except BaseException as e: # pragma nocover
# print("error parsing URDF file", file_path)
# raise e
# else: # pragma nocover
# urdf_string = open(file_path).read()
# urdf = URDF.loadstr(urdf_string, file_path, base_path)
# if not isinstance(urdf_string, str): # pragma nocover
# raise ValueError("Parsing failed, did not get valid URDF string back")
# return urdf.elinks, urdf.name, urdf_string, file_path
# def urdf_load(self, file: str|Path|TextIO, manufacturer: str|None = None, model: str|None = None):
# from roboticstoolbox.tools.urdf import URDF
# from xacrodoc import XacroDoc
# from xacrodoc import packages
# # Explicitly map package names to their paths (no ROS / package.xml needed)
# import rtbdata
# xacro_root = Path(rtbdata.__file__).parent / "xacro"
# packages.update_package_cache({
# d.name: str(d) for d in xacro_root.iterdir() if d.is_dir()
# })
# # Compile xacro → URDF string, resolve all mesh file path
# if isinstance(file, (str, Path)):
# doc = XacroDoc.from_file(xacro_root / file)
# else:
# doc = XacroDoc.from_string(file.read())
# urdf_str = doc.to_urdf_string()
# urdf = URDF.loadstr(urdf_str, None)
# # links, name, urdf_string, urdf_filepath = self.URDF_read(
# # "trossen_descriptions/urdf/vx300.urdf.xacro"
# # )
# super().__init__(
# urdf.elinks,
# name=urdf.name,
# manufacturer=manufacturer,
# # urdf_string=urdf_string,
# # urdf_filepath=urdf_filepath,
# )
[docs]
@classmethod
def URDF(cls, file, gripper=None, manufacturer=None):
"""Deprecated. Use :class:`~roboticstoolbox.models.URDF.URDFRobot` as a
base class, or call ``URDF_read()`` from
``roboticstoolbox.models.URDF.URDFRobot`` and construct ``Robot``
directly."""
import warnings
warnings.warn(
"Robot.URDF() is deprecated. "
"Subclass roboticstoolbox.models.URDF.URDFRobot, or call "
"URDF_read() from that module and construct Robot directly.",
DeprecationWarning,
stacklevel=2,
)
from roboticstoolbox.models.URDF.URDFRobot import URDF_file
elinks, name, _ = URDF_file(file)
gripper_link = None
if isinstance(gripper, int):
gripper_link = elinks[gripper]
elif isinstance(gripper, str):
for link in elinks:
if link.name == gripper:
gripper_link = link
break
else:
raise ValueError(f"no link named '{gripper}'")
return cls(
elinks,
name=name,
manufacturer=manufacturer or "",
gripper_links=gripper_link,
)
# # --------------------------------------------------------------------- #
# # --------- Utility Methods ------------------------------------------- #
# # --------------------------------------------------------------------- #
# def showgraph(self, display_graph: bool = True, **kwargs) -> Union[None, str]:
# """
# Display a link transform graph in browser
# ``robot.showgraph()`` displays a graph of the robot's link frames
# and the ETS between them. It uses GraphViz dot.
# The nodes are:
# - Base is shown as a grey square. This is the world frame origin,
# but can be changed using the ``base`` attribute of the robot.
# - Link frames are indicated by circles
# - ETS transforms are indicated by rounded boxes
# The edges are:
# - an arrow if `jtype` is False or the joint is fixed
# - an arrow with a round head if `jtype` is True and the joint is
# revolute
# - an arrow with a box head if `jtype` is True and the joint is
# prismatic
# Edge labels or nodes in blue have a fixed transformation to the
# preceding link.
# Parameters
# ----------
# display_graph
# Open the graph in a browser if True. Otherwise will return the
# file path
# etsbox
# Put the link ETS in a box, otherwise an edge label
# jtype
# Arrowhead to node indicates revolute or prismatic type
# static
# Show static joints in blue and bold
# Examples
# --------
# >>> import roboticstoolbox as rtb
# >>> panda = rtb.models.URDF.Panda()
# >>> panda.showgraph()
# .. image:: ../figs/panda-graph.svg
# :width: 600
# See Also
# --------
# :func:`dotfile`
# """
# # Lazy import
# import tempfile
# import subprocess
# import webbrowser
# # create the temporary dotfile
# dotfile = tempfile.TemporaryFile(mode="w")
# self.dotfile(dotfile, **kwargs)
# # rewind the dot file, create PDF file in the filesystem, run dot
# dotfile.seek(0)
# pdffile = tempfile.NamedTemporaryFile(suffix=".pdf", delete=False)
# subprocess.run("dot -Tpdf", shell=True, stdin=dotfile, stdout=pdffile)
# # open the PDF file in browser (hopefully portable), then cleanup
# if display_graph: # pragma nocover
# webbrowser.open(f"file://{pdffile.name}")
# else:
# return pdffile.name
# def dotfile(
# self,
# filename: Union[str, IO[str]],
# etsbox: bool = False,
# ets: L["full", "brief"] = "full",
# jtype: bool = False,
# static: bool = True,
# ):
# """
# Write a link transform graph as a GraphViz dot file
# The file can be processed using dot:
# % dot -Tpng -o out.png dotfile.dot
# The nodes are:
# - Base is shown as a grey square. This is the world frame origin,
# but can be changed using the ``base`` attribute of the robot.
# - Link frames are indicated by circles
# - ETS transforms are indicated by rounded boxes
# The edges are:
# - an arrow if `jtype` is False or the joint is fixed
# - an arrow with a round head if `jtype` is True and the joint is
# revolute
# - an arrow with a box head if `jtype` is True and the joint is
# prismatic
# Edge labels or nodes in blue have a fixed transformation to the
# preceding link.
# Note
# ----
# If ``filename`` is a file object then the file will *not*
# be closed after the GraphViz model is written.
# Parameters
# ----------
# file
# Name of file to write to
# etsbox
# Put the link ETS in a box, otherwise an edge label
# ets
# Display the full ets with "full" or a brief version with "brief"
# jtype
# Arrowhead to node indicates revolute or prismatic type
# static
# Show static joints in blue and bold
# See Also
# --------
# :func:`showgraph`
# """
# if isinstance(filename, str):
# file = open(filename, "w")
# else:
# file = filename
# header = r"""digraph G {
# graph [rankdir=LR];
# """
# def draw_edge(link, etsbox, jtype, static):
# # draw the edge
# if jtype:
# if link.isprismatic:
# edge_options = 'arrowhead="box", arrowtail="inv", dir="both"'
# elif link.isrevolute:
# edge_options = 'arrowhead="dot", arrowtail="inv", dir="both"'
# else:
# edge_options = 'arrowhead="normal"'
# else:
# edge_options = 'arrowhead="normal"'
# if link.parent is None:
# parent = "BASE"
# else:
# parent = link.parent.name
# if etsbox:
# # put the ets fragment in a box
# if not link.isjoint and static:
# node_options = ', fontcolor="blue"'
# else:
# node_options = ""
# try:
# file.write(
# ' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format(
# link.name,
# link.ets.__str__(q=f"q{link.jindex}"),
# node_options,
# )
# )
# except UnicodeEncodeError: # pragma nocover
# file.write(
# ' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format(
# link.name,
# link.ets.__str__(q=f"q{link.jindex}")
# .encode("ascii", "ignore")
# .decode("ascii"),
# node_options,
# )
# )
# file.write(" {} -> {}_ets;\n".format(parent, link.name))
# file.write(
# " {}_ets -> {} [{}];\n".format(link.name, link.name, edge_options)
# )
# else:
# # put the ets fragment as an edge label
# if not link.isjoint and static:
# edge_options += 'fontcolor="blue"'
# if ets == "full":
# estr = link.ets.__str__(q=f"q{link.jindex}")
# elif ets == "brief":
# if link.jindex is None:
# estr = ""
# else:
# estr = f"...q{link.jindex}"
# else:
# return
# try:
# file.write(
# ' {} -> {} [label="{}", {}];\n'.format(
# parent,
# link.name,
# estr,
# edge_options,
# )
# )
# except UnicodeEncodeError: # pragma nocover
# file.write(
# ' {} -> {} [label="{}", {}];\n'.format(
# parent,
# link.name,
# estr.encode("ascii", "ignore").decode("ascii"),
# edge_options,
# )
# )
# file.write(header)
# # add the base link
# file.write(" BASE [shape=square, style=filled, fillcolor=gray]\n")
# # add the links
# for link in self:
# # draw the link frame node (circle) or ee node (doublecircle)
# if link in self.ee_links:
# # end-effector
# node_options = 'shape="doublecircle", color="blue", fontcolor="blue"'
# else:
# node_options = 'shape="circle"'
# file.write(" {} [{}];\n".format(link.name, node_options))
# draw_edge(link, etsbox, jtype, static)
# for gripper in self.grippers:
# for link in gripper.links:
# file.write(" {} [shape=cds];\n".format(link.name))
# draw_edge(link, etsbox, jtype, static)
# file.write("}\n")
# if isinstance(filename, str):
# file.close()
# --------------------------------------------------------------------- #
# --------- Kinematic Methods ----------------------------------------- #
# --------------------------------------------------------------------- #
@property
def reach(self) -> float:
r"""
Reach of the robot
:returns: Maximum reach of the robot
:rtype: float
A conservative estimate of the reach of the robot. It is computed as
the sum of the translational ETs that define the link transform.
Computed on the first access. If kinematic parameters
subsequently change this will not be reflected.
.. rubric:: Notes
- Probably an overestimate of reach
- Used by numerical inverse kinematics to scale translational
error.
- For a prismatic joint, uses ``qlim`` if it is set
"""
# TODO
# This should be a start, end method and compute the reach based on the
# given ets. Then use an lru_cache to speed up return
if self._reach is None:
d_all = []
for link in self.ee_links:
d = 0
while True:
for et in link.ets:
if et.istranslation:
if et.isjoint:
# the length of a prismatic joint depends on the
# joint limits. They might be set in the ET
# or in the Link depending on how the robot
# was constructed
if link.qlim is not None:
d += max(link.qlim)
elif et.qlim is not None: # pragma nocover
d += max(et.qlim)
else:
d += abs(et.param)
link = link.parent
if link is None or isinstance(link, str):
d_all.append(d)
break
self._reach = max(d_all)
return self._reach
[docs]
def fkine_all(self, q: ArrayLike) -> SE3:
"""
Compute the pose of every link frame
:param q: The joint configuration
:returns: Pose of all links
``T = robot.fkine_all(q)`` is an SE3 instance with ``robot.nlinks +
1`` values:
- ``T[0]`` is the base transform
- ``T[i]`` is the pose of link whose ``number`` is ``i``
.. rubric:: References
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
"""
q = getvector(q)
Tbase = SE3(self.base) # add base, also sets the type
linkframes = Tbase.__class__.Alloc(self.nlinks + 1)
linkframes[0] = Tbase
def recurse(Tall, Tparent, q, link):
# if joint??
T = Tparent
while True:
T *= SE3(link.A(q[link.jindex]))
Tall[link.number] = T
if link.nchildren == 0:
# no children
return
elif link.nchildren == 1:
# one child
if link in self.ee_links: # pragma nocover
# this link is an end-effector, go no further
return
link = link.children[0]
continue
else:
# multiple children
for child in link.children:
recurse(Tall, T, q, child)
return
recurse(linkframes, Tbase, q, self.links[0])
return linkframes
@overload
def manipulability(
self,
q: ArrayLike = ...,
J: None = None,
end: str | Link | Gripper | None = None,
start: str | Link | Gripper | None = None,
method: L[
"yoshikawa", "asada", "minsingular", "invcondition"
] = "yoshikawa",
axes: L["all", "trans", "rot"] | list[bool] = "all",
**kwargs,
) -> float | NDArray: # pragma nocover
...
@overload
def manipulability(
self,
q: None = None,
J: NDArray = ...,
end: str | Link | Gripper | None = None,
start: str | Link | Gripper | None = None,
method: L[
"yoshikawa", "asada", "minsingular", "invcondition"
] = "yoshikawa",
axes: L["all", "trans", "rot"] | list[bool] = "all",
**kwargs,
) -> float | NDArray: # pragma nocover
...
[docs]
def manipulability(
self,
q=None,
J=None,
end: str | Link | Gripper | None = None,
start: str | Link | Gripper | None = None,
method: L[
"yoshikawa", "asada", "minsingular", "invcondition"
] = "yoshikawa",
axes: L["all", "trans", "rot"] | list[bool] = "all",
**kwargs,
):
"""
Manipulability measure
:param q: Joint coordinates, one of J or q required
:param J: Jacobian in base frame if already computed, one of J or q required
:param method: method to use, "yoshikawa" (default), "invcondition",
"minsingular" or "asada"
:param axes: Task space axes to consider: "all" [default], "trans", or "rot"
:returns: manipulability index
``manipulability(q)`` is the scalar manipulability index
for the robot at the joint configuration ``q``. It indicates
dexterity, that is, how well conditioned the robot is for motion
with respect to the 6 degrees of Cartesian motion. The values is
zero if the robot is at a singularity.
Various measures are supported:
- ``"yoshikawa"`` -- volume of the velocity ellipsoid, *distance*
from singularity [Yoshikawa85]_
- ``"invcondition"`` -- inverse condition number of Jacobian,
isotropy of the velocity ellipsoid [Klein87]_
- ``"minsingular"`` -- minimum singular value of the Jacobian,
*distance* from singularity [Klein87]_
- ``"asada"`` -- isotropy of the task-space acceleration ellipsoid
which is a function of the Cartesian inertia matrix which depends
on the inertial parameters [Asada83]_
**Trajectory operation**:
If ``q`` is a matrix (m,n) then the result (m,) is a vector of
manipulability indices for each joint configuration specified by a row
of ``q``.
.. rubric:: Notes
- Invokes the ``jacob0`` method of the robot if ``J`` is not passed
- The "all" option includes rotational and translational
dexterity, but this involves adding different units. It can be
more useful to look at the translational and rotational
manipulability separately.
- Examples in the RVC book (1st edition) can be replicated by
using the "all" option
- Asada's measure requires inertial a robot model with inertial
parameters.
.. rubric:: References
.. [Yoshikawa85] Manipulability of Robotic Mechanisms. Yoshikawa T.,
The International Journal of Robotics Research.
1985;4(2):3-9. doi:10.1177/027836498500400201
.. [Asada83] A geometrical representation of manipulator dynamics and
its application to arm design, H. Asada,
Journal of Dynamic Systems, Measurement, and Control,
vol. 105, p. 131, 1983.
.. [Klein87] Dexterity Measures for the Design and Control of
Kinematically Redundant Manipulators. Klein CA, Blaho BE.
The International Journal of Robotics Research.
1987;6(2):72-83. doi:10.1177/027836498700600206
- Robotics, Vision & Control, Chap 8, P. Corke, Springer 2011.
.. versionchanged:: 1.0.3
Removed 'both' option for axes, added a custom list option.
"""
ets = self.ets(end, start)
axes_list: list[bool] = []
if isinstance(axes, list):
axes_list = axes
elif axes == "all":
axes_list = [True, True, True, True, True, True]
elif axes.startswith("trans"):
axes_list = [True, True, True, False, False, False]
elif axes.startswith("rot"):
axes_list = [False, False, False, True, True, True]
elif axes == "both":
return (
self.manipulability(
q=q, J=J, end=end, start=start, method=method, axes="trans"
),
self.manipulability(
q=q, J=J, end=end, start=start, method=method, axes="rot"
),
)
else:
raise ValueError("axes must be all, trans, rot or both")
def yoshikawa(robot, J, q, axes_list):
J = J[axes_list, :]
if J.shape[0] == J.shape[1]:
# simplified case for square matrix
return abs(np.linalg.det(J))
else:
m2 = np.linalg.det(J @ J.T)
return np.sqrt(abs(m2))
def condition(robot, J, q, axes_list):
J = J[axes_list, :]
# return 1/cond(J)
return 1 / np.linalg.cond(J)
def minsingular(robot, J, q, axes_list):
J = J[axes_list, :]
s = np.linalg.svd(J, compute_uv=False)
# return last/smallest singular value of J
return s[-1]
def asada(robot, J, q, axes_list):
# dof = np.sum(axes_list)
if np.linalg.matrix_rank(J) < 6:
return 0
Ji = np.linalg.pinv(J)
Mx = Ji.T @ robot.inertia(q) @ Ji
d = np.where(axes_list)[0]
Mx = Mx[d]
Mx = Mx[:, d.tolist()]
e, _ = np.linalg.eig(Mx)
return np.min(e) / np.max(e)
# choose the handler function
if method.lower().startswith("yoshi"):
mfunc = yoshikawa
elif method.lower().startswith("invc"):
mfunc = condition
elif method.lower().startswith("mins"):
mfunc = minsingular
elif method.lower().startswith("asa"):
mfunc = asada
else:
raise ValueError("Invalid method chosen")
# Calculate manipulability based on supplied Jacobian
if J is not None:
w = [mfunc(self, J, q, axes_list)]
# Otherwise use the q vector/matrix
else:
if q is None:
raise ValueError("Either J or q must be supplied")
q = getmatrix(q, (None, self.n))
q = np.array(getmatrix(q, (None, self.n)))
w = np.zeros(q.shape[0])
for k, qk in enumerate(q):
Jk = ets.jacob0(qk)
w[k] = mfunc(self, Jk, qk, axes_list)
if len(w) == 1:
return w[0]
else:
return w
[docs]
def jtraj(
self,
T1: NDArray | SE3,
T2: NDArray | SE3,
t: NDArray | int,
**kwargs,
):
"""
Joint-space trajectory between SE(3) poses
:param T1: initial end-effector pose
:param T2: final end-effector pose
:param t: time vector or number of steps
:param kwargs: arguments passed to the IK solver
:returns: trajectory
The initial and final poses are mapped to joint space using inverse
kinematics:
- if the object has an analytic solution ``ikine_a`` that will be used,
- otherwise the general numerical algorithm ``ikine_lm`` will be used.
``traj = obot.jtraj(T1, T2, t)`` is a trajectory object whose
attribute ``traj.q`` is a row-wise joint-space trajectory.
"""
if hasattr(self, "ikine_a"):
ik = self.ikine_a # type: ignore
else:
ik = self.ikine_LM
q1 = ik(T1, **kwargs)
q2 = ik(T2, **kwargs)
return rtb.jtraj(q1.q, q2.q, t)
@overload
def jacob0_dot(
self,
q: ArrayLike,
qd: ArrayLike,
J0: None = None,
representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None,
) -> NDArray: # pragma no cover
...
@overload
def jacob0_dot(
self,
q: None,
qd: ArrayLike,
J0: NDArray = ...,
representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None,
) -> NDArray: # pragma no cover
...
[docs]
def jacob0_dot(
self,
q,
qd: ArrayLike,
J0=None,
representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None,
):
r"""
Derivative of Jacobian
:param q: The joint configuration of the robot
:param qd: The joint velocity of the robot
:param J0: Jacobian in {0} frame
:param representation: angular representation
:returns: The derivative of the manipulator Jacobian
``robot.jacob_dot(q, qd)`` computes the rate of change of the
Jacobian elements
.. math::
\dmat{J} = \frac{d \mat{J}}{d \vec{q}} \frac{d \vec{q}}{dt}
where the first term is the rank-3 Hessian.
If ``J0`` is already calculated for the joint
coordinates ``q`` it can be passed in to to save computation time.
It is computed as the mode-3 product of the Hessian tensor and the
velocity vector.
The derivative of an analytical Jacobian can be obtained by setting
``representation`` as
.. list-table::
:header-rows: 1
* - ``representation``
- Rotational representation
* - ``'rpy/xyz'``
- RPY angular rates in XYZ order
* - ``'rpy/zyx'``
- RPY angular rates in ZYX order
* - ``'eul'``
- Euler angular rates in ZYZ order
* - ``'exp'``
- exponential coordinate rates
.. rubric:: References
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
See Also
--------
:func:`jacob0`
:func:`hessian0`
"""
qd = np.array(qd)
if representation is None:
if J0 is None:
J0 = self.jacob0(q)
H = self.hessian0(q, J0=J0)
else:
# # determine analytic rotation
# T = self.fkine(q).A
# gamma = smb.r2x(smb.t2r(T), representation=representation)
# # get transformation angular velocity to analytic velocity
# Ai = smb.rotvelxform(
# gamma, representation=representation, inverse=True, full=True
# )
# # get analytic rate from joint rates
# omega = J0[3:, :] @ qd
# gamma_dot = Ai[3:, 3:] @ omega
# Ai_dot = smb.rotvelxform_inv_dot(gamma, gamma_dot, full=True)
# Ai_dot = sp.linalg.block_diag(np.zeros((3, 3)), Ai_dot)
# Jd = Ai_dot @ J0 + Ai @ Jd
# not actually sure this can be written in closed form
H = smb.numhess(
lambda q: self.jacob0_analytical(q, representation=representation), q
)
# Jd = Ai @ Jd
# return Jd
return np.tensordot(H, qd, (0, 0))
@overload
def jacobm(
self,
q: ArrayLike = ...,
J: None = None,
H: None = None,
end: str | Link | Gripper | None = None,
start: str | Link | Gripper | None = None,
axes: L["all", "trans", "rot"] | list[bool] = "all",
) -> NDArray: # pragma no cover
...
@overload
def jacobm(
self,
q: None = None,
J: NDArray = ...,
H: NDArray = ...,
end: str | Link | Gripper | None = None,
start: str | Link | Gripper | None = None,
axes: L["all", "trans", "rot"] | list[bool] = "all",
) -> NDArray: # pragma no cover
...
[docs]
def jacobm(
self,
q=None,
J=None,
H=None,
end: str | Link | Gripper | None = None,
start: str | Link | Gripper | None = None,
axes: L["all", "trans", "rot"] | list[bool] = "all",
) -> NDArray:
r"""
The manipulability Jacobian
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:param J: The manipulator Jacobian in any frame
:param H: The manipulator Hessian in any frame
:param end: the final link or Gripper which the Hessian represents
:param start: the first link which the Hessian represents
:returns: The manipulability Jacobian
This measure relates the rate of change of the manipulability to the
joint velocities of the robot. One of J or q is required. Supply J
and H if already calculated to save computation time.
Yoshikawa's manipulability measure
.. math::
m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}
This method returns its Jacobian with respect to configuration
.. math::
\frac{\partial m(\vec{q})}{\partial \vec{q}}
.. rubric:: References
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
"""
end, start, _ = self._get_limit_links(end, start)
#
if not isinstance(axes, list):
if axes.startswith("all"):
axes = [True, True, True, True, True, True]
elif axes.startswith("trans"):
axes = [True, True, True, False, False, False]
elif axes.startswith("rot"):
axes = [False, False, False, True, True, True]
else:
raise ValueError("axes must be all, trans or rot")
if J is None:
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, self.n)
J = self.jacob0(q, start=start, end=end)
else:
verifymatrix(J, (6, self.n))
n = J.shape[1]
if H is None:
H = self.hessian0(J0=J, start=start, end=end)
# else:
# verifymatrix(H, (6, self.n, self.n))
elif not isinstance(H, np.ndarray):
raise TypeError("Hessian must be numpy array of shape 6xnxn")
elif H.shape != (6, self.n, self.n):
raise ValueError("Hessian must be numpy array of shape 6xnxn")
manipulability = self.manipulability(
q, # type: ignore[arg-type]
J=J,
start=start,
end=end,
axes=axes, # type: ignore
)
J = J[axes, :] # type: ignore
H = H[:, axes, :] # type: ignore
b = np.linalg.inv(J @ np.transpose(J))
Jm = np.zeros((n, 1))
for i in range(n):
c = J @ np.transpose(H[i, :, :])
Jm[i, 0] = manipulability * np.transpose(c.flatten("F")) @ b.flatten("F")
return Jm
# --------------------------------------------------------------------- #
# --------- Collision Methods ----------------------------------------- #
# --------------------------------------------------------------------- #
[docs]
def closest_point(
self, q: ArrayLike, shape: Shape, inf_dist: float = 1.0, skip: bool = False
) -> tuple[int | None, NDArray | None, NDArray | None]:
"""
Find the closest point between robot and shape
:param shape: The shape to compare distance to
:param inf_dist: The minimum distance within which to consider the shape
:param skip: Skip setting all shape transforms based on q, use this
option if using this method in conjuction with Swift to save time
:returns: tuple of (distance, point on robot, point on shape)
``closest_point(shape, inf_dist)`` returns the minimum euclidean
distance between this robot and shape, provided it is less than
inf_dist. It will also return the points on self and shape in the
world frame which connect the line of length distance between the
shapes. If the distance is negative then the shapes are collided.
"""
if not skip:
self._update_link_tf(q)
self._propogate_scene_tree()
shape._propogate_scene_tree()
d = 10000
p1 = None
p2 = None
for link in self.links:
td, tp1, tp2 = link.closest_point(shape, inf_dist, skip=True)
if td is not None and td < d:
d = td
p1 = tp1
p2 = tp2
if d == 10000:
d = None
return d, p1, p2
[docs]
def iscollided(self, q, shape: Shape, skip: bool = False) -> bool:
"""
Check if the robot is in collision with a shape
:param shape: The shape to compare distance to
:param skip: Skip setting all shape transforms based on q, use this
option if using this method in conjuction with Swift to save time
:returns: True if shapes have collided
``iscollided(shape)`` checks if this robot and shape have collided
"""
if not skip:
self._update_link_tf(q)
self._propogate_scene_tree()
shape._propogate_scene_tree()
for link in self.links:
if link.iscollided(shape, skip=True):
return True
if isinstance(self, rtb.Robot):
for gripper in self.grippers:
for link in gripper.links:
if link.iscollided(shape, skip=True):
return True
return False
[docs]
def collided(self, q, shape: Shape, skip: bool = False) -> bool:
"""
Check if the robot is in collision with a shape
:param shape: The shape to compare distance to
:param skip: Skip setting all shape transforms based on q, use this
option if using this method in conjuction with Swift to save time
:returns: True if shapes have collided
``collided(shape)`` checks if this robot and shape have collided
"""
warn("method collided is deprecated, use iscollided instead", FutureWarning)
return self.iscollided(q, shape, skip=skip)
# --------------------------------------------------------------------- #
# --------- Constraint Methods ---------------------------------------- #
# --------------------------------------------------------------------- #
[docs]
def joint_velocity_damper(
self,
q=None,
ps: float = 0.05,
pi: float = 0.1,
n: int | None = None,
gain: float = 1.0,
) -> tuple[NDArray, NDArray]:
"""
Compute the joint velocity damper for QP motion control
:param ps: The minimum angle (in radians) in which the joint is
allowed to approach to its limit
:param pi: The influence angle (in radians) in which the velocity
damper becomes active
:param n: The number of joints to consider. Defaults to all joints
:param gain: The gain for the velocity damper
:returns: tuple of (Ain, Bin) inequality constraint matrices for an optimiser
Formulates an inequality contraint which, when optimised for will
make it impossible for the robot to run into joint limits. Requires
the joint limits of the robot to be specified. See examples/mmc.py
for use case.
"""
if n is None:
n = self.n
if q is None:
q = np.copy(self.q)
Ain = np.zeros((n, n))
Bin = np.zeros(n)
for i in range(n):
if self.q[i] - self.qlim[0, i] <= pi:
Bin[i] = -gain * (((self.qlim[0, i] - q[i]) + ps) / (pi - ps))
Ain[i, i] = -1
if self.qlim[1, i] - self.q[i] <= pi:
Bin[i] = gain * ((self.qlim[1, i] - q[i]) - ps) / (pi - ps)
Ain[i, i] = 1
return Ain, Bin
[docs]
def link_collision_damper(
self,
shape: CollisionShape,
q: ArrayLike,
di: float = 0.3,
ds: float = 0.05,
xi: float = 1.0,
end: Link | None = None,
start: Link | None = None,
collision_list: list[Shape] | None = None,
):
"""
Compute a collision constrain for QP motion control
:param ds: The minimum distance in which a joint is allowed to
approach the collision object shape
:param di: The influence distance in which the velocity
damper becomes active
:param xi: The gain for the velocity damper
:param end: The end link of the robot to consider
:param start: The start link of the robot to consider
:param collision_list: A list of shapes to consider for collision
:returns: tuple of (Ain, Bin) inequality constraint matrices for an optimiser
Formulates an inequality contraint which, when optimised for will
make it impossible for the robot to run into a collision. Requires
See examples/neo.py for use case.
"""
end, start, _ = self._get_limit_links(start=start, end=end)
links, n, _ = self.get_path(start=start, end=end)
q = np.array(q)
j = 0
Ain = None
bin = None
def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray):
d, wTlp, wTcp = link_col.closest_point(shape, di)
if d is not None and wTlp is not None and wTcp is not None:
lpTcp = -wTlp + wTcp
norm = lpTcp / d
norm_h = np.expand_dims(
np.concatenate((norm, [0.0, 0.0, 0.0])),
axis=0, # type: ignore
)
# tool = (self.fkine(q, end=link).inv() * SE3(wTlp)).A[:3, 3]
# Je = self.jacob0(q, end=link, tool=tool)
# Je[:3, :] = self._T[:3, :3] @ Je[:3, :]
# n_dim = Je.shape[1]
# dp = norm_h @ shape.v
# l_Ain = zeros((1, self.n))
Je = self.jacobe(q, start=start, end=link, tool=link_col.T)
n_dim = Je.shape[1]
dp = norm_h @ shape.v
l_Ain = np.zeros((1, n))
l_Ain[0, :n_dim] = 1 * norm_h @ Je
l_bin = (xi * (d - ds) / (di - ds)) + dp
else: # pragma nocover
l_Ain = None
l_bin = None
return l_Ain, l_bin
for link in links:
if link.isjoint:
j += 1
if collision_list is None:
col_list = link.collision
for c in col_list:
pass
else:
col_list = [collision_list[j - 1]] # pragma nocover
for link_col in col_list:
l_Ain, l_bin = indiv_calculation(link, link_col, q) # type: ignore
if l_Ain is not None and l_bin is not None:
if Ain is None:
Ain = l_Ain
else:
Ain = np.concatenate((Ain, l_Ain))
if bin is None:
bin = np.array(l_bin)
else:
bin = np.concatenate((bin, l_bin))
return Ain, bin
[docs]
def vision_collision_damper(
self,
shape: CollisionShape,
camera: Robot | SE3 | None = None,
camera_n: int = 0,
q=None,
di=0.3,
ds=0.05,
xi=1.0,
end=None,
start=None,
collision_list=None,
): # pragma nocover
"""
Compute a vision collision constrain for QP motion control
:param camera: The camera link, either as a robotic link or SE3 pose
:param camera_n: Degrees of freedom of the camera link
:param ds: The minimum distance in which a joint is allowed to
approach the collision object shape
:param di: The influence distance in which the velocity
damper becomes active
:param xi: The gain for the velocity damper
:param end: The end link of the robot to consider
:param start: The start link of the robot to consider
:param collision_list: A list of shapes to consider for collision
:returns: tuple of (Ain, Bin) inequality constraint matrices for an optimiser
Formulates an inequality contraint which, when optimised for will
make it impossible for the robot to run into a line of sight.
See examples/fetch_vision.py for use case.
"""
end, start, _ = self._get_limit_links(start=start, end=end)
links, n, _ = self.get_path(start=start, end=end)
q = np.array(q)
j = 0
Ain = None
bin = None
def rotation_between_vectors(a, b):
a = a / np.linalg.norm(a)
b = b / np.linalg.norm(b)
angle = np.arccos(np.dot(a, b))
axis = np.cross(a, b)
return SE3.AngleAxis(angle, axis)
if isinstance(camera, rtb.BaseRobot):
wTcp = cast(NDArray, camera.fkine(camera.q).A)[:3, 3]
elif isinstance(camera, SE3):
wTcp = camera.t
else:
raise TypeError("Camera must be a robotic link or SE3 pose")
wTtp = shape.T[:3, -1]
# Create line of sight object
los_mid = SE3((wTcp + wTtp) / 2)
los_orientation = rotation_between_vectors(
np.array([0.0, 0.0, 1.0]),
wTcp - wTtp, # type: ignore
)
los = Cylinder(
radius=0.001,
length=np.linalg.norm(wTcp - wTtp), # type: ignore
base=(los_mid * los_orientation),
)
def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray):
d, wTlp, wTvp = link_col.closest_point(los, di)
if d is not None and wTlp is not None and wTvp is not None:
lpTvp = -wTlp + wTvp
norm = lpTvp / d
norm_h = np.expand_dims(np.concatenate((norm, [0.0, 0.0, 0.0])), axis=0) # type: ignore
tool = SE3(
(np.linalg.inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3]
)
Je = self.jacob0(q, end=link, tool=cast(NDArray, tool.A))
Je[:3, :] = self._T[:3, :3] @ Je[:3, :]
n_dim = Je.shape[1]
if isinstance(camera, "Robot"):
Jv = camera.jacob0(camera.q)
Jv[:3, :] = self._T[:3, :3] @ Jv[:3, :]
Jv *= np.linalg.norm(wTvp - shape.T[:3, -1]) / los.length # type: ignore
dpc = norm_h @ Jv
dpc = np.concatenate(
(
dpc[0, :-camera_n],
np.zeros(self.n - (camera.n - camera_n)),
dpc[0, -camera_n:],
)
)
else:
dpc = np.zeros((1, self.n + camera_n))
dpt = norm_h @ shape.v
dpt *= np.linalg.norm(wTvp - wTcp) / los.length # type: ignore
l_Ain = np.zeros((1, self.n + camera_n))
l_Ain[0, :n_dim] = norm_h @ Je
l_Ain -= dpc
l_bin = (xi * (d - ds) / (di - ds)) + dpt
else:
l_Ain = None
l_bin = None
return l_Ain, l_bin
for link in links:
if link.isjoint:
j += 1
if collision_list is None:
col_list = link.collision
else:
col_list = collision_list[j - 1]
for link_col in col_list:
l_Ain, l_bin = indiv_calculation(link, link_col, q)
if l_Ain is not None and l_bin is not None:
if Ain is None:
Ain = l_Ain
else:
Ain = np.concatenate((Ain, l_Ain))
if bin is None:
bin = np.array(l_bin)
else:
bin = np.concatenate((bin, l_bin))
return Ain, bin
# --------------------------------------------------------------------- #
# --------- Dynamics Methods ------------------------------------------ #
# --------------------------------------------------------------------- #
[docs]
def rne(
self,
q: NDArray,
qd: NDArray,
qdd: NDArray,
symbolic: bool = False,
gravity: ArrayLike | None = None,
):
"""
Compute inverse dynamics via recursive Newton-Euler formulation
:param q: Joint coordinates
:param qd: Joint velocity
:param qdd: Joint acceleration
:param symbolic: If True, supports symbolic expressions
:param gravity: Gravitational acceleration, defaults to attribute of self
:returns: Joint force/torques
``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).
.. rubric:: Notes
- This version supports symbolic model parameters
- Verified against MATLAB code
"""
n = self.n
# n = len(self.links)
# allocate intermediate variables
Xup = SE3.Alloc(n)
v = SpatialVelocity.Alloc(n)
a = SpatialAcceleration.Alloc(n)
f = SpatialForce.Alloc(n)
I = SpatialInertia.Alloc(n)
s = [] # joint motion subspace
# Handle trajectory case
q = getmatrix(q, (None, None))
qd = getmatrix(qd, (None, None))
qdd = getmatrix(qdd, (None, None))
l, _ = q.shape
if symbolic: # pragma: nocover
Q = np.empty((l, n), dtype="O") # joint torque/force
else:
Q = np.empty((l, n)) # joint torque/force
link_groups: list[list[int]] = []
# Group links together based on whether they are joints or not
# Static links are grouped with the first joint encountered
current_group = []
for i, link in enumerate(self.links):
current_group.append(i)
# Break after adding the first link
if link.isjoint:
link_groups.append(current_group)
current_group = []
# Make some intermediate variables
for i, group in enumerate(link_groups):
I_int = SpatialInertia()
for idx in group:
link = self.links[idx]
I_int = I_int + SpatialInertia(m=link.m, r=link.r)
if link.v is not None:
s.append(link.v.s) # type: ignore[union-attr]
I[i] = I_int
if gravity is None:
a_grav = -SpatialAcceleration(self.gravity)
else: # pragma nocover
a_grav = -SpatialAcceleration(gravity)
# For the following, v, a, f, I, s, Xup are all lists of length n
# where the indices correspond to the index of the group within
# link_groups
# As always, q, qd, qdd are lists of length n, where indices correspond
# to the jindex of the joint, which will be the last link in the group
# within link_groups
for k in range(l):
qk = q[k, :]
qdk = qd[k, :]
qddk = qdd[k, :]
# forward recursion
for j, group in enumerate(link_groups):
# The joint is the last link in the group
joint = self.links[group[-1]]
jindex = joint.jindex
vJ = SpatialVelocity(s[j] * qdk[jindex])
# transform from parent(j) to j
# Xup_int = SE3()
first_element = True
for idx in group:
link = self.links[idx]
if link.isjoint and link.jindex is not None:
if first_element:
Xup_int = SE3(link.A(qk[link.jindex]))
first_element = False
else:
Xup_int = Xup_int * SE3(link.A(qk[link.jindex]))
else:
if first_element:
Xup_int = SE3(link.A())
first_element = False
else:
Xup_int = Xup_int * SE3(link.A())
Xup[j] = Xup_int.inv() # type: ignore[union-attr]
# The first link in the group
first_link = self.links[group[0]]
if first_link.parent is None:
v[j] = vJ
a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[jindex])
else:
# The index of `link`s parent within self.links
parent_idx = self.links.index(first_link.parent)
# The index of the group that the parent link is in
group_idx = [
i for i, group in enumerate(link_groups) if parent_idx in group
][0]
v[j] = Xup[j] * v[group_idx] + vJ
a[j] = (
Xup[j] * a[group_idx]
+ SpatialAcceleration(s[j] * qddk[jindex])
+ v[j] @ vJ
)
f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
# Backward recursion
for j in reversed(range(n)):
group = link_groups[j]
joint = self.links[group[-1]]
first_link = self.links[group[0]]
# link = self.links[j]
# next line could be dot(), but fails for symbolic arguments
Q[k, j] = sum(f[j].A * s[j])
if first_link.parent is not None:
# The index of `link`s parent within self.links
parent_idx = self.links.index(first_link.parent)
# The index of the group that the parent link is in
group_idx = [
i for i, group in enumerate(link_groups) if parent_idx in group
][0]
f[group_idx] = f[group_idx] + Xup[j] * f[j]
# The current Q has the length equal to the number of links within the robot
# rather than the number of joints. We need to remove the static links
# from the Q array
# joint_idx = [i for i, link in enumerate(self.links) if link.isjoint]
if l == 1:
return Q[0]
else: # pragma nocover
return Q
# ============================================================================= #
# ================= URDFRobot Class =========================================== #
# ============================================================================= #
# class URDFRobot(Robot):
# """A Robot subclass that initialises from a URDF or xacro file.
# Model classes should subclass this and call ``super().__init__()`` with
# the path and manufacturer::
# class vx300(URDFRobot):
# def __init__(self):
# super().__init__(
# "trossen_descriptions/urdf/vx300.urdf.xacro",
# manufacturer="Trossen Robotics",
# )
# """
# def __init__(
# self,
# urdf_path: "str | Path",
# manufacturer: str = "",
# gripper_link_index: "int | None" = None,
# **kwargs,
# ):
# elinks, name = self.URDF_file(urdf_path)
# if gripper_link_index is not None:
# kwargs["gripper_links"] = elinks[gripper_link_index]
# super().__init__(elinks, name=name, manufacturer=manufacturer, **kwargs)
# ============================================================================= #
# ================= Robot2 Class ============================================== #
# ============================================================================= #
[docs]
class Robot2(BaseRobot[Link2]):
def __init__(self, arg, **kwargs):
if isinstance(arg, ETS2):
# we're passed an ETS string
links = []
# chop it up into segments, a link frame after every joint
# split()'s default "last" method folds any base content into
# the first segment, so `base` is always empty and dropped;
# `gripper` holds trailing constant content, if any, and
# becomes one extra static (non-joint) link.
_, *segs, gripper = arg.split()
if gripper:
segs.append(gripper)
parent = None
for j, ets_j in enumerate(segs):
elink = Link2(ETS2(ets_j), parent=parent, name=f"link{j:d}")
parent = elink
if (
elink.qlim is None
and elink.v is not None
and elink.v.qlim is not None
): # pragma nocover
elink.qlim = elink.v.qlim
links.append(elink)
elif smb.islistof(arg, Link2):
links = arg
else: # pragma nocover
raise TypeError("constructor argument must be ETS2 or list of Link2")
super().__init__(links, **kwargs)
# Should just set it to None
self.base = SE2() # override superclass
@property
def base(self) -> SE2:
"""
Get/set robot base transform (Robot superclass)
:returns: robot tool transform
``robot.base`` is the robot base transform
- ``robot.base = ...`` checks and sets the robot base transform
.. rubric:: Notes
- The private attribute ``_base`` will be None in the case of
no base transform, but this property will return ``SE3()`` which
is an identity matrix.
"""
if self._base is None: # pragma nocover
self._base = SE2()
# return a copy, otherwise somebody with
# reference to the base can change it
return self._base.copy()
@base.setter
def base(self, T):
if isinstance(T, SE2):
self._base = T
elif SE2.isvalid(T): # pragma nocover
self._tool = SE2(T, check=True)
[docs]
def jacob0(self, q, start=None, end=None):
return self.ets(start, end).jacob0(q)
[docs]
def jacobe(self, q, start=None, end=None):
return self.ets(start, end).jacobe(q)
[docs]
def fkine(self, q, end=None, start=None):
return self.ets(start, end).fkine(q)
@property
def reach(self) -> float:
r"""
Reach of the robot
:returns: Maximum reach of the robot
:rtype: float
A conservative estimate of the reach of the robot. It is computed as
the sum of the translational ETs that define the link transform.
Computed on the first access. If kinematic parameters
subsequently change this will not be reflected.
.. rubric:: Notes
- Probably an overestimate of reach
- Used by numerical inverse kinematics to scale translational
error.
- For a prismatic joint, uses ``qlim`` if it is set
"""
# TODO
# This should be a start, end method and compute the reach based on the
# given ets. Then use an lru_cache to speed up return
if self._reach is None:
d_all = []
for link in self.ee_links:
d = 0
while True:
for et in link.ets:
if et.istranslation:
if et.isjoint:
# the length of a prismatic joint depends on the
# joint limits. They might be set in the ET
# or in the Link depending on how the robot
# was constructed
if link.qlim is not None:
d += max(link.qlim)
elif et.qlim is not None: # pragma nocover
d += max(et.qlim)
else:
d += abs(et.param)
link = link.parent
if link is None or isinstance(link, str):
d_all.append(d)
break
self._reach = max(d_all)
return self._reach
[docs]
def fkine_all(self, q: ArrayLike) -> SE2:
"""
Compute the pose of every link frame
:param q: The joint configuration
:returns: Pose of all links
``T = robot.fkine_all(q)`` is an SE3 instance with ``robot.nlinks +
1`` values:
- ``T[0]`` is the base transform
- ``T[i]`` is the pose of link whose ``number`` is ``i``
.. rubric:: References
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
"""
q = getvector(q)
Tbase = SE2(self.base) # add base, also sets the type
linkframes = Tbase.__class__.Alloc(self.nlinks + 1)
linkframes[0] = Tbase
def recurse(Tall, Tparent, q, link):
# if joint??
T = Tparent
while True:
T *= SE2(link.A(q[link.jindex]))
Tall[link.number] = T
if link.nchildren == 0:
# no children
return
elif link.nchildren == 1:
# one child
if link in self.ee_links: # pragma nocover
# this link is an end-effector, go no further
return
link = link.children[0]
continue
else:
# multiple children
for child in link.children:
recurse(Tall, T, q, child)
return
recurse(linkframes, Tbase, q, self.links[0])
return linkframes