Skip to content

Commit ba9dffd

Browse files
petercorkepetercorke
committed
add Hessian method to DHRobot, via ETS, move jacob_dot to Robot
1 parent a893978 commit ba9dffd

File tree

2 files changed

+81
-109
lines changed

2 files changed

+81
-109
lines changed

‎roboticstoolbox/robot/DHRobot.py

Lines changed: 44 additions & 109 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
from collections import namedtuple
77
from roboticstoolbox.tools.data import path_to_datafile
8+
import warnings
89

910
import numpy as np
1011
from roboticstoolbox.robot.Robot import Robot # DHLink
@@ -1057,131 +1058,62 @@ def jacob0(self, q=None, T=None, half=None, analytical=None):
10571058
raise ValueError('bad half specified')
10581059
return J0
10591060

1060-
def jacob_dot(self, q=None, qd=None):
1061-
r"""
1062-
Derivative of Jacobian
1063-
1064-
:param q: The joint configuration of the robot (Optional,
1065-
if not supplied will use the stored q values).
1066-
:type q: float ndarray(n)
1067-
:param qd: The joint configuration of the robot (Optional,
1068-
if not supplied will use the stored qd values).
1069-
:type qd: ndarray(n)
1070-
:return: The derivative of the manipulator Jacobian
1071-
:rtype: ndarray(n)
1072-
1073-
``robot.jacob_dot(q, qd)`` computes the product (6) of the temporal
1074-
derivative of the manipulator Jacobian (in the world frame) and the
1075-
joint rates :math:`\frac{d}{dt} \dot{\mathbf{J}}(q) \dot{q}`.
1076-
1077-
.. note::
1078-
- This term appears in the formulation for operational space
1079-
control :math:`\ddot{x} = \mathbf{J}(q) \ddot{q} + \dot{\mathbf{J}}(q) \dot{q}`.
1080-
- Written as per the reference and not very efficient.
1081-
1082-
:references:
1083-
- Fundamentals of Robotics Mechanical Systems (2nd ed)
1084-
J. Angleles, Springer 2003.
1085-
- A unified approach for motion and force control of robot
1086-
manipulators: The operational space formulation
1087-
O Khatib, IEEE Journal on Robotics and Automation, 1987.
1088-
1089-
""" # noqa
1090-
1091-
if q is None:
1092-
q = self.q
1093-
1094-
if qd is None:
1095-
qd = self.qd
10961061

