#!/usr/bin/env python
"""
@author Jesse Haviland
"""
from collections import namedtuple
from email import message
from roboticstoolbox.tools.data import rtb_path_to_datafile
import warnings
import copy
import numpy as np
from roboticstoolbox.robot.Robot import Robot # DHLink
from roboticstoolbox.robot.ETS import ETS, ET
from roboticstoolbox.robot.DHLink import DHLink
from roboticstoolbox import rtb_set_param
from spatialmath.base.argcheck import getvector, isscalar, verifymatrix, getmatrix
# from spatialmath import base
from spatialmath.base import (
tr2jac,
tr2eul,
tr2rpy,
t2r,
trlog,
rotvelxform,
)
from spatialmath import SE3, Twist3
import spatialmath.base.symbolic as sym
# from scipy.optimize import minimize, Bounds
from ansitable import ANSITable, Column
from scipy.linalg import block_diag
from roboticstoolbox.robot.DHLink import _check_rne, DHLink
from roboticstoolbox import rtb_get_param
from roboticstoolbox.frne import init, frne, delete
from numpy import any
from typing import Union, Tuple
from roboticstoolbox.robot.IK import IKSolution
ArrayLike = Union[list, np.ndarray, tuple, set]
# iksol = namedtuple("IKsolution", "q, success, reason")
[docs]class DHRobot(Robot):
"""
Class for robots defined using Denavit-Hartenberg notation
:param L: List of links which define the robot
:type L: list(n)
:param name: Name of the robot
:type name: str
:param manufacturer: Manufacturer of the robot
:type manufacturer: str
:param base: Location of the base
:type base: SE3
:param tool: Location of the tool
:type tool: SE3
:param gravity: Gravitational acceleration vector
:type gravity: ndarray(3)
A concrete superclass for arm type robots defined using Denavit-Hartenberg
notation, that represents a serial-link arm-type robot. Each link and
joint in the chain is described by a DHLink-class object using
Denavit-Hartenberg parameters (standard or modified).
.. note:: Link subclass elements passed in must be all standard, or all
modified, DH parameters.
:reference:
- Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7-9.
- Robot, Modeling & Control,
M.Spong, S. Hutchinson & M. Vidyasagar, Wiley 2006.
"""
def __init__(self, links, meshdir=None, **kwargs):
# Verify L
if not isinstance(links, list):
raise TypeError("The links must be stored in a list.")
all_links = []
self._n = 0
# If we are given a list of standard DH Links, we must convert
# them to modified DH links
# if any([isinstance(link, StandardDH) for link in links]):
# links = DHLink.StandardDH(links)
for link in links:
if isinstance(link, DHLink):
# got a link
all_links.append(link)
link.number = self._n + 1
link.jindex = self._n
self._n += 1
link.name = f"link{self._n}"
elif isinstance(link, DHRobot):
# link is actually a robot
# copy the links
rlinks = copy.copy(link.links)
for rlink in rlinks:
all_links.append(rlink)
rlink.number = self._n
rlink.jindex = self._n
self._n += 1
rlink.name = f"link{self._n}"
else:
raise TypeError("Input can be only DHLink or DHRobot")
for i, link in enumerate(all_links):
if i > 0:
link.parent = all_links[i - 1]
else:
link.parent = None
super().__init__(all_links, **kwargs)
self._ee_links = [self.links[-1]]
# Check the DH convention
self._mdh = self.links[0].mdh
if not all([link.mdh == self.mdh for link in self.links]):
raise ValueError("Robot has mixed D&H link conventions")
# load meshes if required
if meshdir is not None:
self.meshdir = rtb_path_to_datafile(meshdir)
self.basemesh = self.meshdir / "link0.stl"
for j, link in enumerate(self._links, start=1):
link.mesh = self.meshdir / "link{:d}.stl".format(j)
self._hasgeometry = True
else:
self.basemesh = None
# rne parameters
self._rne_ob = None
[docs] def __str__(self):
"""
Pretty prints the DH Model of the robot. Will output angles in degrees
:return: Pretty print of the robot model
:rtype: str
"""
if np.array_equal(self.base.A, np.eye(4)):
base = None
else:
base = self.base
if np.array_equal(self.tool.A, np.eye(4)):
tool = None
else:
tool = self.tool
unicode = rtb_get_param("unicode")
border = "thin" if unicode else "ascii"
s = f"DHRobot: {self.name}"
if self.manufacturer is not None and len(self.manufacturer) > 0:
s += f" (by {self.manufacturer})"
s += f", {self.n} joints ({self.structure})"
deg = 180 / np.pi
if self._hasdynamics:
s += ", dynamics"
if any([link.mesh is not None for link in self.links]):
s += ", geometry"
if self.mdh:
dh = "modified"
else:
dh = "standard"
s += f", {dh} DH parameters\n"
def qstr(j, link):
j += 1
if link.isflip:
s = f"-q{j:d}"
else:
s = f" q{j:d}"
if L.offset != 0:
sign = "+" if L.offset > 0 else "-"
offset = abs(L.offset)
if link.isprismatic:
s += f" {sign} {offset:}"
else:
s += f" {sign} {offset * deg:.3g}\u00b0"
return s
def angle(theta, fmt=None):
if sym.issymbol(theta): # pragma nocover
return "<<red>>" + str(theta)
else:
if fmt is not None:
return fmt.format(theta * deg) + "\u00b0"
else:
return str(theta * deg) + "\u00b0"
def format_attr(attr) -> str:
if isinstance(attr, float):
return f"{attr:.4g}"
else:
return str(attr)
has_qlim = any([link.qlim is not None for link in self])
if has_qlim:
qlim_columns = [
Column("q⁻", headalign="^"),
Column("q⁺", headalign="^"),
]
qlim = self.qlim
else:
qlim = np.array([]) # satisfy type checker
qlim_columns = []
if self.mdh:
# MDH format
table = ANSITable(
Column("aⱼ₋₁", headalign="^"),
Column("⍺ⱼ₋₁", headalign="^"),
Column("θⱼ", headalign="^"),
Column("dⱼ", headalign="^"),
*qlim_columns,
border=border,
)
for j, L in enumerate(self):
if has_qlim:
if L.isprismatic:
ql = [qlim[0, j], qlim[1, j]]
else:
ql = [angle(qlim[k, j], "{:.1f}") for k in [0, 1]]
else:
ql = []
if L.isprismatic:
table.row(L.a, angle(L.alpha), angle(L.theta), qstr(j, L), *ql)
else:
table.row(L.a, angle(L.alpha), qstr(j, L), L.d, *ql)
else:
# DH format
table = ANSITable(
Column("θⱼ", headalign="^", colalign="<"),
Column("dⱼ", headalign="^"),
Column("aⱼ", headalign="^"),
Column("⍺ⱼ", headalign="^"),
*qlim_columns,
border=border,
)
for j, L in enumerate(self):
if has_qlim:
if L.isprismatic:
ql = [qlim[0, j], qlim[1, j]]
else:
ql = [angle(qlim[k, j], "{:.1f}") for k in [0, 1]]
else:
ql = []
if L.isprismatic:
table.row(
angle(L.theta),
qstr(j, L),
format_attr(L.a),
angle(L.alpha),
*ql,
)
else:
table.row(
qstr(j, L),
format_attr(L.d),
format_attr(L.a),
angle(L.alpha),
*ql,
)
s += str(table)
# show tool and base
if self._tool is not None or self._tool is not None:
table = ANSITable(
Column("", colalign=">"),
Column("", colalign="<"),
border=border,
header=False,
)
if base is not None:
table.row(
"base",
base.printline(orient="rpy/xyz", fmt="{:.2g}", file=None),
)
if tool is not None:
table.row(
"tool",
tool.strline(orient="rpy/xyz", fmt="{:.2g}"),
)
s += "\n" + str(table)
# show named configurations
s += self.configurations_str(border=border)
return s
def __add__(self, L):
nlinks = []
# TODO - Should I do a deep copy here a physically copy the DHLinks
# and not just the references?
# Copy DHLink references to new list
for i in range(self.n):
nlinks.append(self.links[i])
if isinstance(L, DHLink):
nlinks.append(L)
elif isinstance(L, DHRobot):
for j in range(L.n):
nlinks.append(L.links[j])
else:
raise TypeError("Can only combine DHRobots with other DHRobots or DHLinks")
return DHRobot(
nlinks,
name=self.name,
manufacturer=self.manufacturer,
base=self.base,
tool=self.tool,
gravity=self.gravity,
)
def __deepcopy__(self, memo):
links = []
for link in self.links:
links.append(copy.deepcopy(link))
name = copy.deepcopy(self.name)
manufacturer = copy.deepcopy(self.manufacturer)
comment = copy.deepcopy(self.comment)
base = copy.deepcopy(self.base)
tool = copy.deepcopy(self.tool)
gravity = copy.deepcopy(self.gravity)
keywords = copy.deepcopy(self.keywords)
symbolic = copy.deepcopy(self.symbolic)
configs = copy.deepcopy(self.configs)
try:
if self.meshdir:
meshdir = copy.deepcopy(self.meshdir)
else:
meshdir = None
except AttributeError:
meshdir = None
# cls = self.__class__
result = DHRobot(
links,
meshdir=meshdir,
name=name,
manufacturer=manufacturer,
comment=comment,
base=base,
tool=tool,
gravity=gravity,
keywords=keywords,
symbolic=symbolic,
configs=configs,
)
# if a configuration was an attribute of original robot, make it an
# attribute of the deep copy
for config in configs:
if hasattr(self, config):
setattr(result, config, configs[config])
try:
setattr(result, "ikine_a", getattr(self, "ikine_a"))
except AttributeError:
pass
memo[id(self)] = result
return result
# def copy(self):
# """
# Copy a robot
# :return: A deepish copy of the robot
# :rtype: Robot subclass instance
# """
# L = [link.copy() for link in self]
# new = DHRobot(
# L,
# name=self.name,
# manufacturer=self.manufacturer,
# base=self.base,
# tool=self.tool,
# gravity=self.gravity)
# new.q = self.q
# new.qd = self.qd
# new.qdd = self.qdd
# return new
# --------------------------------------------------------------------- #
def _set_link_fk(self, q):
"""
robot._set_link_fk(q) evaluates fkine for each link within a
robot and stores that pose in a private variable within the link.
This method is not for general use.
:param q: The joint angles/configuration of the robot
:type q: float ndarray(n)
.. note::
- The robot's base transform, if present, are incorporated
into the result.
"""
q = getvector(q, self.n)
# t = self.base
tall = self.fkine_all(q, old=True)
for i, link in enumerate(self.links):
# Update the link model transforms
for col in link.collision:
col.wT = tall[i]
for gi in link.geometry:
gi.wT = tall[i]
# --------------------------------------------------------------------- #
@property
def mdh(self):
"""
Modified Denavit-Hartenberg status
:return: whether robot is defined using modified Denavit-Hartenberg
notation
:rtype: bool
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.mdh
>>> panda = rtb.models.DH.Panda()
>>> panda.mdh
"""
return self._mdh
@property
def d(self):
r"""
Link offset values
:return: List of link offset values :math:`d_j`.
:rtype: ndarray(n,)
The following are equivalent::
robot.links[j].d
robot.d[j]
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.d
"""
v = []
for i in range(self.n):
v.append(self.links[i].d)
return v
@property
def a(self):
r"""
Link length values
:return: List of link length values :math:`a_j`.
:rtype: ndarray(n,)
The following are equivalent::
robot.links[j].a
robot.a[j]
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.a
"""
v = []
for i in range(self.n):
v.append(self.links[i].a)
return v
@property
def theta(self):
r"""
Joint angle values
:return: List of joint angle values :math:`\theta_j`.
:rtype: ndarray(n,)
The following are equivalent::
robot.links[j].theta
robot.theta[j]
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.theta
"""
v = []
for i in range(self.n):
v.append(self.links[i].theta)
return v
@property
def alpha(self):
r"""
Link twist values
:return: List of link twist values :math:`\alpha_j`.
:rtype: ndarray(n,)
The following are equivalent::
robot.links[j].alpha
robot.alpha[j]
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.alpha
"""
v = []
for i in range(self.n):
v.append(self.links[i].alpha)
return v
@property
def r(self):
r"""
Link centre of mass values
:return: Array of link centre of mass values :math:`r_j`.
:rtype: ndarray(3,n)
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.r
"""
# TODO tidyup
v = np.copy(self.links[0].r)
for i in range(1, self.n):
v = np.c_[v, self.links[i].r]
return v
@property
def offset(self):
r"""
Joint offset values
:return: List of joint offset values :math:`\bar{q}_j`.
:rtype: ndarray(n,)
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.offset
"""
v = []
for i in range(self.n):
v.append(self.links[i].offset)
return v
@property
def reach(self):
r"""
Reach of the robot
:return: Maximum reach of the robot
:rtype: float
A conservative estimate of the reach of the robot. It is computed as
:math:`\sum_j |a_j| + |d_j|` where :math:`d_j` is taken as the maximum
joint coordinate (``qlim``) if the joint is primsmatic.
.. note::
- This is the *length sum* referred to in Craig's book
- Probably an overestimate of the actual reach
- Used by numerical inverse kinematics to scale translational
error.
- For a prismatic joint, uses ``qlim`` if it is set
.. warning:: Computed on the first access. If kinematic parameters
subsequently change this will not be reflected.
"""
if self._reach is None:
d = 0
for link in self:
d += abs(link.a) + (link.d)
if link.isprismatic and link.qlim is not None:
d += link.qlim[1]
self._reach = d
return self._reach
@property
def nbranches(self):
"""
Number of branches
:return: number of branches in the robot's kinematic tree
:rtype: int
Number of branches in this robot.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Panda()
>>> robot.nbranches
:seealso: :func:`n`, :func:`nlinks`
"""
return 1
[docs] def A(self, j, q=None):
"""
Link forward kinematics
:param j: Joints to compute link transform for
:type j: int, 2-tuple
:param q: The joint configuration of the robot (Optional,
if not supplied will use the stored q values)
:type q: float ndarray(1,n)
:return T: The transform between link frames
:rtype T: SE3
- ``robot.A(j, q)`` transform between link frames {0} and {j}. ``q``
is a vector (n) of joint variables.
- ``robot.A([j1, j2], q)`` as above between link frames {j1} and {j2}.
- ``robot.A(j)`` as above except uses the stored q value of the
robot object.
.. note:: Base and tool transforms are not applied.
"""
if isscalar(j):
j0 = 0
jn = int(j)
else:
j = getvector(j, 2)
j0 = int(j[0])
jn = int(j[1])
jn += 1
if jn > self.n:
raise ValueError("The joints value out of range")
q = getvector(q)
T = SE3()
for i in range(j0, jn):
T *= self.links[i].A(q[i])
return T
[docs] def islimit(self, q=None):
"""
Joint limit test
:param q: The joint configuration of the robot (Optional,
if not supplied will use the stored q values)
:type q: ndarray(n
:return v: is a vector of boolean values, one per joint, False if
``q[j]`` is within the joint limits, else True
:rtype v: bool list
- ``robot.islimit(q)`` is a list of boolean values indicating if the
joint configuration ``q`` is in violation of the joint limits.
- ``robot.jointlimit()`` as above except uses the stored q value of the
robot object.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.islimit([0, 0, -4, 4, 0, 0])
"""
if q is None:
q = self.q
return [link.islimit(qk) for (link, qk) in zip(self, q)]
[docs] def isspherical(self):
"""
Test for spherical wrist
:return: True if spherical wrist
:rtype: bool
Tests if the robot has a spherical wrist, that is, the last 3 axes are
revolute and their axes intersect at a point.
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.isspherical()
"""
if self.n < 3:
return False
L = self.links[self.n - 3 : self.n]
alpha = [-np.pi / 2, np.pi / 2]
return (
L[0].a == 0
and L[1].a == 0
and L[1].d == 0
and (
(L[0].alpha == alpha[0] and L[1].alpha == alpha[1])
or (L[0].alpha == alpha[1] and L[1].alpha == alpha[0])
)
and L[0].sigma == 0
and L[1].sigma == 0
and L[2].sigma == 0
)
[docs] def dhunique(self):
"""
Print the unique DH parameters
Print a table showing all the non-zero DH parameters, and their
values. This is the minimum set of kinematic parameters required
to describe the robot.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.dhunique()
"""
table = ANSITable(
Column("param"),
Column("value", headalign="^", colalign="<", fmt="{:.4g}"),
border="thin",
)
for j, link in enumerate(self):
if link.isprismatic:
if link.theta != 0:
table.row(f"θ{j}", link.theta)
elif link.isrevolute:
if link.d != 0:
table.row(f"d{j}", link.d)
if link.a != 0:
table.row(f"a{j}", link.a)
if link.alpha != 0:
table.row(f"⍺{j}", link.alpha)
table.print()
[docs] def twists(self, q=None):
"""
Joint axes as twists
:param q: The joint configuration of the robot, defaults to zero
:return: a vector of joint axis twists
:rtype: Twist3 instance
:return: Pose of the tool
:rtype: SE3 instance
- ``tw, T0 = twists(q)`` calculates a vector of Twist objects (n) that
represent the axes of the joints for the robot with joint coordinates
``q`` (n). Also returns T0 which is an SE3 object representing the
pose of the tool.
- ``tw, T0 = twists()`` as above but the joint coordinates are taken
to be zero.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> tw, T0 = robot.twists()
>>> tw
>>> T0
"""
if q is None:
q = np.zeros((self.n,))
T = self.fkine_all(q)[1:] # don't use first transform which is base
tw = Twist3.Alloc(self.n)
if self.mdh:
# MDH case
for j, link in enumerate(self):
if link.sigma == 0:
tw[j] = Twist3.UnitRevolute(T[j].a, T[j].t)
else:
tw[j] = Twist3.UnitPrismatic(T[j].a)
else:
# DH case
for j, link in enumerate(self):
if j == 0:
# first link case
if link.sigma == 0:
# revolute
tw[j] = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0])
else:
tw[j] = Twist3.UnitPrismatic([0, 0, 1]) # prismatic
else:
# subsequent links
if link.sigma == 0:
tw[j] = Twist3.UnitRevolute(T[j - 1].a, T[j - 1].t) # revolute
else:
tw[j] = Twist3.UnitPrismatic(T[j - 1].a) # prismatic
return tw, T[-1]
[docs] def ets(self, *args, **kwargs) -> ETS:
"""
Robot kinematics as an elemenary transform sequence
:return: elementary transform sequence
:rtype: ETS
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.ets()
"""
# optionally start with the base transform
if np.array_equal(self.base.A, np.eye(4)):
base = None
else:
base = self.base.A
if np.array_equal(self.tool.A, np.eye(4)):
tool = None
else:
tool = self.tool.A
if base is None:
ets = ETS()
else:
ets = ET.SE3(base)
# add the links
for link in self:
ets *= link.ets
# optionally add the base transform
if tool is not None:
ets *= ET.SE3(tool)
return ets
[docs] def fkine(self, q, **kwargs):
"""
Forward kinematics
:param q: The joint configuration
:type q: ndarray(n) or ndarray(m,n)
:return: Forward kinematics as an SE(3) matrix
:rtype: SE3 instance
- ``robot.fkine(q)`` computes the forward kinematics for the robot at
joint configuration ``q``.
If q is a 2D array, the rows are interpreted as the generalized joint
coordinates for a sequence of points along a trajectory. ``q[k,j]`` is
the j'th joint coordinate for the k'th trajectory configuration, and
the returned ``SE3`` instance contains ``n`` values.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.fkine([0, 0, 0, 0, 0, 0])
.. note::
- The robot's base or tool transform, if present, are incorporated
into the result.
- Joint offsets, if defined, are added to ``q`` before the forward
kinematics are computed.
"""
if np.array_equal(self.base.A, np.eye(4)):
base = None
else:
base = self.base
if np.array_equal(self.tool.A, np.eye(4)):
tool = None
else:
tool = self.tool
T = SE3.Empty()
for qr in getmatrix(q, (None, self.n)):
first = True
for q, L in zip(qr, self.links):
if first:
Tr = L.A(q)
first = False
else:
Tr *= L.A(q) # type: ignore
if base is not None:
Tr = base * Tr # type: ignore
if tool is not None:
Tr = Tr * tool # type: ignore
T.append(Tr) # type: ignore
return T
[docs] def fkine_path(self, q, old=None):
"""
Compute the pose of every link frame
:param q: The joint configuration
:type q: darray(n)
:return: Pose of all links
:rtype: SE3 instance
``T = robot.fkine_path(q)`` is an SE3 instance with ``robot.nlinks +
1`` values:
- ``T[0]`` is the base transform
- ``T[i+1]`` is the pose of link whose ``number`` is ``i``
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
T = self.base
q = getvector(q)
Tj = T
for q, L in zip(q, self.links):
Tj *= L.A(q)
T.append(Tj)
if self._tool is not None:
T[-1] *= self._tool
return T
[docs] def segments(self):
segments = [None]
segments.extend(self.links)
return [segments]
[docs] def fkine_all(self, q=None, old=True):
"""
Forward kinematics for all link frames
:param q: The joint configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: ndarray(n) or ndarray(m,n)
:param old: "old" behaviour, defaults to True
:type old: bool, optional
:return: Forward kinematics as an SE(3) matrix
:rtype: SE3 instance with ``n`` values
- ``fkine_all(q)`` evaluates fkine for each joint within a robot and
returns a sequence of link frame poses.
- ``fkine_all()`` as above except uses the stored q value of the
robot object.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> T = puma.fkine_all([0, 0, 0, 0, 0, 0])
>>> len(T)
.. note::
- Old behaviour is to return a list of ``n`` frames {1} to {n}, but
if ``old=False`` it returns ``n``+1 frames {0} to {n}, ie. it
includes the base frame.
- The robot's base or tool transform, if present, are incorporated
into the result.
- Joint offsets, if defined, are added to q before the forward
kinematics are computed.
"""
if q is None:
q = self.q
Tj = self.base.copy()
Tall = Tj
for q, L in zip(q, self.links):
Tj *= L.A(q)
Tall.append(Tj)
return Tall
[docs] def jacobe(self, q, half=None, **kwargs):
r"""
Manipulator Jacobian in end-effector frame
:param q: Joint coordinate vector
:type q: ndarray(n)
:param half: return half Jacobian: 'trans' or 'rot'
:type half: str
:return J: The manipulator Jacobian in the end-effector frame
:rtype: ndarray(6,n)
- ``robot.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, v_z, \omega_x, \omega_y, \omega_z)^T`
is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.jacobe([0, 0, 0, 0, 0, 0])
.. warning:: This is the **geometric Jacobian** as described in texts by
Corke, Spong etal., Siciliano etal. The end-effector velocity is
described in terms of translational and angular velocity, not a
velocity twist as per the text by Lynch & Park.
""" # noqa
q = getvector(q, self.n)
n = self.n
L = self.links
J = np.zeros((6, self.n), dtype=q.dtype) # type: ignore
U = self.tool.A
for j in range(n - 1, -1, -1):
if self.mdh == 0:
# standard DH convention
U = L[j].A(q[j]).A @ U # type: ignore
if not L[j].sigma:
# revolute axis
d = np.array(
[
-U[0, 0] * U[1, 3] + U[1, 0] * U[0, 3],
-U[0, 1] * U[1, 3] + U[1, 1] * U[0, 3],
-U[0, 2] * U[1, 3] + U[1, 2] * U[0, 3],
]
)
delta = U[2, :3] # nz oz az
else:
# prismatic axis
d = U[2, :3] # nz oz az
delta = np.zeros((3,))
J[:, j] = np.r_[d, delta]
if self.mdh != 0:
# modified DH convention
U = L[j].A(q[j]).A @ U # type: ignore
# return top or bottom half if asked
if half is not None:
if half == "trans":
return J[:3, :]
elif half == "rot":
return J[3:, :]
else:
raise ValueError("bad half specified")
return J
[docs] def jacob0(self, q=None, T=None, half=None, start=None, end=None):
r"""
Manipulator Jacobian in world frame
:param q: Joint coordinate vector
:type q: ndarray(n)
:param T: Forward kinematics if known, SE(3 matrix)
:type T: SE3 instance
:param half: return half Jacobian: 'trans' or 'rot'
:type half: str
:return J: The manipulator Jacobian in the world frame
:rtype: ndarray(6,n)
- ``robot.jacob0(q)`` is the manipulator geometric Jacobian matrix which maps
joint velocity to end-effector spatial velocity.
End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T`
is related to joint velocity by :math:`{}^{0}\!\nu = \mathbf{J}_0(q) \dot{q}`.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.jacob0([0, 0, 0, 0, 0, 0])
.. warning:: This is the **geometric Jacobian** is as described in texts by
Corke, Spong etal., Siciliano etal. The end-effector velocity is
described in terms of translational and angular velocity, not a
velocity twist as per the text by Lynch & Park.
.. note:: ``T`` can be passed in to save the cost of computing forward
kinematics which is needed to transform velocity from end-effector
frame to world frame.
""" # noqa
q = getvector(q, self.n)
if T is None:
T = self.fkine(q)
T = T.A
# compute Jacobian in EE frame and transform to world frame
J0 = tr2jac(T) @ self.jacobe(q)
# TODO optimize computation above if half matrix is returned
# return top or bottom half if asked
if half is not None:
if half == "trans":
J0 = J0[:3, :]
elif half == "rot":
J0 = J0[3:, :]
else:
raise ValueError("bad half specified")
return J0
[docs] def jacob0_analytical(self, q, representation=None, T=None):
r"""
Manipulator Jacobian in world frame
:param q: Joint coordinate vector
:type q: ndarray(n)
:param representation: return analytical Jacobian instead of geometric Jacobian
:type representation: str
:param T: Forward kinematics if known, SE(3 matrix)
:type T: SE3 instance
:return J: The manipulator analytical Jacobian in the world frame
:rtype: ndarray(6,n)
Return the manipulator's analytical Jacobian matrix which maps
joint velocity to end-effector spatial velocity.
End-effector spatial velocity :math:`\nu_a = (v_x, v_y, v_z, \dot{\Gamma}_1, \dot{\Gamma}_2, \dot{\Gamma}_3)^T`
is related to joint velocity by :math:`{}^{0}\!\nu_a = \mathbf{J}_{a,0}(q) \dot{q}`.
Where :math:`\dvec{\Gamma} = (\dot{\Gamma}_1, \dot{\Gamma}_2, \dot{\Gamma}_3)` is
orientation rate expressed as one of:
================== ==================================
``representation`` Rotational representation
================== ==================================
``'rpy/xyz'`` RPY angular rates in XYZ order
``'rpy/zyx'`` RPY angular rates in XYZ order
``'eul'`` Euler angular rates in ZYZ order
``'exp'`` exponential coordinate rates
================== ==================================
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.jacob0_analytical([0, 0, 0, 0, 0, 0], "rpy/xyz")
.. warning:: The **geometric Jacobian** is as described in texts by
Corke, Spong etal., Siciliano etal. The end-effector velocity is
described in terms of translational and angular velocity, not a
velocity twist as per the text by Lynch & Park.
.. note:: ``T`` can be passed in to save the cost of computing forward
kinematics which is needed to transform velocity from end-effector
frame to world frame.
""" # noqa
q = getvector(q, self.n)
# compute forward kinematics if not provided
if T is None:
T = self.fkine(q)
# compute Jacobian in world frame
J0 = self.jacob0(q, T)
if representation is None:
return J0
# compute rotational transform if analytical Jacobian required
if representation == "rpy/xyz":
gamma = tr2rpy(T.A, order="xyz")
elif representation == "rpy/zyx":
gamma = tr2rpy(T.A, order="zyx")
elif representation == "eul":
gamma = tr2eul(T.A)
elif representation == "exp":
# TODO: move to SMTB.base, Horner form with skew(v)
gamma = trlog(t2r(T.A), twist=True)
else:
raise ValueError("bad analytical value specified")
A = rotvelxform(gamma, representation=representation, inverse=True, full=True)
return A @ J0
[docs] def hessian0(self, q=None, J0=None, start=None, end=None):
r"""
Manipulator Hessian in base frame
:param q: joint coordinates
:type q: array_like
:param J0: Jacobian in {0} frame
:type J0: ndarray(6,n)
:return: Hessian matrix
:rtype: ndarray(6,n,n)
This method calculcates the Hessisan in the base frame. One of ``J0`` or
``q`` is required. If ``J0`` is already calculated for the joint
coordinates ``q`` it can be passed in to to save computation time.
If we take the time derivative of the differential kinematic
relationship
.. math::
\nu &= \mat{J}(\vec{q}) \dvec{q} \\
\alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q}
where
.. math::
\dmat{J} = \mat{H} \dvec{q}
and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the
Hessian tensor.
The elements of the Hessian are
.. math::
\mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k}
where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements
of the spatial velocity vector.
Similarly, we can write
.. math::
\mat{J}_{i,j} = \frac{d u_i}{d q_j}
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
:seealso: :func:`jacob0`, :func:`jacob_dot`
"""
return self.ets().hessian0(q, J0)
def _get_limit_links(self, end=None, start=None):
# For compatibility with ERobot
return None, None, None
# -------------------------------------------------------------------------- #
def _init_rne(self):
# Compress link data into a 1D array
L = np.zeros(24 * self.n)
for i in range(self.n):
j = i * 24
L[j] = self.links[i].alpha
L[j + 1] = self.links[i].a
L[j + 2] = self.links[i].theta
L[j + 3] = self.links[i].d
L[j + 4] = self.links[i].sigma
L[j + 5] = self.links[i].offset
L[j + 6] = self.links[i].m
L[j + 7 : j + 10] = self.links[i].r.flatten()
L[j + 10 : j + 19] = self.links[i].I.flatten()
L[j + 19] = self.links[i].Jm
L[j + 20] = self.links[i].G
L[j + 21] = self.links[i].B
L[j + 22 : j + 24] = self.links[i].Tc.flatten()
# we negate gravity here, since the C code has the sign wrong
self._rne_ob = init(self.n, self.mdh, L, -self.gravity)
[docs] def delete_rne(self):
"""
Frees the memory holding the robot object in c if the robot object
has been initialised in c.
"""
if self._rne_ob is not None:
delete(self._rne_ob)
self._dynchanged = False
self._rne_ob = None
[docs] @_check_rne
def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False):
r"""
Inverse dynamics
:param q: Joint coordinates
:type q: ndarray(n)
:param qd: Joint velocity
:type qd: ndarray(n)
:param qdd: The joint accelerations of the robot
:type qdd: ndarray(n)
:param gravity: Gravitational acceleration to override robot's gravity
value
:type gravity: ndarray(6)
:param fext: Specify wrench acting on the end-effector
:math:`W=[F_x F_y F_z M_x M_y M_z]`
:type fext: ndarray(6)
``tau = rne(q, qd, qdd, grav, fext)`` is the joint torque required for
the robot to achieve the specified joint position ``q`` (1xn), velocity
``qd`` (1xn) and acceleration ``qdd`` (1xn), where n is the number of
robot joints. ``fext`` describes the wrench acting on the end-effector
Trajectory operation:
If q, qd and qdd (mxn) are matrices with m cols representing a
trajectory then tau (mxn) is a matrix with cols corresponding to each
trajectory step.
.. note::
- The torque computed contains a contribution due to armature
inertia and joint friction.
- If a model has no dynamic parameters set the result is zero.
:seealso: :func:`rne_python`
"""
if base_wrench:
return self.rne_python(
q, qd, qdd, gravity=gravity, fext=fext, base_wrench=base_wrench
)
trajn = 1
try:
q = getvector(q, self.n, "row")
qd = getvector(qd, self.n, "row")
qdd = getvector(qdd, self.n, "row")
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
verifymatrix(qd, (trajn, self.n))
verifymatrix(qdd, (trajn, self.n))
if gravity is None:
gravity = self.gravity
else:
gravity = getvector(gravity, 3)
# The c function doesn't handle base rotation, so we need to hack the
# gravity vector instead
gravity = self.base.R.T @ gravity
if fext is None:
fext = np.zeros(6)
else:
fext = getvector(fext, 6)
tau = np.zeros((trajn, self.n))
for i in range(trajn):
tau[i, :] = frne(
# we negate gravity here, since the C code has the sign wrong
self._rne_ob,
q[i, :],
qd[i, :],
qdd[i, :],
-gravity,
fext,
)
if trajn == 1:
return tau[0, :]
else:
return tau
[docs] def rne_python(
self,
Q,
QD=None,
QDD=None,
gravity=None,
fext=None,
debug=False,
base_wrench=False,
):
"""
Compute inverse dynamics via recursive Newton-Euler formulation
:param Q: Joint coordinates
:param QD: Joint velocity
:param QDD: Joint acceleration
:param gravity: gravitational acceleration, defaults to attribute
of self
:type gravity: array_like(3), optional
:param fext: end-effector wrench, defaults to None
:type fext: array-like(6), optional
:param debug: print debug information to console, defaults to False
:type debug: bool, optional
:param base_wrench: compute the base wrench, defaults to False
:type base_wrench: bool, optional
:raises ValueError: for misshaped inputs
:return: Joint force/torques
:rtype: NumPy array
Recursive Newton-Euler for standard Denavit-Hartenberg notation.
- ``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).
.. note::
- This is a pure Python implementation and slower than the .rne()
which is written in C.
- This version supports symbolic model parameters
- Verified against MATLAB code
:seealso: :func:`rne`
"""
if np.array_equal(self.base.A, np.eye(4)):
base = None
else:
base = self.base.A
def removesmall(x):
return x
n = self.n
if self.symbolic:
dtype = "O"
else:
dtype = None
z0 = np.array([0, 0, 1], dtype=dtype)
if gravity is None:
gravity = self.gravity # default gravity from the object
else:
gravity = getvector(gravity, 3)
if fext is None:
fext = np.zeros((6,), dtype=dtype)
else:
fext = getvector(fext, 6)
if debug:
print("grav", gravity)
print("fext", fext)
# unpack the joint coordinates and derivatives
if Q is not None and QD is None and QDD is None:
# single argument case
Q = getmatrix(Q, (None, self.n * 3))
q = Q[:, 0:n]
qd = Q[:, n : 2 * n]
qdd = Q[:, 2 * n :]
else:
# 3 argument case
q = getmatrix(Q, (None, self.n))
qd = getmatrix(QD, (None, self.n))
qdd = getmatrix(QDD, (None, self.n))
nk = q.shape[0]
tau = np.zeros((nk, n), dtype=dtype)
if base_wrench:
wbase = np.zeros((nk, n), dtype=dtype)
for k in range(nk):
# take the k'th row of data
q_k = q[k, :]
qd_k = qd[k, :]
qdd_k = qdd[k, :]
if debug:
print("q_k", q_k)
print("qd_k", qd_k)
print("qdd_k", qdd_k)
print()
# joint vector quantities stored columwise in matrix
# m suffix for matrix
Fm = np.zeros((3, n), dtype=dtype)
Nm = np.zeros((3, n), dtype=dtype)
# if robot.issym
# pstarm = sym([]);
# else
# pstarm = [];
pstarm = np.zeros((3, n), dtype=dtype)
Rm = []
# rotate base velocity and acceleration into L1 frame
# base has zero angular velocity
w = np.zeros((3,), dtype=dtype)
# base has zero angular acceleration
wd = np.zeros((3,), dtype=dtype)
vd = -gravity # type: ignore
if base is not None:
Rb = t2r(base).T
w = Rb @ w
wd = Rb @ wd
vd = Rb @ gravity
# ---------------- initialize some variables ----------------- #
for j in range(n):
link = self.links[j]
# compute the link rotation matrix
if link.sigma == 0:
# revolute axis
Tj = link.A(q_k[j]).A
d = link.d
else:
# prismatic
Tj = link.A(link.theta).A
d = q_k[j]
# compute pstar:
# O_{j-1} to O_j in {j}, negative inverse of link xform
alpha = link.alpha
if self.mdh:
pstar = np.r_[link.a, -d * sym.sin(alpha), d * sym.cos(alpha)]
if j == 0:
if base:
Tj = base @ Tj
pstar = base @ pstar
else:
pstar = np.r_[link.a, d * sym.sin(alpha), d * sym.cos(alpha)]
# stash them for later
Rm.append(t2r(Tj))
pstarm[:, j] = pstar
# ----------------- the forward recursion -------------------- #
for j, link in enumerate(self.links):
Rt = Rm[j].T # transpose!!
pstar = pstarm[:, j]
r = link.r
# statement order is important here
if self.mdh:
if link.isrevolute:
# revolute axis
w_ = Rt @ w + z0 * qd_k[j]
wd_ = Rt @ wd + z0 * qdd_k[j] + _cross(Rt @ w, z0 * qd_k[j])
vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd
else:
# prismatic axis
w_ = Rt @ w
wd_ = Rt @ wd
vd_ = (
Rt @ (_cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd)
+ 2 * _cross(Rt @ w, z0 * qd_k[j])
+ z0 * qdd_k[j]
)
# trailing underscore means new value, update here
w = w_
wd = wd_
vd = vd_
else:
if link.isrevolute:
# revolute axis
wd = Rt @ (wd + z0 * qdd_k[j] + _cross(w, z0 * qd_k[j]))
w = Rt @ (w + z0 * qd_k[j])
vd = _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + Rt @ vd
else:
# prismatic axis
w = Rt @ w
wd = Rt @ wd
vd = (
Rt @ (z0 * qdd_k[j] + vd)
+ _cross(wd, pstar)
+ 2 * _cross(w, Rt @ z0 * qd_k[j])
+ _cross(w, _cross(w, pstar))
)
vhat = _cross(wd, r) + _cross(w, _cross(w, r)) + vd
Fm[:, j] = link.m * vhat
Nm[:, j] = link.I @ wd + _cross(w, link.I @ w)
if debug:
print("w: ", removesmall(w))
print("wd: ", removesmall(wd))
print("vd: ", removesmall(vd))
print("vdbar: ", removesmall(vhat))
print()
if debug:
print("Fm\n", Fm)
print("Nm\n", Nm)
# ----------------- the backward recursion -------------------- #
f = fext[:3] # force/moments on end of arm
nn = fext[3:]
for j in range(n - 1, -1, -1):
link = self.links[j]
r = link.r
#
# order of these statements is important, since both
# nn and f are functions of previous f.
#
if self.mdh:
if j == (n - 1):
R = np.eye(3, dtype=dtype)
pstar = np.zeros((3,), dtype=dtype)
else:
R = Rm[j + 1]
pstar = pstarm[:, j + 1]
f_ = R @ f + Fm[:, j]
nn_ = (
R @ nn
+ _cross(pstar, R @ f)
+ _cross(pstar, Fm[:, j])
+ Nm[:, j]
)
f = f_
nn = nn_
else:
pstar = pstarm[:, j]
if j == (n - 1):
R = np.eye(3, dtype=dtype)
else:
R = Rm[j + 1]
nn = (
R @ (nn + _cross(R.T @ pstar, f))
+ _cross(pstar + r, Fm[:, j])
+ Nm[:, j]
)
f = R @ f + Fm[:, j]
if debug:
print("f: ", removesmall(f))
print("n: ", removesmall(nn))
R = Rm[j]
if self.mdh:
if link.isrevolute:
# revolute axis
t = nn @ z0
else:
# prismatic
t = f @ z0
else:
if link.isrevolute:
# revolute axis
t = nn @ (R.T @ z0)
else:
# prismatic
t = f @ (R.T @ z0)
# add joint inertia and friction
# no Coulomb friction if model is symbolic
tau[k, j] = (
t
+ link.G**2 * link.Jm * qdd_k[j]
- link.friction(qd_k[j], coulomb=not self.symbolic)
)
if debug:
print(
f"j={j:}, G={link.G:}, Jm={link.Jm:},"
f" friction={link.friction(qd_k[j], coulomb=False):}"
) # noqa
print()
# compute the base wrench and save it
if base_wrench:
R = Rm[0]
nn = R @ nn
f = R @ f
wbase[k, :] = np.r_[f, nn]
# if self.symbolic:
# # simplify symbolic expressions
# print(
# 'start symbolic simplification, this might take a while...')
# # from sympy import trigsimp
# # tau = trigsimp(tau)
# # consider using multiprocessing to spread over cores
# # https://stackoverflow.com/questions/33844085/using-multiprocessing-with-sympy # noqa
# print('done')
# if tau.shape[0] == 1:
# return tau.reshape(self.n)
# else:
# return tau
if base_wrench:
if tau.shape[0] == 1:
return tau.flatten(), wbase.flatten()
else:
return tau, wbase
else:
if tau.shape[0] == 1:
return tau.flatten()
else:
return tau
# -------------------------------------------------------------------------- #
[docs] def ikine_6s(self, T, config, ikfunc):
# Undo base and tool transformations, but if they are not
# set, skip the operation. Nicer for symbolics
if np.array_equal(self.base.A, np.eye(4)):
base = None
else:
base = self.base
if np.array_equal(self.tool.A, np.eye(4)):
tool = None
else:
tool = self.tool
if base is not None:
T = base.inv() * T
if tool is not None:
T = tool.inv() * T
# q = np.zeros((6,))
solutions = []
for k, Tk in enumerate(T):
# get model specific solution for first 3 joints
theta = ikfunc(self, Tk, config)
if isinstance(theta, np.ndarray):
# Solve for the wrist rotation
# We need to account for some random translations between the
# first and last 3 joints (d4) and also d6,a6,alpha6 in the
# final frame.
# Transform of first 3 joints
T13 = self.A([0, 2], theta)
# T = T13 * Tz(d4) * R * Tz(d6) Tx(a5)
Td4 = SE3(0, 0, self.links[3].d) # Tz(d4)
# Tz(d6) Tx(a5) Rx(alpha6)
Tt = SE3(self.links[5].a, 0, self.links[5].d) * SE3.Rx(
self.links[5].alpha
)
R = Td4.inv() * T13.inv() * Tk * Tt.inv()
# The spherical wrist implements Euler angles
if "f" in config:
eul = R.eul(flip=True)
else:
eul = R.eul()
theta = np.r_[theta, eul]
if self.links[3].alpha > 0:
theta[4] = -theta[4]
# Remove the link offset angles
theta = theta - self.offset
# solution = iksol(theta, True, "")
solution = IKSolution(q=theta, success=True)
else:
# ikfunc can return None or a str reason
if theta is None:
solution = IKSolution(q=None, success=False)
else:
solution = IKSolution(q=None, success=False, reason=theta)
solutions.append(solution)
if len(T) == 1:
return solutions[0]
else:
return IKSolution(
np.vstack([sol.q for sol in solutions]),
np.array([sol.success for sol in solutions]),
[sol.reason for sol in solutions],
)
[docs] def config_validate(self, config, allowables):
"""
Validate a configuration string
:param config: a configuration string
:type config: str
:param allowable: [description]
:type allowable: tuple of str
:raises ValueError: bad character in configuration string
:return: configuration string
:rtype: str
For analytic inverse kinematics the Toolbox uses a string whose
letters indicate particular solutions, eg. for the Puma 560
========= ===================
Character Meaning
========= ===================
'l' lefty
'r' righty
'u' elbow up
'd' elbow down
'n' wrist not flipped
'f' wrist flipped
========= ===================
This method checks that the configuration string is valid and adds
default values for missing characters. For example:
config = self.config_validate(config, ('lr', 'ud', 'nf'))
indicates the valid characters, and the first character in each
string is the default, ie. if neither 'l' or 'r' is given then
'l' will be added to the string.
"""
for c in config:
if not any([c in allowable for allowable in allowables]):
raise ValueError(f"bad config specifier <{c}>")
for allowable in allowables:
if all([a not in config for a in allowable]):
config += allowable[0]
return config
# -------------------------------------------------------------------------- #
[docs] def ik_lm_chan(
self,
Tep: Union[np.ndarray, SE3],
q0: Union[np.ndarray, None] = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
reject_jl: bool = True,
we: Union[np.ndarray, None] = None,
λ: float = 1.0,
) -> Tuple[np.ndarray, int, int, int, float]:
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method)
:param Tep: The desired end-effector pose or pose trajectory
:param q0: initial joint configuration (default to random valid joint
configuration contrained by the joint limits of the robot)
:param ilimit: maximum number of iterations per search
:param slimit: maximum number of search attempts
:param tol: final error tolerance
:param reject_jl: constrain the solution to being within the joint limits of
the robot (reject solution with invalid joint configurations and perfrom
another search up to the slimit)
:param we: a mask vector which weights the end-effector error priority.
Corresponds to translation in X, Y and Z and rotation about X, Y and Z
respectively
:param λ: value of lambda for the damping matrix Wn
:return: inverse kinematic solution
:rtype: tuple (q, success, iterations, searches, residual)
``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding
to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
This method can be used for robots with any number of degrees of freedom.
The return value ``sol`` is a tuple with elements:
============ ========== ===============================================
Element Type Description
============ ========== ===============================================
``q`` ndarray(n) joint coordinates in units of radians or metres
``success`` int whether a solution was found
``iterations`` int total number of iterations
``searches`` int total number of searches
``residual`` float final value of cost function
============ ========== ===============================================
If ``success == 0`` the ``q`` values will be valid numbers, but the
solution will be in error. The amount of error is indicated by
the ``residual``.
**Joint Limits**:
``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method.
The solver will initialise a solution attempt with a random valid q0 and
perform a maximum of ilimit steps within this attempt. If a solution is not
found, this process is repeated up to slimit times.
**Global search**:
``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method.
By setting reject_jl to True, the solver will discard any solution which
violates the defined joint limits of the robot. The solver will then
re-initialise with a new random q0 and repeat the process up to slimit times.
Note that finding a solution with valid joint coordinates takes longer than
without.
**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 ``we`` option where the ``we`` vector
(6) specifies the Cartesian DOF (in the wrist coordinate frame) that
will be ignored in reaching a solution. The we vector has six
elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value can be 0 (for ignore)
or above to assign a priority relative to other Cartesian DoF. 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 ``we=[1, 1, 1, 0, 0, 0]``.
.. note::
- See `Toolbox kinematics wiki page
<https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
- Implements a Levenberg-Marquadt variable-damping solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
:references:
TODO
:seealso:
TODO
"""
return self.ets().ik_lm_chan(Tep, q0, ilimit, slimit, tol, reject_jl, we, λ)
[docs] def ik_lm_wampler(
self,
Tep: Union[np.ndarray, SE3],
q0: Union[np.ndarray, None] = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
reject_jl: bool = True,
we: Union[np.ndarray, None] = None,
λ: float = 1.0,
) -> Tuple[np.ndarray, int, int, int, float]:
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization (Wamplers's Method)
:param Tep: The desired end-effector pose or pose trajectory
:param q0: initial joint configuration (default to random valid joint
configuration contrained by the joint limits of the robot)
:param ilimit: maximum number of iterations per search
:param slimit: maximum number of search attempts
:param tol: final error tolerance
:param reject_jl: constrain the solution to being within the joint limits of
the robot (reject solution with invalid joint configurations and perfrom
another search up to the slimit)
:param we: a mask vector which weights the end-effector error priority.
Corresponds to translation in X, Y and Z and rotation about X, Y and Z
respectively
:param λ: value of lambda for the damping matrix Wn
:return: inverse kinematic solution
:rtype: tuple (q, success, iterations, searches, residual)
``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding
to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
This method can be used for robots with any number of degrees of freedom.
The return value ``sol`` is a tuple with elements:
============ ========== ===============================================
Element Type Description
============ ========== ===============================================
``q`` ndarray(n) joint coordinates in units of radians or metres
``success`` int whether a solution was found
``iterations`` int total number of iterations
``searches`` int total number of searches
``residual`` float final value of cost function
============ ========== ===============================================
If ``success == 0`` the ``q`` values will be valid numbers, but the
solution will be in error. The amount of error is indicated by
the ``residual``.
**Joint Limits**:
``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method.
The solver will initialise a solution attempt with a random valid q0 and
perform a maximum of ilimit steps within this attempt. If a solution is not
found, this process is repeated up to slimit times.
**Global search**:
``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method.
By setting reject_jl to True, the solver will discard any solution which
violates the defined joint limits of the robot. The solver will then
re-initialise with a new random q0 and repeat the process up to slimit times.
Note that finding a solution with valid joint coordinates takes longer than
without.
**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 ``we`` option where the ``we`` vector
(6) specifies the Cartesian DOF (in the wrist coordinate frame) that
will be ignored in reaching a solution. The we vector has six
elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value can be 0 (for ignore)
or above to assign a priority relative to other Cartesian DoF. 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 ``we=[1, 1, 1, 0, 0, 0]``.
.. note::
- See `Toolbox kinematics wiki page
<https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
- Implements a Levenberg-Marquadt variable-damping solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
:references:
TODO
:seealso:
TODO
"""
return self.ets().ik_lm_wampler(Tep, q0, ilimit, slimit, tol, reject_jl, we, λ)
[docs] def ik_lm_sugihara(
self,
Tep: Union[np.ndarray, SE3],
q0: Union[np.ndarray, None] = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
reject_jl: bool = True,
we: Union[np.ndarray, None] = None,
λ: float = 1.0,
) -> Tuple[np.ndarray, int, int, int, float]:
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization (Sugihara's Method)
:param Tep: The desired end-effector pose or pose trajectory
:param q0: initial joint configuration (default to random valid joint
configuration contrained by the joint limits of the robot)
:param ilimit: maximum number of iterations per search
:param slimit: maximum number of search attempts
:param tol: final error tolerance
:param reject_jl: constrain the solution to being within the joint limits of
the robot (reject solution with invalid joint configurations and perfrom
another search up to the slimit)
:param we: a mask vector which weights the end-effector error priority.
Corresponds to translation in X, Y and Z and rotation about X, Y and Z
respectively
:param λ: value of lambda for the damping matrix Wn
:return: inverse kinematic solution
:rtype: tuple (q, success, iterations, searches, residual)
``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding
to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
This method can be used for robots with any number of degrees of freedom.
The return value ``sol`` is a tuple with elements:
============ ========== ===============================================
Element Type Description
============ ========== ===============================================
``q`` ndarray(n) joint coordinates in units of radians or metres
``success`` int whether a solution was found
``iterations`` int total number of iterations
``searches`` int total number of searches
``residual`` float final value of cost function
============ ========== ===============================================
If ``success == 0`` the ``q`` values will be valid numbers, but the
solution will be in error. The amount of error is indicated by
the ``residual``.
**Joint Limits**:
``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method.
The solver will initialise a solution attempt with a random valid q0 and
perform a maximum of ilimit steps within this attempt. If a solution is not
found, this process is repeated up to slimit times.
**Global search**:
``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method.
By setting reject_jl to True, the solver will discard any solution which
violates the defined joint limits of the robot. The solver will then
re-initialise with a new random q0 and repeat the process up to slimit times.
Note that finding a solution with valid joint coordinates takes longer than
without.
**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 ``we`` option where the ``we`` vector
(6) specifies the Cartesian DOF (in the wrist coordinate frame) that
will be ignored in reaching a solution. The we vector has six
elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value can be 0 (for ignore)
or above to assign a priority relative to other Cartesian DoF. 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 ``we=[1, 1, 1, 0, 0, 0]``.
.. note::
- See `Toolbox kinematics wiki page
<https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
- Implements a Levenberg-Marquadt variable-damping solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
:references:
TODO
:seealso:
TODO
"""
return self.ets().ik_lm_sugihara(Tep, q0, ilimit, slimit, tol, reject_jl, we, λ)
[docs] def ik_nr(
self,
Tep: Union[np.ndarray, SE3],
q0: Union[np.ndarray, None] = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
reject_jl: bool = True,
we: Union[np.ndarray, None] = None,
use_pinv: int = True,
pinv_damping: float = 0.0,
) -> Tuple[np.ndarray, int, int, int, float]:
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization (Newton-Raphson Method)
:param Tep: The desired end-effector pose or pose trajectory
:param q0: initial joint configuration (default to random valid joint
configuration contrained by the joint limits of the robot)
:param ilimit: maximum number of iterations per search
:param slimit: maximum number of search attempts
:param tol: final error tolerance
:param reject_jl: constrain the solution to being within the joint limits of
the robot (reject solution with invalid joint configurations and perfrom
another search up to the slimit)
:param we: a mask vector which weights the end-effector error priority.
Corresponds to translation in X, Y and Z and rotation about X, Y and Z
respectively
:param λ: value of lambda for the damping matrix Wn
:return: inverse kinematic solution
:rtype: tuple (q, success, iterations, searches, residual)
``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding
to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
This method can be used for robots with any number of degrees of freedom.
The return value ``sol`` is a tuple with elements:
============ ========== ===============================================
Element Type Description
============ ========== ===============================================
``q`` ndarray(n) joint coordinates in units of radians or metres
``success`` int whether a solution was found
``iterations`` int total number of iterations
``searches`` int total number of searches
``residual`` float final value of cost function
============ ========== ===============================================
If ``success == 0`` the ``q`` values will be valid numbers, but the
solution will be in error. The amount of error is indicated by
the ``residual``.
**Joint Limits**:
``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method.
The solver will initialise a solution attempt with a random valid q0 and
perform a maximum of ilimit steps within this attempt. If a solution is not
found, this process is repeated up to slimit times.
**Global search**:
``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method.
By setting reject_jl to True, the solver will discard any solution which
violates the defined joint limits of the robot. The solver will then
re-initialise with a new random q0 and repeat the process up to slimit times.
Note that finding a solution with valid joint coordinates takes longer than
without.
**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 ``we`` option where the ``we`` vector
(6) specifies the Cartesian DOF (in the wrist coordinate frame) that
will be ignored in reaching a solution. The we vector has six
elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value can be 0 (for ignore)
or above to assign a priority relative to other Cartesian DoF. 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 ``we=[1, 1, 1, 0, 0, 0]``.
.. note::
- See `Toolbox kinematics wiki page
<https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
- Implements a Levenberg-Marquadt variable-damping solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
:references:
TODO
:seealso:
TODO
"""
return self.ets().ik_nr(
Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping
)
[docs] def ik_gn(
self,
Tep: Union[np.ndarray, SE3],
q0: Union[np.ndarray, None] = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
reject_jl: bool = True,
we: Union[np.ndarray, None] = None,
use_pinv: int = True,
pinv_damping: float = 0.0,
) -> Tuple[np.ndarray, int, int, int, float]:
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization (Gauss-Newton Method)
:param Tep: The desired end-effector pose or pose trajectory
:param q0: initial joint configuration (default to random valid joint
configuration contrained by the joint limits of the robot)
:param ilimit: maximum number of iterations per search
:param slimit: maximum number of search attempts
:param tol: final error tolerance
:param reject_jl: constrain the solution to being within the joint limits of
the robot (reject solution with invalid joint configurations and perfrom
another search up to the slimit)
:param we: a mask vector which weights the end-effector error priority.
Corresponds to translation in X, Y and Z and rotation about X, Y and Z
respectively
:param λ: value of lambda for the damping matrix Wn
:return: inverse kinematic solution
:rtype: tuple (q, success, iterations, searches, residual)
``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding
to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
This method can be used for robots with any number of degrees of freedom.
The return value ``sol`` is a tuple with elements:
============ ========== ===============================================
Element Type Description
============ ========== ===============================================
``q`` ndarray(n) joint coordinates in units of radians or metres
``success`` int whether a solution was found
``iterations`` int total number of iterations
``searches`` int total number of searches
``residual`` float final value of cost function
============ ========== ===============================================
If ``success == 0`` the ``q`` values will be valid numbers, but the
solution will be in error. The amount of error is indicated by
the ``residual``.
**Joint Limits**:
``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method.
The solver will initialise a solution attempt with a random valid q0 and
perform a maximum of ilimit steps within this attempt. If a solution is not
found, this process is repeated up to slimit times.
**Global search**:
``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method.
By setting reject_jl to True, the solver will discard any solution which
violates the defined joint limits of the robot. The solver will then
re-initialise with a new random q0 and repeat the process up to slimit times.
Note that finding a solution with valid joint coordinates takes longer than
without.
**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 ``we`` option where the ``we`` vector
(6) specifies the Cartesian DOF (in the wrist coordinate frame) that
will be ignored in reaching a solution. The we vector has six
elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value can be 0 (for ignore)
or above to assign a priority relative to other Cartesian DoF. 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 ``we=[1, 1, 1, 0, 0, 0]``.
.. note::
- See `Toolbox kinematics wiki page
<https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
- Implements a Levenberg-Marquadt variable-damping solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess ``q0``.
:references:
TODO
:seealso:
TODO
"""
return self.ets().ik_gn(
Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping
)
[docs] def ikine_LM(
self,
Tep: Union[np.ndarray, SE3],
q0: Union[ArrayLike, None] = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
joint_limits: bool = False,
mask: Union[ArrayLike, None] = None,
seed: Union[int, None] = None,
):
return self.ets().ikine_LM(
Tep=Tep,
q0=q0,
ilimit=ilimit,
slimit=slimit,
tol=tol,
joint_limits=joint_limits,
mask=mask,
seed=seed,
)
class SerialLink(DHRobot):
def __init__(self, *args, **kwargs):
warnings.warn(
"SerialLink is deprecated, use DHRobot instead", DeprecationWarning
)
super().__init__(*args, **kwargs)
def _cross(a, b):
return np.r_[
a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]
]
if __name__ == "__main__": # pragma nocover
import roboticstoolbox as rtb
# import spatialmath.base.symbolic as sym
# planar = rtb.models.DH.Planar2()
# J = puma.jacob0(puma.qn)
# print(J)
# print(puma.manipulability(puma.qn))
# print(puma.manipulability(puma.qn, 'asada'))
# tw, T0 = puma.twists(puma.qz)
# print(planar)
puma = rtb.models.DH.Puma560()
print(puma)
# print(puma.jacob0(puma.qn, analytical="eul"))
# puma.base = None
# print('base', puma.base)
# print('tool', puma.tool)
# print(puma.ets())
# puma[2].flip = True
# puma[3].offset = 1
# puma[4].flip = True
# puma[4].offset = -1
# print(puma)
# print(puma.ets())
# print(puma.dyntable())