First go at IK LM

Author: jhavl

roboticstoolbox/core/fknm.cpp

@@ -89,19 +89,25 @@ extern "C"
     static PyObject *IK(PyObject *self, PyObject *args)
     {
         ETS *ets;
-        npy_float64 *np_Tep, *np_ret;
-        PyObject *py_q, *py_Tep;
-        PyArrayObject *py_np_q, *py_np_Tep;
-        PyObject *py_ets;
-        int n;
-        PyObject *py_ret;
-        npy_intp dim[1] = {7};
+        npy_float64 *np_Tep, *np_ret, *np_q0;
+        PyArrayObject *py_np_Tep;
+        PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0;
+        PyObject *py_tup, *py_it, *py_search, *py_solution, *py_E;
+        npy_intp dim[1] = {1};
+        int ilimit, slimit, q0_used = 0, reject_jl;
+        double tol, E;
 
+        int it = 0, search = 0, solution = 0;
 
         if (!PyArg_ParseTuple(
-                args, "OO",
+                args, "OOOiidi",
                 &py_ets,
-                &py_Tep))
+                &py_Tep,
+                &py_q0,
+                &ilimit,
+                &slimit,
+                &tol,
+                &reject_jl))
             return NULL;
 
         if (!_check_array_type(py_Tep))
@@ -111,26 +117,67 @@ extern "C"
         if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
             return NULL;
 
+        // Assign empty q0
+        MapVectorX q0(NULL, 0);
+
+        // Check if q0 is None
+        if (py_q0 != Py_None)
+        {
+            // Make sure q is number array
+            // Cast to numpy array
+            // Get data out
+            if (!_check_array_type(py_q0))
+                return NULL;
+            q0_used = 1;
+            py_np_q0 = (PyObject *)PyArray_FROMANY(py_q0, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
+            np_q0 = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q0);
+            // MapVectorX q0(np_q0, ets->n);
+            new (&q0) MapVectorX(np_q0, ets->n);
+        }
+
+        // std::cout << q0.size() << '\n';
+
+        // Set the dimension of the returned array to match the number of joints
+        dim[0] = ets->n;
+
         py_np_Tep = (PyArrayObject *)PyArray_FROMANY(py_Tep, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT);
         np_Tep = (npy_float64 *)PyArray_DATA(py_np_Tep);
-        MapMatrix4dc Tep(np_Tep);
 
-        py_ret = PyArray_EMPTY(1, dim, NPY_DOUBLE, 0);
-        np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);
-        // MapMatrix4dc Tep(np_Tep);
+        // Tep in row major from Python
+        MapMatrix4dr row_Tep(np_Tep);
 
-        std::cout << Tep << std::endl;
+        // Convert to col major here
+        Matrix4dc Tep = row_Tep;
 
+        py_ret = PyArray_EMPTY(1, dim, NPY_DOUBLE, 0);
+        np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);
+        MapVectorX ret(np_ret, ets->n);
 
+        // std::cout << Tep << std::endl;
+        // std::cout << ret << std::endl;
 
-        // _ETS_IK(ets, n, q, Tep, ret);
+        _IK_LM_Chan(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E);
 
         // _angle_axis(Te, Tep, ret);
 
         // Free the memory
         Py_DECREF(py_np_Tep);
 
-        return py_ret;
+        if (q0_used)
+        {
+            Py_DECREF(py_np_q0);
+        }
+
+        // Build the return tuple
+
+        py_it = Py_BuildValue("i", it);
+        py_search = Py_BuildValue("i", search);
+        py_solution = Py_BuildValue("i", solution);
+        py_E = Py_BuildValue("d", E);
+
+        py_tup = PyTuple_Pack(5, py_ret, py_solution, py_it, py_search, py_E);
+
+        return py_tup;
     }
 
     static PyObject *Robot_link_T(PyObject *self, PyObject *args)
@@ -658,8 +705,10 @@ extern "C"
 
     static PyObject *ETS_init(PyObject *self, PyObject *args)
     {
+        ET *et;
         ETS *ets;
         PyObject *etsl, *ret;
+        int j = 0;
 
         ets = (ETS *)PyMem_RawMalloc(sizeof(ETS));
 
@@ -679,6 +728,25 @@ extern "C"
                 return NULL;
         }
 
+        ets->qlim_l = (double *)PyMem_RawMalloc(ets->n * sizeof(double));
+        ets->qlim_h = (double *)PyMem_RawMalloc(ets->n * sizeof(double));
+        ets->q_range2 = (double *)PyMem_RawMalloc(ets->n * sizeof(double));
+
+        // Cache joint limits
+        for (int i = 0; i < ets->m; i++)
+        {
+            et = ets->ets[i];
+
+            if (et->isjoint)
+            {
+                ets->qlim_l[j] = et->qlim[0];
+                ets->qlim_h[j] = et->qlim[1];
+                ets->q_range2[j] = (et->qlim[1] - et->qlim[0]) / 2.0;
+
+                j += 1;
+            }
+        }
+
         Py_DECREF(iter_et);
 
         ret = PyCapsule_New(ets, "ETS", NULL);
@@ -691,6 +759,7 @@ extern "C"
         int jointtype;
         PyObject *ret, *py_et;
         PyArrayObject *py_T, *py_qlim;
+        npy_float64 *np_qlim;
         int isjoint, isflip, jindex;
 
         et = (ET *)PyMem_RawMalloc(sizeof(ET));
@@ -708,9 +777,12 @@ extern "C"
         if (!(et = (ET *)PyCapsule_GetPointer(py_et, "ET")))
             return NULL;
 
+        np_qlim = (npy_float64 *)PyArray_DATA(py_qlim);
+        et->qlim[0] = np_qlim[0];
+        et->qlim[1] = np_qlim[1];
+
         et->T = (npy_float64 *)PyArray_DATA(py_T);
         new (&et->Tm) MapMatrix4dc(et->T);
-        et->qlim = (npy_float64 *)PyArray_DATA(py_qlim);
         et->axis = jointtype;
 
         et->isjoint = isjoint;
@@ -752,6 +824,7 @@ extern "C"
         int jointtype;
         PyObject *ret;
         PyArrayObject *py_T, *py_qlim;
+        npy_float64 *np_qlim;
 
         et = (ET *)PyMem_RawMalloc(sizeof(ET));
 
@@ -764,9 +837,13 @@ extern "C"
                               &PyArray_Type, &py_qlim))
             return NULL;
 
+        np_qlim = (npy_float64 *)PyArray_DATA(py_qlim);
+        et->qlim = (double *)PyMem_RawMalloc(2 * sizeof(double));
+        et->qlim[0] = np_qlim[0];
+        et->qlim[1] = np_qlim[1];
+
         et->T = (npy_float64 *)PyArray_DATA(py_T);
         new (&et->Tm) MapMatrix4dc(et->T);
-        et->qlim = (npy_float64 *)PyArray_DATA(py_qlim);
 
         et->axis = jointtype;
 

roboticstoolbox/core/linalg.cpp

@@ -100,72 +100,6 @@ extern "C"
             q[2] = -q[2];
     }
 
-    void _angle_axis(double *Te, double *Tep, double *e)
-    {
-        int i, j, k;
-        double num, li_norm, R_tr, ang;
-        double *R = (double *)PyMem_RawCalloc(9, sizeof(double));
-        double li[3];
-
-        // e[:3] = Tep[:3, 3] - Te[:3, 3]
-        e[0] = Tep[0 * 4 + 3] - Te[0 * 4 + 3];
-        e[1] = Tep[1 * 4 + 3] - Te[1 * 4 + 3];
-        e[2] = Tep[2 * 4 + 3] - Te[2 * 4 + 3];
-
-        // R = Tep.R @ T.R.T
-        // R = Tep[:3, :3] @ T[:3, :3].T
-        for (i = 0; i < 3; i++)
-        {
-            for (j = 0; j < 3; j++)
-            {
-                num = 0;
-                for (k = 0; k < 3; k++)
-                {
-                    num += Tep[i * 4 + k] * Te[j * 4 + k];
-                }
-                R[i * 3 + j] = num;
-            }
-        }
-
-        // li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]);
-        li[0] = R[2 * 3 + 1] - R[1 * 3 + 2];
-        li[1] = R[0 * 3 + 2] - R[2 * 3 + 0];
-        li[2] = R[1 * 3 + 0] - R[0 * 3 + 1];
-
-        // if base.iszerovec(li)
-        li_norm = _norm(li, 3);
-        R_tr = _trace(R, 3);
-        if (li_norm < 1e-6)
-        {
-            // diagonal matrix case
-            // if np.trace(R) > 0
-            if (R_tr > 0)
-            {
-                // (1,1,1) case
-                // a = np.zeros((3, ));
-                e[3] = 0;
-                e[4] = 0;
-                e[5] = 0;
-            }
-            else
-            {
-                // a = np.pi / 2 * (np.diag(R) + 1);
-                e[3] = PI_2 * (R[0] + 1);
-                e[4] = PI_2 * (R[4] + 1);
-                e[5] = PI_2 * (R[8] + 1);
-            }
-        }
-        else
-        {
-            // non-diagonal matrix case
-            // a = math.atan2(li_norm, np.trace(R) - 1) * li / li_norm
-            ang = atan2(li_norm, R_tr - 1);
-            e[3] = ang * li[0] / li_norm;
-            e[4] = ang * li[1] / li_norm;
-            e[5] = ang * li[2] / li_norm;
-        }
-    }
-
     // --------------------------------------------------------------------- //
     // 4x4 matrix linalg
     // --------------------------------------------------------------------- //

roboticstoolbox/core/linalg.h

@@ -17,11 +17,15 @@ extern "C"
 
 #define PI_2 1.57079632679489661923132169163975144
 
+#define Matrix3dc Eigen::Matrix3d
+
 #define Matrix4dc Eigen::Matrix4d
 #define Matrix4dr Eigen::Matrix<double, 4, 4, Eigen::RowMajor>
 
 #define MapMatrix4dc Eigen::Map<Matrix4dc>
-#define MapMatrix4dr Eigen::Map<Matrix4dc>
+#define MapMatrix4dr Eigen::Map<Matrix4dr>
+
+#define Matrix6dc Eigen::Matrix<double, 6, 6, Eigen::ColMajor>
 
 #define MatrixJc Eigen::Matrix<double, 6, Eigen::Dynamic, Eigen::ColMajor>
 #define MatrixJr Eigen::Matrix<double, 6, Eigen::Dynamic, Eigen::RowMajor>
@@ -36,9 +40,11 @@ extern "C"
 #define Vector3 Eigen::Vector3d
 #define MapVector3 Eigen::Map<Vector3>
 
+#define VectorX Eigen::VectorXd
+#define MapVectorX Eigen::Map<VectorX>
+
     void _inv(double *m, double *invOut);
     void _r2q(double *r, double *q);
-    void _angle_axis(double *Te, double *Tep, double *e);
     void _eye4(double *data);
     void eye4(Matrix4dc &data);
     void _copy(double *A, double *B);