Skip to content

Commit 19bef91

Browse files
jhavljhavl
committed
c extension revision
1 parent 3a95b35 commit 19bef91

File tree

13 files changed

+301
-398369
lines changed

13 files changed

+301
-398369
lines changed

‎roboticstoolbox/core/fknm.c

Lines changed: 157 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -16,13 +16,15 @@
1616
// forward defines
1717
static PyObject *fkine_all(PyObject *self, PyObject *args);
1818
static PyObject *jacob0(PyObject *self, PyObject *args);
19+
static PyObject *jacobe(PyObject *self, PyObject *args);
1920
static PyObject *fkine(PyObject *self, PyObject *args);
2021
static PyObject *link_init(PyObject *self, PyObject *args);
2122
static PyObject *link_A(PyObject *self, PyObject *args);
2223
static PyObject *link_update(PyObject *self, PyObject *args);
2324
static PyObject *compose(PyObject *self, PyObject *args);
2425

2526
void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J);
27+
void _jacobe(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J);
2628
void _fkine(PyObject *links, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *ret);
2729
void A(Link *link, npy_float64 *ret, double eta);
2830
void mult(npy_float64 *A, npy_float64 *B, npy_float64 *C);
@@ -61,6 +63,10 @@ static PyMethodDef fknmMethods[] = {
6163
(PyCFunction)jacob0,
6264
METH_VARARGS,
6365
"Link"},
66+
{"jacobe",
67+
(PyCFunction)jacobe,
68+
METH_VARARGS,
69+
"Link"},
6470
{"fkine_all",
6571
(PyCFunction)fkine_all,
6672
METH_VARARGS,
@@ -85,8 +91,7 @@ PyMODINIT_FUNC PyInit_fknm(void)
8591
static PyObject *fkine_all(PyObject *self, PyObject *args)
8692
{
8793
Link *link;
88-
npy_float64 *q, *base, *fk, *pfk, *ret;
89-
npy_float64 *s_base;
94+
npy_float64 *q, *base, *ret;
9095
PyArrayObject *py_q, *py_base;
9196
PyObject *links, *iter_links;
9297
int m;
@@ -115,34 +120,56 @@ static PyObject *fkine_all(PyObject *self, PyObject *args)
115120
// Calculate the current link transform
116121
A(link, ret, q[link->jindex]);
117122

118-
// Get pointer to link._fk array
119-
fk = (npy_float64 *)PyArray_DATA(link->fk);
120-
121123
if (link->parent)
122124
{
123-
// Get pointer to link.parent._fk array
124-
pfk = (npy_float64 *)PyArray_DATA(link->parent->fk);
125-
126125
// Multiply parent._fk by link.A and store in link._fk
127-
mult(pfk, ret, fk);
126+
mult(link->parent->fk, ret, link->fk);
128127
}
129128
else
130129
{
131130
// Multiply base by link.A and store in link._fk
132-
mult(base, ret, fk);
131+
mult(base, ret, link->fk);
133132
}
134133

135134
// Set dependant shapes
136135
for (int i = 0; i < link->n_shapes; i++)
137136
{
138-
copy(fk, link->shape_wT[i]);
139-
mult(fk, link->shape_base[i], link->shape_sT[i]);
137+
copy(link->fk, link->shape_wT[i]);
138+
mult(link->fk, link->shape_base[i], link->shape_sT[i]);
140139
}
141140
}
142141

143142
Py_RETURN_NONE;
144143
}
145144

145+
static PyObject *jacobe(PyObject *self, PyObject *args)
146+
{
147+
npy_float64 *J, *q, *etool, *tool;
148+
PyArrayObject *py_J, *py_q, *py_tool, *py_etool;
149+
PyObject *links;
150+
int m, n;
151+
152+
if (!PyArg_ParseTuple(
153+
args, "iiOO!O!O!O!",
154+
&m,
155+
&n,
156+
&links,
157+
&PyArray_Type, &py_q,
158+
&PyArray_Type, &py_etool,
159+
&PyArray_Type, &py_tool,
160+
&PyArray_Type, &py_J))
161+
return NULL;
162+
163+
q = (npy_float64 *)PyArray_DATA(py_q);
164+
J = (npy_float64 *)PyArray_DATA(py_J);
165+
tool = (npy_float64 *)PyArray_DATA(py_tool);
166+
etool = (npy_float64 *)PyArray_DATA(py_etool);
167+
168+
_jacobe(links, m, n, q, etool, tool, J);
169+
170+
Py_RETURN_NONE;
171+
}
172+
146173
static PyObject *jacob0(PyObject *self, PyObject *args)
147174
{
148175
npy_float64 *J, *q, *etool, *tool;
@@ -207,7 +234,7 @@ static PyObject *link_init(PyObject *self, PyObject *args)
207234
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT;
208235
PyObject *iter_base, *iter_wT, *iter_sT;
209236
PyArrayObject *pys_base, *pys_wT, *pys_sT;
210-
npy_float64 *s_base, *s_wT, *s_sT;
237+
PyArrayObject *py_A, *py_fk;
211238

212239
link = (Link *)PyMem_RawMalloc(sizeof(Link));
213240

@@ -217,8 +244,8 @@ static PyObject *link_init(PyObject *self, PyObject *args)
217244
&jointtype,
218245
&link->jindex,
219246
&link->n_shapes,
220-
&PyArray_Type, &link->A,
221-
&PyArray_Type, &link->fk,
247+
&PyArray_Type, &py_A,
248+
&PyArray_Type, &py_fk,
222249
&py_shape_base,
223250
&py_shape_wT,
224251
&py_shape_sT,
@@ -234,14 +261,17 @@ static PyObject *link_init(PyObject *self, PyObject *args)
234261
return NULL;
235262
}
236263

264+
link->A = (npy_float64 *)PyArray_DATA(py_A);
265+
link->fk = (npy_float64 *)PyArray_DATA(py_fk);
266+
237267
// Set shape pointers
238268
iter_base = PyObject_GetIter(py_shape_base);
239269
iter_wT = PyObject_GetIter(py_shape_wT);
240270
iter_sT = PyObject_GetIter(py_shape_sT);
241271

242-
link->shape_base = (npy_float64 *)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
243-
link->shape_wT = (npy_float64 *)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
244-
link->shape_sT = (npy_float64 *)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
272+
link->shape_base = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
273+
link->shape_wT = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
274+
link->shape_sT = (npy_float64 **)PyMem_RawCalloc(link->n_shapes, sizeof(npy_float64));
245275

