Correctly recognise static ET with symbolic values

Author: jhavl

roboticstoolbox/core/fknm.cpp

@@ -1124,8 +1124,9 @@ extern "C"
 
         et = (ET *)PyMem_RawMalloc(sizeof(ET));
 
-        if (!PyArg_ParseTuple(args, "OiiiiO!O!",
+        if (!PyArg_ParseTuple(args, "OiiiiiO!O!",
                               &py_et,
+                              &et->isstaticsym,
                               &isjoint,
                               &isflip,
                               &jindex,
@@ -1188,7 +1189,8 @@ extern "C"
 
         et = (ET *)PyMem_RawMalloc(sizeof(ET));
 
-        if (!PyArg_ParseTuple(args, "iiiiO!O!",
+        if (!PyArg_ParseTuple(args, "iiiiiO!O!",
+                              &et->isstaticsym,
                               &et->isjoint,
                               &et->isflip,
                               &et->jindex,
@@ -1252,6 +1254,12 @@ extern "C"
         if (!(et = (ET *)PyCapsule_GetPointer(py_et, "ET")))
             return NULL;
 
+        if (et->isstaticsym)
+        {
+            PyErr_SetString(PyExc_TypeError, "Symbolic value");
+            return NULL;
+        }
+
         if (py_eta != Py_None)
         {
             if (PyFloat_Check(py_eta))

roboticstoolbox/core/structs.h

@@ -40,6 +40,7 @@ extern "C"
 
         struct ET
         {
+                int isstaticsym; /* this ET is static and has a symbolic value */
                 int isjoint;
                 int isflip;
                 int jindex;

roboticstoolbox/robot/ET.py

@@ -46,6 +46,10 @@ def __init__(
     ):
         self._axis = axis
 
+        # A flag to check if the ET is a static joint with a symbolic value
+        # Defaults to False as is set to True if eta is a symbol below
+        self._isstaticsym = False
+
         if eta is None:
             self._eta = None
         else:
@@ -74,6 +78,10 @@ def __init__(
                 self._joint = False
                 self._T = T.copy(order="F")
         else:
+            # This is a static joint
+            if issymbol(eta):
+                self._isstaticsym = True
+
             self._joint = False
             if axis_func is not None:
                 self._T = axis_func(self.eta).copy(order="F")
@@ -102,6 +110,7 @@ def __init_c(self):
             qlim = self.qlim
 
         return ET_init(
+            self._isstaticsym,
             self.isjoint,
             self.isflip,
             jindex,
@@ -126,6 +135,7 @@ def __update_c(self):
 
         ET_update(
             self.fknm,
+            self._isstaticsym,
             self.isjoint,
             self.isflip,
             jindex,
@@ -135,7 +145,6 @@ def __update_c(self):
         )
 
     def __str__(self):
-
         eta_str = ""
 
         if self.isjoint:
@@ -175,7 +184,6 @@ def __str__(self):
         return f"{self.axis}({eta_str})"
 
     def __repr__(self):
-
         s_eta = "" if self.eta is None else f"eta={self.eta}"
         s_T = (
             f"T={repr(self._T)}"
@@ -553,7 +561,7 @@ def A(self, q: Union[float, Sym] = 0.0) -> ndarray:
             # We can't use the fast version, lets use Python instead
             if self.isjoint:
                 if self.isflip:
-                    q = -1.0 * q
+                    q = -q
 
                 if self.axis_func is not None:
                     return self.axis_func(q)