Skip to content

Commit 77eb32f

Browse files
jhavljhavl
committed
shapes
1 parent 844347c commit 77eb32f

File tree

4 files changed

+115
-86
lines changed

4 files changed

+115
-86
lines changed

‎examples/Swift.py

Lines changed: 0 additions & 41 deletions
This file was deleted.

‎examples/swift.py

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
import roboticstoolbox as rp
7+
import spatialmath as sm
8+
import numpy as np
9+
import time
10+
import qpsolvers as qp
11+
12+
env = rp.backend.Swift()
13+
env.launch()
14+
15+
panda = rp.models.Panda()
16+
panda.q = panda.qr
17+
# Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
18+
Tep = panda.fkine() * sm.SE3.Tz(0.6) * sm.SE3.Tx(-0.1) #* sm.SE3.Ty(-0.1)
19+
20+
sphere = rp.Shape.Sphere(0.05, sm.SE3(0.5, 0, 0.2))
21+
22+
arrived = False
23+
env.add(panda, show_collision=False, show_robot=True)
24+
env.add(sphere)
25+
time.sleep(1)
26+
27+
dt = 0.05
28+
ps = 0.05
29+
pi = 0.9
30+
31+
# env.record_start('file.webm')
32+
33+
while not arrived:
34+
35+
start = time.time()
36+
v, arrived = rp.p_servo(panda.fkine(), Tep, 1.0)
37+
# panda.qd = np.linalg.pinv(panda.jacobe()) @ v
38+
# env.step(5)
39+
40+
v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
41+
eTep = panda.fkine().inv() * Tep
42+
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi/180]))
43+
Y = 0.01
44+
45+
# Ain = np.zeros((panda.n + 6, panda.n + 6))
46+
# bin = np.zeros(panda.n + 6)
47+
48+
# for i in range(panda.n):
49+
# if panda.q[i] - panda.qlim[0, i] <= pi:
50+
# bin[i] = -1.0 * (((panda.qlim[0, i] - panda.q[i]) + ps) / (pi - ps))
51+
# Ain[i, i] = -1
52+
# if panda.qlim[1, i] - panda.q[i] <= pi:
53+
# bin[i] = ((panda.qlim[1, i] - panda.q[i]) - ps) / (pi - ps)
54+
# Ain[i, i] = 1
55+
56+
Q = np.eye(panda.n + 6)
57+
Q[:panda.n, :panda.n] *= Y
58+
Q[panda.n:, panda.n:] = (1 / e) * np.eye(6)
59+
Aeq = np.c_[panda.jacobe(), np.eye(6)]
60+
beq = v.reshape((6,))
61+
c = np.r_[-panda.jacobm().reshape((panda.n,)), np.zeros(6)]
62+
63+
# Distance Jacobian
64+
eTc0 = panda.fkine().inv() * sphere.base
65+
d0 = np.linalg.norm(eTc0.t)
66+
n0 = eTc0.t / d0
67+
ds = 0.05
68+
di = 0.6
69+
70+
if d0 <= di:
71+
nh0 = np.expand_dims(np.r_[n0, 0, 0, 0], axis=0)
72+
Ain = np.c_[nh0 @ panda.jacobe(), np.zeros((1, 6))]
73+
bin = np.array([0.1 * (d0 - ds) / (di - ds)])
74+
else:
75+
Ain = None
76+
bin = None
77+
78+
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq)
79+
80+
if np.any(np.isnan(qd)):
81+
panda.qd = panda.qz
82+
else:
83+
panda.qd = qd[:panda.n]
84+
85+
env.step(25)
86+
87+
stop = time.time()
88+
if stop - start < dt:
89+
time.sleep(dt - (stop - start))
90+
91+
# env.record_stop()
92+
93+
# Uncomment to stop the plot from closing
94+
# env.hold()

‎roboticstoolbox/backend/Swift/Swift.py

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ def launch(self):
3232
super().launch()
3333

3434
self.robots = []
35+
self.shapes = []
3536

3637
self.swift = zerorpc.Client()
3738
self.swift.connect("tcp://127.0.0.1:4242")
@@ -107,6 +108,12 @@ def add(self, ob, show_robot=True, show_collision=False):
107108
id = self.swift.robot(robot)
108109
self.robots.append(ob)
109110
return id
111+
elif isinstance(ob, rp.Shape):
112+
shape = ob.to_dict()
113+
id = self.swift.shape(shape)
114+
self.shapes.append(ob)
115+
return id
116+
110117

111118
def remove(self):
112119
'''

‎roboticstoolbox/robot/ETS.py

Lines changed: 14 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -617,72 +617,41 @@ def jacob0(self, q=None):
617617
if k != link.q_idx:
618618
U = U @ link.ets[k].T().A
619619
else:
620-
if link.ets[k].axis == 'Rz':
621-
U = U @ link.ets[k].T(q[j]).A
622-
Tu = np.linalg.inv(U) @ T
623-
624-
n = U[:3, 0]
625-
o = U[:3, 1]
626-
a = U[:3, 2]
627-
y = Tu[1, 3]
628-
x = Tu[0, 3]
620+
# self._jacoblink(link, k, T)
621+
U = U @ link.ets[k].T(q[j]).A
622+
Tu = np.linalg.inv(U) @ T
623+
n = U[:3, 0]
624+
o = U[:3, 1]
625+
a = U[:3, 2]
626+
x = Tu[0, 3]
627+
y = Tu[1, 3]
628+
z = Tu[2, 3]
629629

630+
if link.ets[k].axis == 'Rz':
630631
J[:3, j] = (o * x) - (n * y)
631632
J[3:, j] = a
632633

633-
j += 1
634-
if link.ets[k].axis == 'Ry':
635-
U = U @ link.ets[k].T(q[j]).A
636-
Tu = np.linalg.inv(U) @ T
637-
638-
n = U[:3, 0]
639-
o = U[:3, 1]
640-
a = U[:3, 2]
641-
z = Tu[2, 3]
642-
x = Tu[0, 3]
643-
634+
elif link.ets[k].axis == 'Ry':
644635
J[:3, j] = (n * z) - (a * x)
645636
J[3:, j] = o
646637

647-
j += 1
648-
if link.ets[k].axis == 'Rx':
649-
U = U @ link.ets[k].T(q[j]).A
650-
Tu = np.linalg.inv(U) @ T
651-
652-
n = U[:3, 0]
653-
o = U[:3, 1]
654-
a = U[:3, 2]
655-
y = Tu[1, 3]
656-
z = Tu[2, 3]
657-
638+
elif link.ets[k].axis == 'Rx':
658639
J[:3, j] = (a * y) - (o * z)
659640
J[3:, j] = n
660641

661-
j += 1
662642
elif link.ets[k].axis == 'tx':
663-
U = U @ link.ets[k].T(q[j]).A
664-
n = U[:3, 0]
665-
666643
J[:3, j] = n
667644
J[3:, j] = np.array([0, 0, 0])
668645

669-
j += 1
670646
elif link.ets[k].axis == 'ty':
671-
U = U @ link.ets[k].T(q[j]).A
672-
o = U[:3, 1]
673-
674647
J[:3, j] = o
675648
J[3:, j] = np.array([0, 0, 0])
676649

677-
j += 1
678650
elif link.ets[k].axis == 'tz':
679-
U = U @ link.ets[k].T(q[j]).A
680-
a = U[:3, 2]
681-
682651
J[:3, j] = a
683652
J[3:, j] = np.array([0, 0, 0])
684653

685-
j += 1
654+
j += 1
686655

687656
return J
688657

@@ -956,7 +925,7 @@ def scollision(self):
956925
# t1 = ob1.wT.t + np1
957926
# t2 = ob2.wT.t + np2
958927
# d = np.linalg.norm(bTp - aTp)
959-
d = np.linalg.norm(np1 - np2)
928+
# d = np.linalg.norm(np1 - np2)
960929
print(d)
961930

962931

0 commit comments

Comments
 (0)