8000 Merge branch 'master' of https://github.com/petercorke/robotics-toolb… · A905275/robotics-toolbox-python@09d7792 · GitHub
[go: up one dir, main page]
Skip to content

Commit 09d7792

Browse files
petercorkepetercorke
committed
2 parents d25a912 + f5b188d commit 09d7792

File tree

8 files changed

+252
-143
lines changed

8 files changed

+252
-143
lines changed

‎examples/swift2.py

Lines changed: 87 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -9,32 +9,102 @@
99
import numpy as np
1010

1111
env = swift.Swift()
12-
env.launch()
12+
env.launch(realtime=True)
1313

1414

15+
r = rtb.models.YuMi()
16+
17+
# r.q = [
18+
# -0.6, -0.3, -0.3, 0.2, 0, 0.3,
19+
# 0, 0,
20+
# 0.6, -0.3, 0.3, 0.2, 0, 0.3,
21+
# 0, 0
22+
# ]
23+
24+
env.add(r)
25+
26+
27+
ev_r = [0, 0.1, 0, 0, 0, 0]
28+
ev_l = [0, -0.001, 0, 0, 0, 0]
29+
30+
for i, link in enumerate(r.links):
31+
print(link)
32+
print(i)
33+
34+
35+
# print()
36+
37+
# path, n, _ = r.get_path(end=r.grippers[0])
38+
39+
40+
# def path_to_q(q_partial, robot, path):
41+
# j = 0
42+
# for link in path:
43+
# if link.isjoint:
44+
# q_whole[link.jindex] = q_partial[j]
45+
# j += 1
46+
47+
48+
# for link in path:
49+
# print(link.jindex)
50+
51+
52+
# for i in range(10000):
53+
# qd_r = np.linalg.pinv(r.jacob0(r.q, end=r.grippers[0])) @ ev_r
54+
# # # qd_l = np.linalg.pinv(r.jacob0(r.q, end=r.grippers[1])) @ ev_l
55+
56+
# r.qd[:6] = qd_r[:6]
57+
58+
# # r.qd = resolve_qd(path, qd_whole, qd_subset)
59+
60+
# print(qd_r)
61+
# # # print(qd_l)
62+
# # # r.qd[3] = 1
63+
# env.step(0.001)
64+
65+
env.hold()
66+
1567
# ur = rtb.models.UR5()
1668
# ur.base = sm.SE3(0.3, 1, 0) * sm.SE3.Rz(np.pi / 2)
1769
# ur.q = [-0.4, -np.pi / 2 - 0.3, np.pi / 2 + 0.3, -np.pi / 2, -np.pi / 2, 0]
1870
# env.add(ur)
1971

20-
# lbr = rtb.models.LBR()
21-
# lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi / 2)
22-
# lbr.q = lbr.qr
23-
# env.add(lbr)
72+
# # lbr = rtb.models.LBR()
73+
# # lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi / 2)
74+
# # lbr.q = lbr.qr
75+
# # env.add(lbr)
2476

25-
# k = rtb.models.KinovaGen3()
26-
# k.q = k.qr
27-
# k.q[0] = np.pi + 0.15
28-
# k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi / 2)
29-
# env.add(k)
77+
# # k = rtb.models.KinovaGen3()
78+
# # k.q = k.qr
79+
# # k.q[0] = np.pi + 0.15
80+
# # k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi / 2)
81+
# # env.add(k)
3082

31-
# panda = rtb.models.Panda()
32-
# panda.q = panda.qr
33-
# panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi / 2)
34-
# env.add(panda, show_robot=True)
83+
# # panda = rtb.models.Panda()
84+
# # panda.q = panda.qr
85+
# # panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi / 2)
86+
# # env.add(panda, show_robot=True)
3587

36-
r = rtb.models.YuMi()
37-
env.add(r)
88+
# r = rtb.models.YuMi()
89+
# env.add(r)
3890

3991

40-
env.hold()
92+
# env.hold()
93+
94+
# import roboticstoolbox as rtb
95+
# from spatialmath import SE3
96+
# from swift import Swift
97+
98+
# total_robots = 1000
99+
# robots = [] # list of robots
100+
# d = 1 # robot spacing
101+
# env = Swift(_dev=True)
102+
# env.launch()
103+
# for i in range(total_robots):
104+
# base = SE3(d * (i % 2), d * (i // 2), 0.0) # place them on grid
105+
# robot = rtb.models.URDF.Puma560()
106+
# robot.base = base
107+
# robots.append(robot)
108+
# env.add(robots[i])
109+
110+
# env.hold()

‎roboticstoolbox/core/fknm.c

Lines changed: 52 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,9 @@ static PyObject *fkine_all(PyObject *self, PyObject *args)
146146
}
147147
}
148148

149+
Py_DECREF(iter_links);
150+
free(ret);
151+
149152
Py_RETURN_NONE;
150153
}
151154

