Merge branch 'future' of https://github.com/petercorke/robotics-toolbox-python into future

petercorke · petercorke · commit 832d35aeb14c · 2021-06-15T16:23:55.000+10:00
diff --git a/roboticstoolbox/core/fknm.c b/roboticstoolbox/core/fknm.c
@@ -1114,8 +1114,10 @@ int _inv(npy_float64 *m, npy_float64 *invOut)
 
     det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
 
-    if (det == 0)
+    if (det == 0) {
+        free(inv);
         return 0;
+    }
 
     det = 1.0 / det;
 
diff --git a/roboticstoolbox/core/frne.c b/roboticstoolbox/core/frne.c
@@ -106,7 +106,8 @@ static PyObject *delete(PyObject *self, PyObject *args) {
 static PyObject *frne(PyObject *self, PyObject *args) {
 
     Robot *robot;
-    PyObject *rO, *qO, *qdO, *qddO, *gravO, *fextO;
+    PyObject *rO, *qO, *qdO, *qddO, *gravO, *fextO, *temp;
+    // PyObject *python_float;
     double  *q, *qd, *qdd, *fext;
     // Vect *grav;
     int nq = 1; //, nqd = njoints, nqdd = njoints;
@@ -127,7 +128,6 @@ static PyObject *frne(PyObject *self, PyObject *args) {
     qd = (double *)PyMem_RawCalloc(njoints, sizeof(double));
     qdd = (double *)PyMem_RawCalloc(njoints, sizeof(double));
     fext = (double *)PyMem_RawCalloc(6, sizeof(double));
-    // grav = (Vect *)PyMem_RawMalloc(sizeof(Vect));
 
     // Create iterators for arrays
     PyObject *iq = PyObject_GetIter(qO);
@@ -137,27 +137,45 @@ static PyObject *frne(PyObject *self, PyObject *args) {
     PyObject *ifext = PyObject_GetIter(fextO);
 
     // Create the gravity vector
-    robot->gravity->x = PyFloat_AsDouble(PyIter_Next(igrav));
-    robot->gravity->y = PyFloat_AsDouble(PyIter_Next(igrav));
-    robot->gravity->z = PyFloat_AsDouble(PyIter_Next(igrav));
+    temp = PyIter_Next(igrav);
+    robot->gravity->x = PyFloat_AsDouble(temp);
+    Py_DECREF(temp);
+
+    temp = PyIter_Next(igrav);
+    robot->gravity->y = PyFloat_AsDouble(temp);
+    Py_DECREF(temp);
+
+    temp = PyIter_Next(igrav);
+    robot->gravity->z = PyFloat_AsDouble(temp);
+    Py_DECREF(temp);
+
 
     // Create the joint arrays
     for (int i = 0; i < njoints; i++) {
-        q[i] = PyFloat_AsDouble(PyIter_Next(iq));
-        qd[i] = PyFloat_AsDouble(PyIter_Next(iqd));
-        qdd[i] = PyFloat_AsDouble(PyIter_Next(iqdd));
+        temp = PyIter_Next(iq);
+        q[i] = PyFloat_AsDouble(temp);
+        Py_DECREF(temp);
+
+        temp = PyIter_Next(iqd);
+        qd[i] = PyFloat_AsDouble(temp);
+        Py_DECREF(temp);
+
+        temp = PyIter_Next(iqdd);
+        qdd[i] = PyFloat_AsDouble(temp);
+        Py_DECREF(temp);
     }
 
     // Create the fext array
     for (int i = 0; i < 6; i++) {
-        fext[i] = PyFloat_AsDouble(PyIter_Next(ifext));
+        temp = PyIter_Next(ifext);
+        fext[i] = PyFloat_AsDouble(temp);
+        Py_DECREF(temp);
     }
 
     // Create a matrix for the return argument */
     double  *tau;
     tau = (double *)PyMem_RawCalloc(njoints, sizeof(double));
 
-
     #define MEL(x,R,C)  (x[(R)+(C)*nq])
 
     // // For each point in the input trajectory
@@ -185,14 +203,19 @@ static PyObject *frne(PyObject *self, PyObject *args) {
     PyMem_RawFree(q);
     PyMem_RawFree(qd);
     PyMem_RawFree(qdd);
-    // PyMem_RawFree(grav);
     PyMem_RawFree(fext);
 
+    Py_DECREF(iq);
+    Py_DECREF(iqd);
+    Py_DECREF(iqdd);
+    Py_DECREF(igrav);
+    Py_DECREF(ifext);
+
     PyObject* ret = PyList_New(njoints);
     for (int i = 0; i < njoints; ++i) {
-        PyObject* python_float = Py_BuildValue("d", tau[i]);
-        PyList_SetItem(ret, i, python_float);
+        PyList_SetItem(ret, i, Py_BuildValue("d", tau[i]));
     }
+
     PyMem_RawFree(tau);
 
     return ret;
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1275,6 +1275,9 @@ def fkine(
               Sequence, J. Haviland and P. Corke
         """
 
+        if start is not None:
+            include_base = False
+
         # Use c extension to calculate fkine
         if fast:
             path, _, etool = self.get_path(end, start, _fknm=True)
@@ -1286,10 +1289,10 @@ def fkine(
             T = np.empty((4, 4))
             fknm.fkine(m, path, q, etool, tool, T)
 
-            if not include_base:
-                return T
-            else:
+            if self._base is not None and start is None and include_base == True:
                 return self.base.A @ T
+            else:
+                return T
 
         # Otherwise use Python method
         # we work with NumPy arrays not SE2/3 classes for speed
@@ -1340,7 +1343,11 @@ def fkine(
                     break
 
             # add base transform if it is set
-            if self._base is not None and start == self.base_link:
+            if (
+                self._base is not None
+                and start == self.base_link
+                and include_base == True
+            ):
                 Tk = self.base.A @ Tk
 
             # cast to pose class and append
