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)
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