@@ -333,6 +333,23 @@ def joints(self):
333
333
"""
334
334
return np .where ([e .isjoint for e in self ])[0 ]
335
335
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
+
336
353
def T (self , q = None ):
337
354
"""
338
355
Evaluate an elementary transformation
@@ -824,6 +841,23 @@ class ETS(SuperETS):
824
841
:seealso: :func:`rx`, :func:`ry`, :func:`rz`, :func:`tx`,
825
842
:func:`ty`, :func:`tz`
826
843
"""
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 ]
827
861
828
862
@classmethod
829
863
def rx (cls , eta = None , unit = 'rad' , ** kwargs ):
@@ -1036,16 +1070,16 @@ def jacob0(self, q=None, T=None):
1036
1070
:math:`\mathbf{E}(q)`.
1037
1071
1038
1072
.. 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)
1041
1075
1042
1076
The temporal derivative of this is the spatial
1043
1077
velocity :math:`\nu` which is a 6-vector is related to the rate of
1044
1078
change of joint coordinates by the Jacobian matrix.
1045
1079
1046
1080
.. math::
1047
1081
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
1049
1083
1050
1084
This velocity can be expressed relative to the {0} frame or the {e}
1051
1085
frame.
@@ -1173,15 +1207,15 @@ def hessian0(self, q=None, J0=None):
1173
1207
1174
1208
.. math::
1175
1209
1176
- {}^0 T_e & = \mathbf{E}(q) \in \mbox{SE}(3)
1210
+ {}^0 T_e = \mathbf{E}(q) \in \mbox{SE}(3)
1177
1211
1178
1212
The temporal derivative of this is the spatial
1179
1213
velocity :math:`\nu` which is a 6-vector is related to the rate of
1180
1214
change of joint coordinates by the Jacobian matrix.
1181
1215
1182
1216
8A89
.. math::
1183
1217
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
1185
1219
1186
1220
This velocity can be expressed relative to the {0} frame or the {e}
1187
1221
frame.
@@ -1191,7 +1225,7 @@ def hessian0(self, q=None, J0=None):
1191
1225
1192
1226
.. math::
1193
1227
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 \\
1195
1229
&= \mathbf{J}(q) \ddot{q} + \dot{q}^T \mathbf{H}(q) \dot{q}
1196
1230
1197
1231
The manipulator Hessian tensor :math:`H` maps joint velocity to
0 commit comments