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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy