Skip to content

Commit c5cc2e7

Browse files
jhavljhavl
committed
First go at IK LM
1 parent 20f023e commit c5cc2e7

File tree

5 files changed

+272
-159
lines changed

5 files changed

+272
-159
lines changed

‎roboticstoolbox/core/fknm.cpp

Lines changed: 95 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -89,19 +89,25 @@ extern "C"
8989
static PyObject *IK(PyObject *self, PyObject *args)
9090
{
9191
ETS *ets;
92-
npy_float64 *np_Tep, *np_ret;
93-
PyObject *py_q, *py_Tep;
94-
PyArrayObject *py_np_q, *py_np_Tep;
95-
PyObject *py_ets;
96-
int n;
97-
PyObject *py_ret;
98-
npy_intp dim[1] = {7};
92+
npy_float64 *np_Tep, *np_ret, *np_q0;
93+
PyArrayObject *py_np_Tep;
94+
PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0;
95+
PyObject *py_tup, *py_it, *py_search, *py_solution, *py_E;
96+
npy_intp dim[1] = {1};
97+
int ilimit, slimit, q0_used = 0, reject_jl;
98+
double tol, E;
9999

100+
int it = 0, search = 0, solution = 0;
100101

101102
if (!PyArg_ParseTuple(
102-
args, "OO",
103+
args, "OOOiidi",
103104
&py_ets,
104-
&py_Tep))
105+
&py_Tep,
106+
&py_q0,
107+
&ilimit,
108+
&slimit,
109+
&tol,
110+
&reject_jl))
105111
return NULL;
106112

107113
if (!_check_array_type(py_Tep))
@@ -111,26 +117,67 @@ extern "C"
111117
if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
112118
return NULL;
113119

120+
// Assign empty q0
121+
MapVectorX q0(NULL, 0);
122+
123+
// Check if q0 is None
124+
if (py_q0 != Py_None)
125+
{
126+
// Make sure q is number array
127+
// Cast to numpy array
128+
// Get data out
129+
if (!_check_array_type(py_q0))
130+
return NULL;
131+
q0_used = 1;
132+
py_np_q0 = (PyObject *)PyArray_FROMANY(py_q0, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
133+
np_q0 = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q0);
134+
// MapVectorX q0(np_q0, ets->n);
135+
new (&q0) MapVectorX(np_q0, ets->n);
136+
}
137+
138+
// std::cout << q0.size() << '\n';
139+
140+
// Set the dimension of the returned array to match the number of joints
141+
dim[0] = ets->n;
142+
114143
py_np_Tep = (PyArrayObject *)PyArray_FROMANY(py_Tep, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT);
115144
np_Tep = (npy_float64 *)PyArray_DATA(py_np_Tep);
116-
MapMatrix4dc Tep(np_Tep);
117145

118-
py_ret = PyArray_EMPTY(1, dim, NPY_DOUBLE, 0);
119-
np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);
120-
// MapMatrix4dc Tep(np_Tep);
146+
// Tep in row major from Python
147+
MapMatrix4dr row_Tep(np_Tep);
121148

122-
std::cout << Tep << std::endl;
149+
// Convert to col major here
150+
Matrix4dc Tep = row_Tep;
123151

152+
py_ret = PyArray_EMPTY(1, dim, NPY_DOUBLE, 0);
153+
np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);
154+
MapVectorX ret(np_ret, ets->n);
124155

156+
// std::cout << Tep << std::endl;
157+
// std::cout << ret << std::endl;
125158

126-
// _ETS_IK(ets, n, q, Tep, ret);
159+
_IK_LM_Chan(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E);
127160

128161
// _angle_axis(Te, Tep, ret);
129162

130163
// Free the memory
131164
Py_DECREF(py_np_Tep);
132165

133-
return py_ret;
166+
if (q0_used)
167+
{
168+
Py_DECREF(py_np_q0);
169+
}
170+
171+
// Build the return tuple
172+
173+
py_it = Py_BuildValue("i", it);
174+
py_search = Py_BuildValue("i", search);
175+
py_solution = Py_BuildValue("i", solution);
176+
py_E = Py_BuildValue("d", E);
177+
178+
py_tup = PyTuple_Pack(5, py_ret, py_solution, py_it, py_search, py_E);
179+
180+
return py_tup;
134181
}
135182

136183
static PyObject *Robot_link_T(PyObject *self, PyObject *args)
@@ -658,8 +705,10 @@ extern "C"
658705

659706
static PyObject *ETS_init(PyObject *self, PyObject *args)
660707
{
708+
ET *et;
661709
ETS *ets;
662710
PyObject *etsl, *ret;
711+
int j = 0;
663712

664713
ets = (ETS *)PyMem_RawMalloc(sizeof(ETS));
665714

@@ -679,6 +728,25 @@ extern "C"
679728
return NULL;
680729
}
681730

731+
ets->qlim_l = (double *)PyMem_RawMalloc(ets->n * sizeof(double));
732+
ets->qlim_h = (double *)PyMem_RawMalloc(ets->n * sizeof(double));
733+
ets->q_range2 = (double *)PyMem_RawMalloc(ets->n * sizeof(double));
734+
735+
// Cache joint limits
736+
for (int i = 0; i < ets->m; i++)
737+
{
738+
et = ets->ets[i];
739+
740+
if (et->isjoint)
741+
{
742+
ets->qlim_l[j] = et->qlim[0];
743+
ets->qlim_h[j] = et->qlim[1];
744+
ets->q_range2[j] = (et->qlim[1] - et->qlim[0]) / 2.0;
745+
746+
j += 1;
747+
}
748+
}
749+
682750
Py_DECREF(iter_et);
683751

684752
ret = PyCapsule_New(ets, "ETS", NULL);
@@ -691,6 +759,7 @@ extern "C"
691759
int jointtype;
692760
PyObject *ret, *py_et;
693761
PyArrayObject *py_T, *py_qlim;
762+
npy_float64 *np_qlim;
694763
int isjoint, isflip, jindex;
695764

696765
et = (ET *)PyMem_RawMalloc(sizeof(ET));
@@ -708,9 +777,12 @@ extern "C"
708777
if (!(et = (ET *)PyCapsule_GetPointer(py_et, "ET")))
709778
return NULL;
710779

780+
np_qlim = (npy_float64 *)PyArray_DATA(py_qlim);
781+
et->qlim[0] = np_qlim[0];
782+
et->qlim[1] = np_qlim[1];
783+
711784
et->T = (npy_float64 *)PyArray_DATA(py_T);
712785
new (&et->Tm) MapMatrix4dc(et->T);
713-
et->qlim = (npy_float64 *)PyArray_DATA(py_qlim);
714786
et->axis = jointtype;
715787

716788
et->isjoint = isjoint;
@@ -752,6 +824,7 @@ extern "C"
752824
int jointtype;
753825
PyObject *ret;
754826
PyArrayObject *py_T, *py_qlim;
827+
npy_float64 *np_qlim;
755828

756829
et = (ET *)PyMem_RawMalloc(sizeof(ET));
757830

@@ -764,9 +837,13 @@ extern "C"
764837
&PyArray_Type, &py_qlim))
765838
return NULL;
766839

840+
np_qlim = (npy_float64 *)PyArray_DATA(py_qlim);
841+
et->qlim = (double *)PyMem_RawMalloc(2 * sizeof(double));
842+
et->qlim[0] = np_qlim[0];
843+
et->qlim[1] = np_qlim[1];
844+
767845
et->T = (npy_float64 *)PyArray_DATA(py_T);
768846
new (&et->Tm) MapMatrix4dc(et->T);
769-
et->qlim = (npy_float64 *)PyArray_DATA(py_qlim);
770847

771848
et->axis = jointtype;
772849

‎roboticstoolbox/core/linalg.cpp

Lines changed: 0 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -100,72 +100,6 @@ extern "C"
100100
q[2] = -q[2];
101101
}
102102

