Add symbolic simplification to ETS fkine methods
@@ -19,6 +19,7 @@ from spatialmath.base import (
trlog,
t2r,
angvelxform,
+ simplify,
)
from roboticstoolbox import rtb_get_param
@@ -704,6 +705,10 @@ class ETS(BaseETS):
ret = SE3.Empty()
fk = self.eval(q, base, tool, include_base)
+ if fk.dtype == 'O':
+ # symbolic
+ fk = simplify(fk)
+
if fk.ndim == 3:
for T in fk:
print(T)
@@ -1471,6 +1476,10 @@ class ETS2(BaseETS):
else:
ret = SE2(Tk, check=False)
+ if ret.A.dtype == 'O':
+ # if symbolic, simplify it
+ ret = ret.simplify()
+
return ret
def jacob0(
