8000 new ik methods · weizx208/robotics-toolbox-python@713939a · GitHub
[go: up one dir, main page]
Skip to content

Commit 713939a

Browse files
jhavljhavl
committed
new ik methods
1 parent 27e8bd6 commit 713939a

File tree

1 file changed

+75
-43
lines changed

1 file changed

+75
-43
lines changed

‎roboticstoolbox/core/fknm.c

Lines changed: 75 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
#include <math.h>
1313
#include "fknm.h"
1414
#include <stdio.h>
15+
// #include "findblas.h"
16+
// #include <npy_cblas.h>
1517

1618
// forward defines
1719
static PyObject *IK(PyObject *self, PyObject *args);
@@ -1284,25 +1286,43 @@ void _ETS_IK(PyObject *ets, int n, npy_float64 *q, npy_float64 *Tep, npy_float64
12841286
npy_float64 *e = (npy_float64 *)PyMem_RawCalloc(6, sizeof(npy_float64));
12851287

12861288
npy_float64 *a = (npy_float64 *)PyMem_RawCalloc(6, sizeof(npy_float64));
1289+
// a[0] = 1.0;
1290+
// a[1] = 4.0;
1291+
// a[2] = 2.0;
1292+
// a[3] = 5.0;
1293+
// a[4] = 3.0;
1294+
// a[5] = 6.0;
12871295
a[0] = 1.0;
1288-
a[1] = 4.0;
1289-
a[2] = 2.0;
1290-
a[3] = 5.0;
1291-
a[4] = 3.0;
1296+
a[1] = 2.0;
1297+
a[2] = 3.0;
1298+
a[3] = 4.0;
1299+
a[4] = 5.0;
12921300
a[5] = 6.0;
12931301
npy_float64 *b = (npy_float64 *)PyMem_RawCalloc(12, sizeof(npy_float64));
12941302
b[0] = 11.0;
1295-
b[1] = 12.0;
1296-
b[2] = 13.0;
1297-
b[3] = 14.0;
1298-
b[4] = 15.0;
1299-
b[5] = 16.0;
1300-
b[6] = 17.0;
1301-
b[7] = 18.0;
1302-
b[8] = 19.0;
1303-
b[9] = 20.0;
1304-
b[10] = 21.0;
1303+
b[1] = 15.0;
1304+
b[2] = 19.0;
1305+
b[3] = 12.0;
1306+
b[4] = 16.0;
1307+
b[5] = 20.0;
1308+
b[6] = 13.0;
1309+
b[7] = 17.0;
1310+
b[8] = 21.0;
1311+
b[9] = 14.0;
1312+
b[10] = 18.0;
13051313
b[11] = 22.0;
1314+
// b[0] = 11.0;
1315+
// b[1] = 12.0;
1316+
// b[2] = 13.0;
1317+
// b[3] = 14.0;
1318+
// b[4] = 15.0;
1319+
// b[5] = 16.0;
1320+
// b[6] = 17.0;
1321+
// b[7] = 18.0;
1322+
// b[8] = 19.0;
1323+
// b[9] = 20.0;
1324+
// b[10] = 21.0;
1325+
// b[11] = 22.0;
13061326

13071327
// npy_float64 *U = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
13081328
// npy_float64 *invU = (npy_float64 *)PyMem_RawCalloc(16, sizeof(npy_float64));
@@ -1311,22 +1331,33 @@ void _ETS_IK(PyObject *ets, int n, npy_float64 *q, npy_float64 *Tep, npy_float64
13111331
// Py_ssize_t m;
13121332
int arrived = 0, iter = 0;
13131333

1314-
// while (arrived == 0 && iter < 500)
1315-
// {
1316-
// // Current pose Te
1317-
// _ETS_fkine(ets, q, (npy_float64 *)NULL, NULL, Te);
1334+
while (arrived == 0 && iter < 500)
1335+
{
1336+
// Current pose Te
1337+
_ETS_fkine(ets, q, (npy_float64 *)NULL, NULL, Te);
13181338

1319-
// // Angle axis error e
1320-
// _angle_axis(Te, Tep, e);
1339+
// Angle axis error e
1340+
_angle_axis(Te, Tep, e);
13211341

1322-
// // Squared error E
1323-
// // E = 0.5 * e @ We @ e
1324-
// E = 0.5 * (e[0] * e[0] + e[1] * e[1] + e[2] * e[2] + e[3] * e[3] + e[4] * e[4] + e[5] * e[5]);
1325-
// }
1342+
// Squared error E
1343+
// E = 0.5 * e @ We @ e
1344+
E = 0.5 * (e[0] * e[0] + e[1] * e[1] + e[2] * e[2] + e[3] * e[3] + e[4] * e[4] + e[5] * e[5]);
1345+
}
13261346

1327-
_ETS_fkine(ets, q, (npy_float64 *)NULL, NULL, Te);
1347+
// _ETS_fkine(ets, q, (npy_float64 *)NULL, NULL, Te);
13281348

1329-
_mult_T(3, 2, 1, a, 3, 4, 0, b, ret);
1349+
// for (int i = 0; i < 2; i++)
1350+
// {
1351+
// for (int j = 0; j < 4; j++)
1352+
// {
1353+
// ret[i * 4 + j] = 0.0;
1354+
// }
1355+
// }
1356+
1357+
// _mult_T(2, 3, 0, a, 3, 4, 0, b, ret);
1358+
// _mult_T(3, 2, 1, a, 3, 4, 0, b, ret);
1359+
// _mult_T(3, 2, 1, a, 4, 3, 1, b, ret);
1360+
// _mult_T(2, 3, 0, a, 4, 3, 1, b, ret);
13301361

13311362
int j = 0;
13321363
}
@@ -1608,7 +1639,8 @@ void _ETS_fkine(PyObject *ets, npy_float64 *q, npy_float64 *base, npy_float64 *t
16081639
return;
16091640

16101641
_ET_T(et, ret, q[et->jindex]);
1611-
mult(current, ret, temp);
1642+
// mult(current, ret, temp);
1643+
// cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, 4, 4, 4, 1.0, current, 4, ret, 4, 0.0, temp, 4);
16121644
copy(temp, current);
16131645
}
16141646

@@ -2283,17 +2315,17 @@ void _mult_T(int n, int m, int AT, npy_float64 *A, int p, int q, int BT, npy_flo
22832315

22842316
if (AT)
22852317
{
2286-
n = temp;
2318+
temp = n;
22872319
n = m;
22882320
m = temp;
22892321
}
22902322

2291-
// if (BT)
2292-
// {
2293-
// p = temp;
2294-
// p = q;
2295-
// q = temp;
2296-
// }
2323+
if (BT)
2324+
{
2325+
temp = p;
2326+
p = q;
2327+
q = temp;
2328+
}
22972329

22982330
for (i = 0; i < n; i++)
22992331
{
@@ -2304,21 +2336,21 @@ void _mult_T(int n, int m, int AT, npy_float64 *A, int p, int q, int BT, npy_flo
23042336
{
23052337
if (AT)
23062338
{
2307-
a = A[k * m + i];
2339+
a = A[k * n + i];
23082340
}
23092341
else
23102342
{
23112343
a = A[i * m + k];
23122344
}
23132345

2314-
// if (BT)
2315-
// {
2316-
// b = B[j * q + k];
2317-
// }
2318-
// else
2319-
// {
2320-
b = B[k * q + j];
2321-
// }
2346+
if (BT)
2347+
{
2348+
b = B[j * p + k];
2349+
}
2350+
else
2351+
{
2352+
b = B[k * q + j];
2353+
}
23222354

23232355
num += a * b;
23242356
}

0 commit comments

Comments
 (0)
0