Skip to content

Commit bc3fe42

Browse files
jhavljhavl
committed
Added quaternions
1 parent 71183f3 commit bc3fe42

File tree

2 files changed

+91
-11
lines changed

2 files changed

+91
-11
lines changed

‎roboticstoolbox/core/fknm.c‎

Lines changed: 80 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ static PyObject *link_init(PyObject *self, PyObject *args);
2222
static PyObject *link_A(PyObject *self, PyObject *args);
2323
static PyObject *link_update(PyObject *self, PyObject *args);
2424
static PyObject *compose(PyObject *self, PyObject *args);
25+
static PyObject *r2q(PyObject *self, PyObject *args);
2526

2627
void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J);
2728
void _jacobe(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J);
@@ -37,6 +38,7 @@ void ty(npy_float64 *data, double eta);
3738
void tz(npy_float64 *data, double eta);
3839
void _eye(npy_float64 *data);
3940
int _inv(npy_float64 *m, npy_float64 *invOut);
41+
void _r2q(npy_float64 *r, npy_float64 *q);
4042

4143
static PyMethodDef fknmMethods[] = {
4244
{"link_init",
@@ -59,6 +61,10 @@ static PyMethodDef fknmMethods[] = {
5961
(PyCFunction)compose,
6062
METH_VARARGS,
6163
"Link"},
64+
{"r2q",
65+
(PyCFunction)r2q,
66+
METH_VARARGS,
67+
"Link"},
6268
{"jacob0",
6369
(PyCFunction)jacob0,
6470
METH_VARARGS,
@@ -136,6 +142,7 @@ static PyObject *fkine_all(PyObject *self, PyObject *args)
136142
{
137143
copy(link->fk, link->shape_wT[i]);
138144
mult(link->fk, link->shape_base[i], link->shape_sT[i]);
145+
_r2q(link->shape_sT[i], link->shape_sq[i]);
139146
}
140147
}
141148

@@ -231,14 +238,14 @@ static PyObject *link_init(PyObject *self, PyObject *args)
231238
int jointtype;
232239
PyObject *ret, *py_parent;
233240

234-
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT;
235-
PyObject *iter_base, *iter_wT, *iter_sT;
236-
PyArrayObject *pys_base, *pys_wT, *pys_sT;
241+
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT, *py_shape_sq;
242+
PyObject *iter_base, *iter_wT, *iter_sT, *iter_sq;
243+
PyArrayObject *pys_base, *pys_wT, *pys_sT, *pys_sq;
237244
PyArrayObject *py_A, *py_fk;
238245

239246
link = (Link *)PyMem_RawMalloc(sizeof(Link));
240247

241-
if (!PyArg_ParseTuple(args, "iiiiiO!O!OOOO",
248+
if (!PyArg_ParseTuple(args, "iiiiiO!O!OOOOO",
242249
&link->isjoint,
243250
&link->isflip,
244251
&jointtype,
@@ -249,6 +256,7 @@ static PyObject *link_init(PyObject *self, PyObject *args)
249256
&py_shape_base,
250257
&py_shape_wT,
251258
&py_shape_sT,
259+
&py_shape_sq,
252260
&py_parent))
253261
return NULL;
254262

@@ -268,22 +276,26 @@ static PyObject *link_init(PyObject *self, PyObject *args)
268276
iter_base = PyObject_GetIter(py_shape_base);
269277
iter_wT = PyObject_GetIter(py_shape_wT);
270278
iter_sT = PyObject_GetIter(py_shape_sT);
279+
iter_sq = PyObject_GetIter(py_shape_sq);
271280

272281
link->shape_base = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
273282
link->shape_wT = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
274283
link->shape_sT = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
284+
link->shape_sq = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
275285

276286
for (int i = 0; i < link->n_shapes; i++)
277287
{
278288
if (
279289
!(pys_base = (PyArrayObject *)PyIter_Next(iter_base)) ||
280290
!(pys_wT = (PyArrayObject *)PyIter_Next(iter_wT)) ||
281-
!(pys_sT = (PyArrayObject *)PyIter_Next(iter_sT)))
291+
!(pys_sT = (PyArrayObject *)PyIter_Next(iter_sT)) ||
292+
!(pys_sq = (PyArrayObject *)PyIter_Next(iter_sq)))
282293
return NULL;
283294

284295
link->shape_base[i] = (npy_float64 *)PyArray_DATA(pys_base);
285296
link->shape_wT[i] = (npy_float64 *)PyArray_DATA(pys_wT);
286297
link->shape_sT[i] = (npy_float64 *)PyArray_DATA(pys_sT);
298+
link->shape_sq[i] = (npy_float64 *)PyArray_DATA(pys_sq);
287299
}
288300

289301
link->axis = jointtype;
@@ -326,11 +338,11 @@ static PyObject *link_update(PyObject *self, PyObject *args)
326338
PyObject *lo, *py_parent;
327339
PyArrayObject *py_A, *py_fk;
328340

329-
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT;
330-
PyObject *iter_base, *iter_wT, *iter_sT;
331-
PyArrayObject *pys_base, *pys_wT, *pys_sT;
341+
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT, *py_shape_sq;
342+
PyObject *iter_base, *iter_wT, *iter_sT, *iter_sq;
343+
PyArrayObject *pys_base, *pys_wT, *pys_sT, *pys_sq;
332344

333-
if (!PyArg_ParseTuple(args, "OiiiiiO!O!OOOO",
345+
if (!PyArg_ParseTuple(args, "OiiiiiO!O!OOOOO",
334346
&lo,
335347
&isjoint,
336348
&isflip,
@@ -342,6 +354,7 @@ static PyObject *link_update(PyObject *self, PyObject *args)
342354
&py_shape_base,
343355
&py_shape_wT,
344356
&py_shape_sT,
357+
&py_shape_sq,
345358
&py_parent))
346359
return NULL;
347360

@@ -363,22 +376,26 @@ static PyObject *link_update(PyObject *self, PyObject *args)
363376
iter_base = PyObject_GetIter(py_shape_base);
364377
iter_wT = PyObject_GetIter(py_shape_wT);
365378
iter_sT = PyObject_GetIter(py_shape_sT);
379+
iter_sq = PyObject_GetIter(py_shape_sq);
366380

367381
link->shape_base = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
368382
link->shape_wT = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
369383
link->shape_sT = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
384+
link->shape_sq = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
370385

371386
for (int i = 0; i < n_shapes; i++)
372387
{
373388
if (
374389
!(pys_base = (PyArrayObject *)PyIter_Next(iter_base)) ||
375390
!(pys_wT = (PyArrayObject *)PyIter_Next(iter_wT)) ||
376-
!(pys_sT = (PyArrayObject *)PyIter_Next(iter_sT)))
391+
!(pys_sT = (PyArrayObject *)PyIter_Next(iter_sT)) ||
392+
!(pys_sq = (PyArrayObject *)PyIter_Next(iter_sq)))
377393
return NULL;
378394

379395
link->shape_base[i] = (npy_float64 *)PyArray_DATA(pys_base);
380396
link->shape_wT[i] = (npy_float64 *)PyArray_DATA(pys_wT);
381397
link->shape_sT[i] = (npy_float64 *)PyArray_DATA(pys_sT);
398+
link->shape_sq[i] = (npy_float64 *)PyArray_DATA(pys_sq);
382399
}
383400

384401
if (jointtype == 0)
@@ -459,14 +476,33 @@ static PyObject *compose(PyObject *self, PyObject *args)
459476
Py_RETURN_NONE;
460477
}
461478

479+
static PyObject *r2q(PyObject *self, PyObject *args)
480+
{
481+
// r is actually an SE3
482+
npy_float64 *r, *q;
483+
PyArrayObject *py_r, *py_q;
484+
485+
if (!PyArg_ParseTuple(
486+
args, "O!O!",
487+
&PyArray_Type, &py_r,
488+
&PyArray_Type, &py_q))
489+
return NULL;
490+
491+
r = (npy_float64 *)PyArray_DATA(py_r);
492+
q = (npy_float64 *)PyArray_DATA(py_q);
493+
494+
_r2q(r, q);
495+
496+
Py_RETURN_NONE;
497+
}
498+
462499
void _jacobe(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J)
463500
{
464501
Link *link;
465502
npy_float64 *T = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
466503
npy_float64 *U = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
467504
npy_float64 *temp = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
468505
npy_float64 *ret = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
469-
npy_float64 *invU = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
470506
int j = n - 1;
471507

472508
_eye(U);
@@ -1040,3 +1076,36 @@ int _inv(npy_float64 *m, npy_float64 *invOut)
10401076

10411077
return 1;
10421078
}
1079+
1080+
void _r2q(npy_float64 *r, npy_float64 *q)
1081+
{
1082+
double t12p, t13p, t23p;
1083+
double t12m, t13m, t23m;
1084+
double d1, d2, d3, d4;
1085+
1086+
t12p = pow((r[0 * 4 + 1] + r[1 * 4 + 0]), 2);
1087+
t13p = pow((r[0 * 4 + 2] + r[2 * 4 + 0]), 2);
1088+
t23p = pow((r[1 * 4 + 2] + r[2 * 4 + 1]), 2);
1089+
1090+
t12m = pow((r[0 * 4 + 1] - r[1 * 4 + 0]), 2);
1091+
t13m = pow((r[0 * 4 + 2] - r[2 * 4 + 0]), 2);
1092+
t23m = pow((r[1 * 4 + 2] - r[2 * 4 + 1]), 2);
1093+
1094+
d1 = pow(( r[0 * 4 + 0] + r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
1095+
d2 = pow(( r[0 * 4 + 0] - r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
1096+
d3 = pow((-r[0 * 4 + 0] + r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
1097+
d4 = pow((-r[0 * 4 + 0] - r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
1098+
1099+
q[3] = sqrt(d1 + t23m + t13m + t12m) / 4.0;
1100+
q[0] = sqrt(t23m + d2 + t12p + t13p) / 4.0;
1101+
q[1] = sqrt(t13m + t12p + d3 + t23p) / 4.0;
1102+
q[2] = sqrt(t12m + t13p + t23p + d4) / 4.0;
1103+
1104+
// transfer sign from rotation element differences
1105+
if (r[2 * 4 + 1] < r[1 * 4 + 2])
1106+
q[0] = -q[0];
1107+
if (r[0 * 4 + 2] < r[2 * 4 + 0])
1108+
q[1] = -q[1];
1109+
if (r[1 * 4 + 0] < r[0 * 4 + 1])
1110+
q[2] = -q[2];
1111+
}

‎tests/test_fknm.py‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,21 @@
88
import roboticstoolbox as rtb
99
import unittest
1010
import spatialmath as sm
11+
import fknm
1112

1213

1314
class Testfknm(unittest.TestCase):
1415

16+
def test_r2q(self):
17+
18+
q = np.empty(4)
19+
20+
for _ in range(100):
21+
r = sm.SE3.Rand()
22+
qr = sm.base.r2q(r.R, order='xyzs')
23+
fknm.r2q(r.A, q)
24+
nt.assert_array_almost_equal(qr, q)
25+
1526
def test_fkine(self):
1627
r = rtb.models.Panda()
1728
q = np.array([1.0, 2, 3, 4, 5, 6, 7])

0 commit comments

Comments
 (0)