246276
for (int i = 0; i < link->n_shapes; i++)
247277
{
@@ -294,12 +324,11 @@ static PyObject *link_update(PyObject *self, PyObject *args)
294324
int isjoint, isflip;
295325
int jointtype, jindex, n_shapes;
296326
PyObject *lo, *py_parent;
297-
PyArrayObject *A, *fk;
327+
PyArrayObject *py_A, *py_fk;
298328

299329
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT;
300330
PyObject *iter_base, *iter_wT, *iter_sT;
301331
PyArrayObject *pys_base, *pys_wT, *pys_sT;
302-
npy_float64 *s_base, *s_wT, *s_sT;
303332

304333
if (!PyArg_ParseTuple(args, "OiiiiiO!O!OOOO",
305334
&lo,
@@ -308,8 +337,8 @@ static PyObject *link_update(PyObject *self, PyObject *args)
308337
&jointtype,
309338
&jindex,
310339
&n_shapes,
311-
&PyArray_Type, &A,
312-
&PyArray_Type, &fk,
340+
&PyArray_Type, &py_A,
341+
&PyArray_Type, &py_fk,
313342
&py_shape_base,
314343
&py_shape_wT,
315344
&py_shape_sT,
@@ -335,9 +364,9 @@ static PyObject *link_update(PyObject *self, PyObject *args)
335364
iter_wT = PyObject_GetIter(py_shape_wT);
336365
iter_sT = PyObject_GetIter(py_shape_sT);
337366

338-
link->shape_base = (npy_float64 *)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
339-
link->shape_wT = (npy_float64 *)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
340-
link->shape_sT = (npy_float64 *)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
367+
link->shape_base = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
368+
link->shape_wT = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
369+
link->shape_sT = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
341370

342371
for (int i = 0; i < n_shapes; i++)
343372
{
@@ -352,11 +381,6 @@ static PyObject *link_update(PyObject *self, PyObject *args)
352381
link->shape_sT[i] = (npy_float64 *)PyArray_DATA(pys_sT);
353382
}
354383

355-
// if (n_shapes > 2)
356-
// {
357-
// copy(link->shape_base[0], link->shape_base[1]);
358-
// }
359-
360384
if (jointtype == 0)
361385
{
362386
link->op = rx;
@@ -384,8 +408,8 @@ static PyObject *link_update(PyObject *self, PyObject *args)
384408

385409
link->isjoint = isjoint;
386410
link->isflip = isflip;
387-
link->A = A;
388-
link->fk = fk;
411+
link->A = (npy_float64 *)PyArray_DATA(py_A);
412+
link->fk = (npy_float64 *)PyArray_DATA(py_fk);
389413
link->jindex = jindex;
390414
link->axis = jointtype;
391415
link->parent = parent;
@@ -435,6 +459,104 @@ static PyObject *compose(PyObject *self, PyObject *args)
435459
Py_RETURN_NONE;
436460
}
437461

462+
void _jacobe(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J)
463+
{
464+
Link *link;
465+
npy_float64 *T = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
466+
npy_float64 *U = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
467+
npy_float64 *temp = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
468+
npy_float64 *ret = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
469+
npy_float64 *invU = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
470+
int j = n - 1;
471+
472+
_eye(U);
473+
_fkine(links, m, q, etool, tool, T);
474+
475+
PyList_Reverse(links);
476+
PyObject *iter_links = PyObject_GetIter(links);
477+
478+
mult(etool, U, temp);
479+
copy(temp, U);
480+
mult(tool, U, temp);
481+
copy(temp, U);
482+
483+
for (int i = 0; i < m; i++)
484+
{
485+
if (!(link = (Link *)PyCapsule_GetPointer(PyIter_Next(iter_links), "Link")))
486+
return;
487+
488+
if (link->isjoint)
489+
{
490+
if (link->axis == 0)
491+
{
492+
J[0 * n + j] = U[2 * 4 + 0] * U[1 * 4 + 3] - U[1 * 4 + 0] * U[2 * 4 + 3];
493+
J[1 * n + j] = U[2 * 4 + 1] * U[1 * 4 + 3] - U[1 * 4 + 1] * U[2 * 4 + 3];
494+
J[2 * n + j] = U[2 * 4 + 2] * U[1 * 4 + 3] - U[1 * 4 + 2] * U[2 * 4 + 3];
495+
J[3 * n + j] = U[0 * 4 + 0];
496+
J[4 * n + j] = U[0 * 4 + 1];
497+
J[5 * n + j] = U[0 * 4 + 2];
498+
}
499+
else if (link->axis == 1)
500+
{
501+
J[0 * n + j] = U[0 * 4 + 0] * U[2 * 4 + 3] - U[2 * 4 + 0] * U[0 * 4 + 3];
502+
J[1 * n + j] = U[0 * 4 + 1] * U[2 * 4 + 3] - U[2 * 4 + 1] * U[0 * 4 + 3];
503+
J[2 * n + j] = U[0 * 4 + 2] * U[2 * 4 + 3] - U[2 * 4 + 2] * U[0 * 4 + 3];
504+
J[3 * n + j] = U[1 * 4 + 0];
505+
J[4 * n + j] = U[1 * 4 + 1];
506+
J[5 * n + j] = U[1 * 4 + 2];
507+
}
508+
else if (link->axis == 2)
509+
{
510+
J[0 * n + j] = U[1 * 4 + 0] * U[0 * 4 + 3] - U[0 * 4 + 0] * U[1 * 4 + 3];
511+
J[1 * n + j] = U[1 * 4 + 1] * U[0 * 4 + 3] - U[0 * 4 + 1] * U[1 * 4 + 3];
512+
J[2 * n + j] = U[1 * 4 + 2] * U[0 * 4 + 3] - U[0 * 4 + 2] * U[1 * 4 + 3];
513+
J[3 * n + j] = U[2 * 4 + 0];
514+
J[4 * n + j] = U[2 * 4 + 1];
515+
J[5 * n + j] = U[2 * 4 + 2];
516+
}
517+
else if (link->axis == 3)
518+
{
519+
J[0 * n + j] = U[0 * 4 + 0];
520+
J[1 * n + j] = U[0 * 4 + 1];
521+
J[2 * n + j] = U[0 * 4 + 2];
522+
J[3 * n + j] = 0.0;
523+
J[4 * n + j] = 0.0;
524+
J[5 * n + j] = 0.0;
525+
}
526+
else if (link->axis == 4)
527+
{
528+
J[0 * n + j] = U[1 * 4 + 0];
529+
J[1 * n + j] = U[1 * 4 + 1];
530+
J[2 * n + j] = U[1 * 4 + 2];
531+
J[3 * n + j] = 0.0;
532+
J[4 * n + j] = 0.0;
533+
J[5 * n + j] = 0.0;
534+
}
535+
else if (link->axis == 5)
536+
{
537+
J[0 * n + j] = U[2 * 4 + 0];
538+
J[1 * n + j] = U[2 * 4 + 1];
539+
J[2 * n + j] = U[2 * 4 + 2];
540+
J[3 * n + j] = 0.0;
541+
J[4 * n + j] = 0.0;
542+
J[5 * n + j] = 0.0;
543+
}
544+
545+
A(link, ret, q[link->jindex]);
546+
mult(ret, U, temp);
547+
copy(temp, U);
548+
j--;
549+
}
550+
else
551+
{
552+
A(link, ret, q[link->jindex]);
553+
mult(ret, U, temp);
554+
copy(temp, U);
555+
}
556+
}
557+
PyList_Reverse(links);
558+
}
559+
438560
void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J)
439561
{
440562
Link *link;
@@ -571,12 +693,11 @@ void _fkine(PyObject *links, int n, npy_float64 *q, npy_float64 *etool, npy_floa
571693

572694
void A(Link *link, npy_float64 *ret, double eta)
573695
{
574-
npy_float64 *A, *v;
575-
A = (npy_float64 *)PyArray_DATA(link->A);
696+
npy_float64 *v;
576697

577698
if (!link->isjoint)
578699
{
579-
copy(A, ret);
700+
copy(link->A, ret);
580701
return;
581702
}
582703

@@ -590,7 +711,7 @@ void A(Link *link, npy_float64 *ret, double eta)
590711
link->op(v, eta);
591712

592713
// Multiply ret = A * v
593-
mult(A, v, ret);
714+
mult(link->A, v, ret);
594715
}
595716

596717
void copy(npy_float64 *A, npy_float64 *B)

‎roboticstoolbox/core/fknm.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@ struct Link
2828
int jindex;
2929
int axis;
3030
int n_shapes;
31-
PyArrayObject *A; /* link static transform */
32-
PyArrayObject *fk; /* link world transform */
31+
npy_float64 *A; /* link static transform */
32+
npy_float64 *fk; /* link world transform */
3333
void (*op)(npy_float64 *data, double eta);
3434
Link *parent;
3535
npy_float64 **shape_base; /* link visual and collision geometries */

‎roboticstoolbox/examples/swift_benchmark.py

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import spatialmath as sm
99
import numpy as np
1010
import cProfile
11+
# import numpy.testing as nt
1112

1213
env = swift.Swift(_dev=True)
1314
env.launch()
@@ -19,7 +20,7 @@
1920

2021

2122
ev = [0.01, 0, 0, 0, 0, 0]
22-
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
23+
panda.qd = np.linalg.pinv(panda.jacobe(panda.q, fast=True)) @ ev
2324
env.step(0.001)
2425

2526

@@ -31,3 +32,19 @@ def stepper():
3132

3233

3334
cProfile.run('stepper()')
35+
36+
# r = rtb.models.Frankie()
37+
# r.q = r.qr
38+
39+
# for i in range(1000):
40+
# q = np.round(np.random.random(9), 2)
41+
# j1 = r.jacobe(q)
42+
# # j2 = r.jacobe_new(q)
43+
# j2 = r.jacobe(q, fast=True)
44+
45+
# print(np.round(j1, 2))
46+
# print(np.round(j2, 2))
47+
# print(q)
48+
# print()
49+
50+
# nt.assert_almost_equal(j1, j2)

0 commit comments

Comments
 (0)