Don't emit constant term if it's identity matrix
@@ -10,7 +10,7 @@ from abc import ABC
import numpy as np
from spatialmath import SE3, SE2
from spatialmath.base import getvector, getunit, trotx, troty, trotz, \
- issymbol, tr2jac, transl2, trot2, removesmall, trinv, verifymatrix
+ issymbol, tr2jac, transl2, trot2, removesmall, trinv, verifymatrix, iseye
class SuperETS(UserList, ABC):
@@ -477,6 +477,7 @@ class SuperETS(UserList, ABC):
# a joint
if const is not None:
# flush the constant
+ if not iseye(const):
ets *= ETS._CONST(const)
const = None
ets *= et # emit the joint ET
@@ -489,6 +490,7 @@ class SuperETS(UserList, ABC):
if const is not None:
# flush the constant, tool transform
+ if not iseye(const):
ets *= ETS._CONST(const)
return ets