1097-
q = getvector(q, self.n)
1098-
qd = getvector(qd, self.n)
1099-
1100-
# Using the notation of Angeles:
1101-
# [Q,a] ~ [R,t] the per link transformation
1102-
# P ~ R the cumulative rotation t2r(Tj) in world frame
1103-
# e the last column of P, the local frame z axis in world
1104-
# coordinates
1105-
# w angular velocity in base frame
1106-
# ed deriv of e
1107-
# r is distance from final frame
1108-
# rd deriv of r
1109-
# ud ??
1110-
1111-
Q = np.zeros((3, 3, self.n))
1112-
a = np.zeros((3, self.n))
1113-
P = np.zeros((3, 3, self.n))
1114-
e = np.zeros((3, self.n))
1115-
w = np.zeros((3, self.n))
1116-
ed = np.zeros((3, self.n))
1117-
rd = np.zeros((3, self.n))
1118-
r = np.zeros((3, self.n))
1119-
ud = np.zeros((3, self.n))
1120-
v = np.zeros((6, self.n))
1121-
1122-
for i in range(self.n):
1123-
T = self.links[i].A(q[i])
1124-
Q[:, :, i] = T.R
1125-
a[:, i] = T.t
1126-
1127-
P[:, :, 0] = Q[:, :, 0]
1128-
e[:, 0] = [0, 0, 1]
1129-
1130-
for i in range(1, self.n):
1131-
P[:, :, i] = P[:, :, i - 1] @ Q[:, :, i]
1132-
e[:, i] = P[:, 2, i]
1062+
def hessian0(self, q=None, J0=None):
1063+
r"""
1064+
Manipulator Hessian in base frame
11331065
1134-
# Step 1
1135-
w[:, 0] = qd[0] * e[:, 0]
1066+
:param q: joint coordinates
1067+
:type q: array_like
1068+
:param J0: Jacobian in {0} frame
1069+
:type J0: ndarray(6,n)
1070+
:return: Hessian matrix
1071+
:rtype: ndarray(6,n,n)
11361072
1137-
for i in range(self.n - 1):
1138-
w[:, i + 1] = (
1139-
qd[i + 1] *
1140-
np.array([0, 0, 1]) +
1141-
Q[:, :, i].T @ w[:, i])
1073+
This method calculcates the Hessisan in the base frame. One of ``J0`` or
1074+
``q`` is required. If ``J0`` is already calculated for the joint
1075+
coordinates ``q`` it can be passed in to to save computation time.
11421076
1143-
# Step 2
1144-
ed[:, 0] = np.array([0, 0, 1])
1077+
If we take the time derivative of the differential kinematic
1078+
relationship
11451079
1146-
for i in range(1, self.n):
1147-
ed[:, i] = np.cross(w[:, i], e[:, i])
1080+
.. math::
11481081
1149-
# Step 3
1150-
rd[:, self.n - 1] = np.cross(w[:, self.n - 1], a[:, self.n - 1])
1082+
\nu &= \mat{J}(\vec{q}) \dvec{q} \\
1083+
\alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q}
11511084
1152-
for i in range(self.n - 2, -1, -1):
1153-
rd[:, i] = np.cross(w[:, i], a[:, i]) + Q[:, :, i] @ rd[:, i + 1]
1085+
where
11541086
1155-
r[:, self.n - 1] = a[:, self.n - 1]
1087+
.. math::
11561088
1157-
for i in range(self.n - 2, -1, -1):
1158-
r[:, i] = a[:, i] + Q[:, :, i] @ r[:, i + 1]
1089+
\dmat{J} = \mat{H} \dvec{q}
1090+
1091+
and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the
1092+
Hessian tensor.
11591093
1160-
ud[:, 0] = np.cross(e[:, 0], rd[:, 0])
1094+
The elements of the Hessian are
11611095
1162-
for i in range(1, self.n):
1163-
ud[:, i] = \
1164-
np.cross(ed[:, i], r[:, i]) + np.cross(e[:, i], rd[:, i])
1096+
.. math::
1097+
1098+
\mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k}
11651099
1166-
# Step 4
1167-
# Swap ud and ed
1168-
v[:, self.n - 1] = \
1169-
qd[self.n - 1] * np.r_[ud[:, self.n - 1], ed[:, self.n - 1]]
1100+
where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements
1101+
of the spatial velocity vector.
11701102
1171-
for i in range(self.n - 2, -1, -1):
1172-
Ui = np.r_[
1173-
np.c_[Q[:, :, i], np.zeros((3, 3))],
1174-
np.c_[np.zeros((3, 3)), Q[:, :, i]]]
1103+
Similarly, we can write
11751104
1176-
v[:, i] = (
1177-
qd[i] *
1178-
np.r_[ud[:, i], ed[:, i]] +
1179-
Ui @ v[:, i + 1])
1105+
.. math::
1106+
1107+
\mat{J}_{i,j} = \frac{d u_i}{d q_j}
11801108
1181-
Jdot = v[:, 0]
1109+
:references:
1110+
- Kinematic Derivatives using the Elementary Transform
1111+
Sequence, J. Haviland and P. Corke
11821112
1183-
return Jdot
1113+
:seealso: :func:`jacob0`, :func:`jacob_dot`
1114+
"""
11841115

1116+
return self.ets().hessian0(q, J0)
11851117
# -------------------------------------------------------------------------- #
11861118

11871119
def _init_rne(self):
@@ -1720,7 +1652,10 @@ def config_validate(self, config, allowables):
17201652
return config
17211653
class SerialLink(DHRobot):
17221654
def __init__(self, *args, **kwargs):
1723-
print('SerialLink is deprecated, use DHRobot instead')
1655+
warnings.warn(
1656+
'SerialLink is deprecated, use DHRobot instead',
1657+
DeprecationWarning
1658+
)
17241659
super().__init__(*args, **kwargs)
17251660

17261661

‎roboticstoolbox/robot/Robot.py

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -561,6 +561,43 @@ def asada(robot, J, q, axes, **kwargs):
561561
else:
562562
return w
563563

564+
def jacob_dot(self, q=None, qd=None, J0=None):
565+
r"""
566+
Derivative of Jacobian
567+
568+
:param q: The joint configuration of the robot
569+
:type q: float ndarray(n)
570+
:param qd: The joint velocity of the robot
571+
:type qd: ndarray(n)
572+
:param J0: Jacobian in {0} frame
573+
:type J0: ndarray(6,n)
574+
:return: The derivative of the manipulator Jacobian
575+
:rtype: ndarray(6,n)
576+
577+
``robot.jacob_dot(q, qd)`` computes the rate of change of the
578+
Jacobian elements. If ``J0`` is already calculated for the joint
579+
coordinates ``q`` it can be passed in to to save computation time.
580+
581+
It is computed as the mode-3 product of the Hessian tensor and the
582+
velocity vector.
583+
584+
:references:
585+
- Kinematic Derivatives using the Elementary Transform
586+
Sequence, J. Haviland and P. Corke
587+
588+
:seealso: :func:`jacob0`, :func:`hessian0`
589+
""" # noqa
590+
n = len(q)
591+
if J0 is None:
592+
J0 = self.jacob0(q)
593+
H = self.hessian0(q, J0)
594+
595+
# Jd = H qd using mode 3 product
596+
Jd = np.zeros((6, n))
597+
for i in range(n):
598+
Jd += H[:, :, i] * qd[i]
599+
600+
return Jd
564601
# --------------------------------------------------------------------- #
565602

566603
@property

0 commit comments

Comments
 (0)