@@ -326,6 +329,11 @@ static PyObject *link_init(PyObject *self, PyObject *args)
326329
link->op = tz;
327330
}
328331

332+
Py_DECREF(iter_base);
333+
Py_DECREF(iter_wT);
334+
Py_DECREF(iter_sT);
335+
Py_DECREF(iter_sq);
336+
329337
ret = PyCapsule_New(link, "Link", NULL);
330338
return ret;
331339
}
@@ -378,6 +386,20 @@ static PyObject *link_update(PyObject *self, PyObject *args)
378386
iter_sT = PyObject_GetIter(py_shape_sT);
379387
iter_sq = PyObject_GetIter(py_shape_sq);
380388

389+
if (link->shape_base != 0)
390+
free(link->shape_base);
391+
if (link->shape_wT != 0)
392+
free(link->shape_wT);
393+
if (link->shape_sT != 0)
394+
free(link->shape_sT);
395+
if (link->shape_sq != 0)
396+
free(link->shape_sq);
397+
398+
link->shape_base = 0;
399+
link->shape_wT = 0;
400+
link->shape_sT = 0;
401+
link->shape_sq = 0;
402+
381403
link->shape_base = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
382404
link->shape_wT = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
383405
link->shape_sT = (npy_float64 **)PyMem_RawCalloc(n_shapes, sizeof(npy_float64));
@@ -432,6 +454,11 @@ static PyObject *link_update(PyObject *self, PyObject *args)
432454
link->parent = parent;
433455
link->n_shapes = n_shapes;
434456

457+
Py_DECREF(iter_base);
458+
Py_DECREF(iter_wT);
459+
Py_DECREF(iter_sT);
460+
Py_DECREF(iter_sq);
461+
435462
Py_RETURN_NONE;
436463
}
437464

@@ -591,6 +618,13 @@ void _jacobe(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool,
591618
}
592619
}
593620
PyList_Reverse(links);
621+
622+
Py_DECREF(iter_links);
623+
624+
free(T);
625+
free(U);
626+
free(temp);
627+
free(ret);
594628
}
595629

596630
void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *J)
@@ -693,6 +727,14 @@ void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool,
693727
copy(temp, U);
694728
}
695729
}
730+
731+
Py_DECREF(iter_links);
732+
733+
free(T);
734+
free(U);
735+
free(temp);
736+
free(ret);
737+
free(invU);
696738
}
697739

698740
void _fkine(PyObject *links, int n, npy_float64 *q, npy_float64 *etool, npy_float64 *tool, npy_float64 *ret)
@@ -723,8 +765,13 @@ void _fkine(PyObject *links, int n, npy_float64 *q, npy_float64 *etool, npy_floa
723765
}
724766

725767
mult(current, etool, ret);
726-
copy(temp, current);
768+
copy(ret, current);
727769
mult(current, tool, ret);
770+
771+
Py_DECREF(iter_links);
772+
773+
free(temp);
774+
free(current);
728775
}
729776

730777
void A(Link *link, npy_float64 *ret, double eta)
@@ -748,6 +795,7 @@ void A(Link *link, npy_float64 *ret, double eta)
748795

749796
// Multiply ret = A * v
750797
mult(link->A, v, ret);
798+
free(v);
751799
}
752800

753801
void copy(npy_float64 *A, npy_float64 *B)
@@ -1074,6 +1122,7 @@ int _inv(npy_float64 *m, npy_float64 *invOut)
10741122
for (i = 0; i < 16; i++)
10751123
invOut[i] = inv[i] * det;
10761124

1125+
free(inv);
10771126
return 1;
10781127
}
10791128

@@ -1091,8 +1140,8 @@ void _r2q(npy_float64 *r, npy_float64 *q)
10911140
t13m = pow((r[0 * 4 + 2] - r[2 * 4 + 0]), 2);
10921141
t23m = pow((r[1 * 4 + 2] - r[2 * 4 + 1]), 2);
10931142

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);
1143+
d1 = pow((r[0 * 4 + 0] + r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
1144+
d2 = pow((r[0 * 4 + 0] - r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
10961145
d3 = pow((-r[0 * 4 + 0] + r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
10971146
d4 = pow((-r[0 * 4 + 0] - r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
10981147

‎roboticstoolbox/examples/swift_benchmark.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,8 @@
55

66
from roboticstoolbox.backends import swift
77
import roboticstoolbox as rtb
8-
import spatialmath as sm
98
import numpy as np
109
import cProfile
11-
# import numpy.testing as nt
1210

1311
env = swift.Swift(_dev=False)
1412
env.launch()
@@ -27,8 +25,10 @@
2725
def stepper():
2826
for i in range(10000):
2927
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
28+
# panda.jacob0(panda.q, fast=True)
29+
3030
# panda.fkine_all_fast(panda.q)
3131
env.step(0.001)
3232

3333

34-
cProfile.run('stepper()')
34+
cProfile.run("stepper()")

0 commit comments

Comments
 (0)
0