103-
void _angle_axis(double *Te, double *Tep, double *e)
104-
{
105-
int i, j, k;
106-
double num, li_norm, R_tr, ang;
107-
double *R = (double *)PyMem_RawCalloc(9, sizeof(double));
108-
double li[3];
109-
110-
// e[:3] = Tep[:3, 3] - Te[:3, 3]
111-
e[0] = Tep[0 * 4 + 3] - Te[0 * 4 + 3];
112-
e[1] = Tep[1 * 4 + 3] - Te[1 * 4 + 3];
113-
e[2] = Tep[2 * 4 + 3] - Te[2 * 4 + 3];
114-
115-
// R = Tep.R @ T.R.T
116-
// R = Tep[:3, :3] @ T[:3, :3].T
117-
for (i = 0; i < 3; i++)
118-
{
119-
for (j = 0; j < 3; j++)
120-
{
121-
num = 0;
122-
for (k = 0; k < 3; k++)
123-
{
124-
num += Tep[i * 4 + k] * Te[j * 4 + k];
125-
}
126-
R[i * 3 + j] = num;
127-
}
128-
}
129-
130-
// li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]);
131-
li[0] = R[2 * 3 + 1] - R[1 * 3 + 2];
132-
li[1] = R[0 * 3 + 2] - R[2 * 3 + 0];
133-
li[2] = R[1 * 3 + 0] - R[0 * 3 + 1];
134-
135-
// if base.iszerovec(li)
136-
li_norm = _norm(li, 3);
137-
R_tr = _trace(R, 3);
138-
if (li_norm < 1e-6)
139-
{
140-
// diagonal matrix case
141-
// if np.trace(R) > 0
142-
if (R_tr > 0)
143-
{
144-
// (1,1,1) case
145-
// a = np.zeros((3, ));
146-
e[3] = 0;
147-
e[4] = 0;
148-
e[5] = 0;
149-
}
150-
else
151-
{
152-
// a = np.pi / 2 * (np.diag(R) + 1);
153-
e[3] = PI_2 * (R[0] + 1);
154-
e[4] = PI_2 * (R[4] + 1);
155-
e[5] = PI_2 * (R[8] + 1);
156-
}
157-
}
158-
else
159-
{
160-
// non-diagonal matrix case
161-
// a = math.atan2(li_norm, np.trace(R) - 1) * li / li_norm
162-
ang = atan2(li_norm, R_tr - 1);
163-
e[3] = ang * li[0] / li_norm;
164-
e[4] = ang * li[1] / li_norm;
165-
e[5] = ang * li[2] / li_norm;
166-
}
167-
}
168-
169103
// --------------------------------------------------------------------- //
170104
// 4x4 matrix linalg
171105
// --------------------------------------------------------------------- //

‎roboticstoolbox/core/linalg.h

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,11 +17,15 @@ extern "C"
1717

1818
#define PI_2 1.57079632679489661923132169163975144
1919

20+
#define Matrix3dc Eigen::Matrix3d
21+
2022
#define Matrix4dc Eigen::Matrix4d
2123
#define Matrix4dr Eigen::Matrix<double, 4, 4, Eigen::RowMajor>
2224

2325
#define MapMatrix4dc Eigen::Map<Matrix4dc>
24-
#define MapMatrix4dr Eigen::Map<Matrix4dc>
26+
#define MapMatrix4dr Eigen::Map<Matrix4dr>
27+
28+
#define Matrix6dc Eigen::Matrix<double, 6, 6, Eigen::ColMajor>
2529

2630
#define MatrixJc Eigen::Matrix<double, 6, Eigen::Dynamic, Eigen::ColMajor>
2731
#define MatrixJr Eigen::Matrix<double, 6, Eigen::Dynamic, Eigen::RowMajor>
@@ -36,9 +40,11 @@ extern "C"
3640
#define Vector3 Eigen::Vector3d
3741
#define MapVector3 Eigen::Map<Vector3>
3842

43+
#define VectorX Eigen::VectorXd
44+
#define MapVectorX Eigen::Map<VectorX>
45+
3946
void _inv(double *m, double *invOut);
4047
void _r2q(double *r, double *q);
41-
void _angle_axis(double *Te, double *Tep, double *e);
4248
void _eye4(double *data);
4349
void eye4(Matrix4dc &data);
4450
void _copy(double *A, double *B);

0 commit comments

Comments
 (0)