8000 new fast fkine_links · oridong/robotics-toolbox-python@a90edc5 · GitHub
[go: up one dir, main page]
Skip to content

Commit a90edc5

Browse files
jhavljhavl
committed
new fast fkine_links
1 parent 66b7c93 commit a90edc5

File tree

6 files changed

+229
-132
lines changed

6 files changed

+229
-132
lines changed

‎roboticstoolbox/core/fknm.c

Lines changed: 91 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -85,9 +85,10 @@ PyMODINIT_FUNC PyInit_fknm(void)
8585
static PyObject *fkine_all(PyObject *self, PyObject *args)
8686
{
8787
Link *link;
88-
npy_float64 *q, *base, *fk, *pfk;
88+
npy_float64 *q, *base, *fk, *pfk, *ret;
89+
npy_float64 *s_base;
8990
PyArrayObject *py_q, *py_base;
90-
PyObject *links;
91+
PyObject *links, *iter_links;
9192
int m;
9293

9394
if (!PyArg_ParseTuple(
@@ -100,28 +101,43 @@ static PyObject *fkine_all(PyObject *self, PyObject *args)
100101

101102
q = (npy_float64 *)PyArray_DATA(py_q);
102103
base = (npy_float64 *)PyArray_DATA(py_base);
103-
PyObject *iter_links = PyObject_GetIter(links);
104-
npy_float64 *ret = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
104+
iter_links = PyObject_GetIter(links);
105+
ret = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
105106

107+
// Loop through each link in links which is m long
106108
for (int i = 0; i < m; i++)
107109
{
108110
if (!(link = (Link *)PyCapsule_GetPointer(PyIter_Next(iter_links), "Link")))
109111
{
110112
return NULL;
111113
}
112114

115+
// Calculate the current link transform
113116
A(link, ret, q[link->jindex]);
117+
118+
// Get pointer to link._fk array
114119
fk = (npy_float64 *)PyArray_DATA(link->fk);
115120

116121
if (link->parent)
117122
{
123+
// Get pointer to link.parent._fk array
118124
pfk = (npy_float64 *)PyArray_DATA(link->parent->fk);
125+
126+
// Multiply parent._fk by link.A and store in link._fk
119127
mult(pfk, ret, fk);
120128
}
121129
else
122130
{
131+
// Multiply base by link.A and store in link._fk
123132
mult(base, ret, fk);
124133
}
134+
135+
// Set dependant shapes
136+
for (int i = 0; i < link->n_shapes; i++)
137+
{
138+
copy(fk, link->shape_wT[i]);
139+
mult(fk, link->shape_base[i], link->shape_sT[i]);
140+
}
125141
}
126142

127143
Py_RETURN_NONE;
@@ -188,15 +204,24 @@ static PyObject *link_init(PyObject *self, PyObject *args)
188204
int jointtype;
189205
PyObject *ret, *py_parent;
190206

207+
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT;
208+
PyObject *iter_base, *iter_wT, *iter_sT;
209+
PyArrayObject *pys_base, *pys_wT, *pys_sT;
210+
npy_float64 *s_base, *s_wT, *s_sT;
211+
191212
link = (Link *)PyMem_RawMalloc(sizeof(Link));
192213

193-
if (!PyArg_ParseTuple(args, "iiiiO!O!O",
214+
if (!PyArg_ParseTuple(args, "iiiiiO!O!OOOO",
194215
&link->isjoint,
195216
&link->isflip,
196217
&jointtype,
197218
&link->jindex,
219+
&link->n_shapes,
198220
&PyArray_Type, &link->A,
199221
&PyArray_Type, &link->fk,
222+
&py_shape_base,
223+
&py_shape_wT,
224+
&py_shape_sT,
200225
&py_parent))
201226
return NULL;
202227

@@ -209,6 +234,28 @@ static PyObject *link_init(PyObject *self, PyObject *args)
209234
return NULL;
210235
}
211236

237+
// Set shape pointers
238+
iter_base = PyObject_GetIter(py_shape_base);
239+
iter_wT = PyObject_GetIter(py_shape_wT);
240+
iter_sT = PyObject_GetIter(py_shape_sT);
241+
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));
245+
246+
for (int i = 0; i < link->n_shapes; i++)
247+
{
248+
if (
249+
!(pys_base = (PyArrayObject *)PyIter_Next(iter_base)) ||
250+
!(pys_wT = (PyArrayObject *)PyIter_Next(iter_wT)) ||
251+
!(pys_sT = (PyArrayObject *)PyIter_Next(iter_sT)))
252+
return NULL;
253+
254+
link->shape_base[i] = (npy_float64 *)PyArray_DATA(pys_base);
255+
link->shape_wT[i] = (npy_float64 *)PyArray_DATA(pys_wT);
256+
link->shape_sT[i] = (npy_float64 *)PyArray_DATA(pys_sT);
257+
}
258+
212259
link->axis = jointtype;
213260
link->parent = parent;
214261

@@ -245,19 +292,27 @@ static PyObject *link_update(PyObject *self, PyObject *args)
245292
{
246293
Link *link, *parent;
247294
int isjoint, isflip;
248-
int jointtype;
249-
int jindex;
295+
int jointtype, jindex, n_shapes;
250296
PyObject *lo, *py_parent;
251297
PyArrayObject *A, *fk;
252298

253-
if (!PyArg_ParseTuple(args, "OiiiiO!O!O",
299+
PyObject *py_shape_base, *py_shape_wT, *py_shape_sT;
300+
PyObject *iter_base, *iter_wT, *iter_sT;
301+
PyArrayObject *pys_base, *pys_wT, *pys_sT;
302+
npy_float64 *s_base, *s_wT, *s_sT;
303+
304+
if (!PyArg_ParseTuple(args, "OiiiiiO!O!OOOO",
254305
&lo,
255306
&isjoint,
256307
&isflip,
257308
&jointtype,
258309
&jindex,
310+
&n_shapes,
259311
&PyArray_Type, &A,
260312
&PyArray_Type, &fk,
313+
&py_shape_base,
314+
&py_shape_wT,
315+
&py_shape_sT,
261316
&py_parent))
262317
return NULL;
263318

@@ -275,6 +330,33 @@ static PyObject *link_update(PyObject *self, PyObject *args)
275330
return NULL;
276331
}
277332

333+
// Set shape pointers
334+
iter_base = PyObject_GetIter(py_shape_base);
335+
iter_wT = PyObject_GetIter(py_shape_wT);
336+
iter_sT = PyObject_GetIter(py_shape_sT);
337+
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));
341+
342+
for (int i = 0; i < n_shapes; i++)
343+
{
344+
if (
345+
!(pys_base = (PyArrayObject *)PyIter_Next(iter_base)) ||
346+
!(pys_wT = (PyArrayObject *)PyIter_Next(iter_wT)) ||
347+
!(pys_sT = (PyArrayObject *)PyIter_Next(iter_sT)))
348+
return NULL;
349+
350+
link->shape_base[i] = (npy_float64 *)PyArray_DATA(pys_base);
351+
link->shape_wT[i] = (npy_float64 *)PyArray_DATA(pys_wT);
352+
link->shape_sT[i] = (npy_float64 *)PyArray_DATA(pys_sT);
353+
}
354+
355+
// if (n_shapes > 2)
356+
// {
357+
// copy(link->shape_base[0], link->shape_base[1]);
358+
// }
359+
278360
if (jointtype == 0)
279361
{
280362
link->op = rx;
@@ -307,6 +389,7 @@ static PyObject *link_update(PyObject *self, PyObject *args)
307389
link->jindex = jindex;
308390
link->axis = jointtype;
309391
link->parent = parent;
392+
link->n_shapes = n_shapes;
310393

311394
Py_RETURN_NONE;
312395
}

‎roboticstoolbox/core/fknm.h

Lines changed: 5 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -16,33 +16,6 @@
1616
#define TRUE 1
1717
#define FALSE 0
1818

19-
// /*
20-
// * Accessing information within a MATLAB structure is inconvenient and slow.
21-
// * To get around this we build our own robot and link data structures, and
22-
// * copy the information from the MATLAB objects once per call. If the call
23-
// * is for multiple states values then our efficiency becomes very high.
24-
// */
25-
26-
// /* Robot kinematic convention */
27-
// typedef enum _dhtype
28-
// {
29-
// STANDARD,
30-
// MODIFIED
31-
// } DHType;
32-
33-
/* Link joint type */
34-
// typedef enum _axistype
35-
// {
36-
// Rx,
37-
// Ry,
38-
// Rz,
39-
// Tx,
40-
// Ty,
41-
// Tz
42-
// } Sigma;
43-
44-
/* A robot link structure */
45-
4619
typedef struct Link Link;
4720

4821
struct Link
@@ -54,41 +27,14 @@ struct Link
5427
int isflip;
5528
int jindex;
5629
int axis;
57-
PyArrayObject *A; /* link static transform */
30+
int n_shapes;
31+
PyArrayObject *A; /* link static transform */
5832
PyArrayObject *fk; /* link world transform */
5933
void (*op)(npy_float64 *data, double eta);
6034
Link *parent;
35+
npy_float64 **shape_base; /* link visual and collision geometries */
36+
npy_float64 **shape_wT; /* link visual and collision geometries */
37+
npy_float64 **shape_sT; /* link visual and collision geometries */
6138
};
6239

63-
// typedef struct _link
64-
// {
65-
// /**********************************************************
66-
// *************** kinematic parameters *********************
67-
// **********************************************************/
68-
// int isjoint;
69-
// int isflip;
70-
// int jindex;
71-
// int axis;
72-
// PyArrayObject *A; /* link static transform */
73-
// void (*op)(npy_float64 *data, double eta);
74-
// // Link *parent;
75-
// } Link;
76-
77-
// /* A robot */
78-
// typedef struct _robot
79-
// {
80-
// int njoints; /* number of joints */
81-
// Vect *gravity; /* gravity vector */
82-
// DHType dhtype; /* kinematic convention */
83-
// Link *links; /* the links */
84-
// } Robot;
85-
86-
// void newton_euler(
87-
// Robot *robot, /*!< robot object */
88-
// double *tau, /*!< returned joint torques */
89-
// double *qd, /*!< joint velocities */
90-
// double *qdd, /*!< joint accelerations */
91-
// double *fext, /*!< external force on manipulator tip */
92-
// int stride /*!< indexing stride for qd, qdd */
93-
// );
9440
#endif

‎roboticstoolbox/examples/swift_benchmark.py

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
from roboticstoolbox.backends import swift
7+
import roboticstoolbox as rtb
8+
import spatialmath as sm
9+
import numpy as np
10+
import cProfile
11+
12+
env = swift.Swift(_dev=False)
13+
env.launch()
14+
15+
16+
panda = rtb.models.Panda()
17+
panda.q = panda.qr
18+
env.add(panda, show_robot=True)
19+
20+
21+
ev = [0.01, 0, 0, 0, 0, 0]
22+
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
23+
env.step(0.001)
24+
25+
26+
def stepper():
27+
for i in range(10000):
28+
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
29+
# panda.fkine_all_fast(panda.q)
30+
env.step(0.001)
31+
32+
33+
cProfile.run('stepper()')

0 commit comments

Comments
 (0)
0