8000 add property for ET screw axis · stonebig/robotics-toolbox-python@cc56130 · GitHub
[go: up one dir, main page]

Skip to content

Commit cc56130

Browse files
committed
add property for ET screw axis
add method to get set of joint indices
1 parent 85f5c69 commit cc56130

File tree

1 file changed

+40
-6
lines changed

1 file changed

+40
-6
lines changed

roboticstoolbox/robot/ETS.py

Lines changed: 40 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -333,6 +333,23 @@ def joints(self):
333333
"""
334334
return np.where([e.isjoint for e in self])[0]
335335

336+
def jointset(self):
337+
"""
338+
Get set of joint indices
339+
340+
:return: set of unique joint indices
341+
:rtype: set
342+
343+
Example:
344+
345+
.. runblock:: pycon
346+
347+
>>> from roboticstoolbox import ETS
348+
>>> e = ETS.rz(j=1) * ETS.tx(j=2) * ETS.rz(j=1) * ETS.tx(1)
349+
>>> e.jointset()
350+
"""
351+
return set([self[j].jindex for j in self.joints()])
352+
336353
def T(self, q=None):
337354
"""
338355
Evaluate an elementary transformation
@@ -824,6 +841,23 @@ class ETS(SuperETS):
824841
:seealso: :func:`rx`, :func:`ry`, :func:`rz`, :func:`tx`,
825842
:func:`ty`, :func:`tz`
826843
"""
844+
@property
845+
def s(self):
846+
if self.axis[1] == 'x':
847+
if self.axis[0] == 'R':
848+
return np.r_[0, 0, 0, 1, 0, 0]
849+
else:
850+
return np.r_[1, 0, 0, 0, 0, 0]
851+
elif self.axis[1] == 'y':
852+
if self.axis[0] == 'R':
853+
return np.r_[0, 0, 0, 0, 1, 0]
854+
else:
855+
return np.r_[0, 1, 0, 0, 0, 0]
856+
else:
857+
if self.axis[0] == 'R':
858+
return np.r_[0, 0, 0, 0, 0, 1]
859+
else:
860+
return np.r_[0, 0, 1, 0, 0, 0]
827861

828862
@classmethod
829863
def rx(cls, eta=None, unit='rad', **kwargs):
@@ -1036,16 +1070,16 @@ def jacob0(self, q=None, T=None):
10361070
:math:`\mathbf{E}(q)`.
10371071
10381072
.. math::
1039-
1040-
{}^0 T_e &= \mathbf{E}(q) \in \mbox{SE}(3)
1073+
1074+
{}^0 T_e = \mathbf{E}(q) \in \mbox{SE}(3)
10411075
10421076
The temporal derivative of this is the spatial
10431077
velocity :math:`\nu` which is a 6-vector is related to the rate of
10441078
change of joint coordinates by the Jacobian matrix.
10451079
10461080
.. math::
10471081
1048-
{}^0 \nu & = {}^0 \mathbf{J}(q) \dot{q} \in \mathbb{R}^6
1082+
{}^0 \nu = {}^0 \mathbf{J}(q) \dot{q} \in \mathbb{R}^6
10491083
10501084
This velocity can be expressed relative to the {0} frame or the {e}
10511085
frame.
@@ -1173,15 +1207,15 @@ def hessian0(self, q=None, J0=None):
11731207
11741208
.. math::
11751209
1176-
{}^0 T_e &= \mathbf{E}(q) \in \mbox{SE}(3)
1210+
{}^0 T_e = \mathbf{E}(q) \in \mbox{SE}(3)
11771211
11781212
The temporal derivative of this is the spatial
11791213
velocity :math:`\nu` which is a 6-vector is related to the rate of
11801214
change of joint coordinates by the Jacobian matrix.
11811215
11821216 8A89
.. math::
11831217
1184-
{}^0 \nu & = {}^0 \mathbf{J}(q) \dot{q} \in \mathbb{R}^6
1218+
{}^0 \nu = {}^0 \mathbf{J}(q) \dot{q} \in \mathbb{R}^6
11851219
11861220
This velocity can be expressed relative to the {0} frame or the {e}
11871221
frame.
@@ -1191,7 +1225,7 @@ def hessian0(self, q=None, J0=None):
11911225
11921226
.. math::
11931227
1194-
{}^0 \dot{\nu} &= \mathbf{J}(q) \ddot{q} + \dot{\mathbf{J}}(q) \dot{q} \in \mathbb{R}^6 \\
1228+
{}^0 \dot{\nu} = \mathbf{J}(q) \ddot{q} + \dot{\mathbf{J}}(q) \dot{q} \in \mathbb{R}^6 \\
11951229
&= \mathbf{J}(q) \ddot{q} + \dot{q}^T \mathbf{H}(q) \dot{q}
11961230
11971231
The manipulator Hessian tensor :math:`H` maps joint velocity to

0 commit comments

Comments
 (0)
0