8000 New geometry strategy · ctc-eng/robotics-toolbox-python@66a04d1 · GitHub
[go: up one dir, main page]
Skip to content

Commit 66a04d1

Browse files
jhavljhavl
committed
New geometry strategy
1 parent 9b71b62 commit 66a04d1

File tree

6 files changed

+59
-41
lines changed

6 files changed

+59
-41
lines changed

‎examples/Swift.py

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -17,23 +17,23 @@
1717
Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
1818

1919
arrived = False
20-
env.add(panda)
20+
env.add(panda, show_collision=True)
2121
time.sleep(1)
2222

2323
dt = 0.05
2424

2525
# env.record_start('file.webm')
2626

27-
# while not arrived:
27+
while not arrived:
2828

29-
# start = time.time()
30-
# v, arrived = rp.p_servo(panda.fkine(), Tep, 1.0)
31-
# panda.qd = np.linalg.pinv(panda.jacobe()) @ v
32-
# env.step(5)
33-
# stop = time.time()
29+
start = time.time()
30+
v, arrived = rp.p_servo(panda.fkine(), Tep, 1.0)
31+
panda.qd = np.linalg.pinv(panda.jacobe()) @ v
32+
env.step(5)
33+
stop = time.time()
3434

35-
# if stop - start < dt:
36-
# time.sleep(dt - (stop - start))
35+
if stop - start < dt:
36+
time.sleep(dt - (stop - start))
3737

3838
# env.record_stop()
3939

‎roboticstoolbox/backend/Swift/Swift.py

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ def close(self):
9090
# Methods to interface with the robots created in other environemnts
9191
#
9292

93-
def add(self, ob):
93+
def add(self, ob, show_robot=True, show_collision=False):
9494
'''
9595
id = add(robot) adds the robot to the external environment. robot must
9696
be of an appropriate class. This adds a robot object to a list of
@@ -102,6 +102,8 @@ def add(self, ob):
102102

103103
if isinstance(ob, rp.ETS):
104104
robot = ob.to_dict()
105+
robot['show_robot'] = show_robot
106+
robot['show_collision'] = show_collision
105107
id = self.swift.robot(robot)
106108
self.robots.append(ob)
107109
return id

‎roboticstoolbox/models/xacro/franka_description/robots/panda_arm.urdf.xacro

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
<?xml version='1.0' encoding='utf-8'?>
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
33
<xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
4-
<xacro:panda_arm safety_distance="0.03"/>
4+
<xacro:panda_arm safety_distance="0.00"/>
55
</robot>

‎roboticstoolbox/models/xacro/franka_description/robots/panda_arm_hand.urdf.xacro

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,6 @@
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
33
<xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
44
<xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
5-
<xacro:panda_arm safety_distance="0.03"/>
6-
<xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8" safety_distance="0.03"/>
5+
<xacro:panda_arm safety_distance="0.00"/>
6+
<xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8" safety_distance="0.00"/>
77
</robot>

‎roboticstoolbox/robot/ETS.py

Lines changed: 17 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -198,30 +198,18 @@ def to_dict(self):
198198
'eta': [],
199199
'q_idx': link.q_idx,
200200
'geometry': [],
201-
'collision': [],
202-
't': link._fk.t.tolist(),
203-
'q': r2q(link._fk.R).tolist()
201+
'collision': []
204202
}
205203

206204
for et in link.ets:
207205
li['axis'].append(et.axis)
208206
li['eta'].append(et.eta)
209207

210-
# for gi in link.geometry:
211-
# g_fk = link._fk * gi.base
212-
# if gi.scale is not None:
213-
# scale = gi.scale.tolist()
214-
# else:
215-
# scale = [1, 1, 1]
216-
# li['geometry'].append({
217-
# 'filename': gi.filename,
218-
# 'scale': scale,
219-
# 't': g_fk.t.tolist(),
220-
# 'q': r2q(g_fk.R).tolist()
221-
# })
208+
for gi in link.geometry:
209+
li['geometry'].append(gi.to_dict())
222210

223211
for gi in link.collision:
224-
li['geometry'].append(gi.to_dict(link._fk))
212+
li['collision'].append(gi.to_dict())
225213

226214
ob['links'].append(li)
227215

@@ -238,10 +226,16 @@ def fk_dict(self):
238226
for link in self.ets:
239227

240228
li = {
241-
't': link._fk.t.tolist(),
242-
'q': r2q(link._fk.R).tolist()
229+
'geometry': [],
230+
'collision': []
243231
}
244232

233+
for gi in link.geometry:
234+
li['geometry'].append(gi.fk_dict())
235+
236+
for gi in link.collision:
237+
li['collision'].append(gi.fk_dict())
238+
245239
ob['links'].append(li)
246240

247241
return ob
@@ -582,6 +576,9 @@ def fkine_all(self, q=None):
582576
for col in self.ets[i].collision:
583577
col.wT = self.ets[i]._fk
584578

579+
for gi in self.ets[i].geometry:
580+
gi.wT = self.ets[i]._fk
581+
585582
def jacob0(self, q=None):
586583
"""
587584
J0 = jacob0(q) is the manipulator Jacobian matrix which maps joint
@@ -947,8 +944,8 @@ def scollision(self):
947944
print(l1.name + ' -> ' + l2.name)
948945
# import code
949946
# code.interact(local=dict(globals(), **locals()))
950-
print(ob1)
951-
print(ob2)
947+
# print(ob1)
948+
# print(ob2)
952949
# print(ret)
953950

954951

‎roboticstoolbox/robot/Shape.py

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
from spatialmath import SE3
77
from spatialmath.base.argcheck import getvector
88
from spatialmath.base import r2q
9+
import numpy as np
910

1011
try:
1112
import fcl
@@ -27,27 +28,45 @@ def __init__(
2728
co=None,
2829
stype=None):
2930

31+
self.co = co
3032
self.base = base
33+
self.wT = None
3134
self.primitive = primitive
3235
self.scale = scale
3336
self.radius = radius
3437
self.length = length
3538
self.filename = filename
36-
self.co = co
3739
self.stype = stype
3840

39-
def to_dict(self, link_fk):
40-
shape_fk = self.base * link_fk
41-
# shape_fk = self.base
41+
def to_dict(self):
42+
43+
if self.stype == 'cylinder':
44+
fk = self.wT * SE3.Rx(np.pi/2)
45+
else:
46+
fk = self.wT
4247

4348
shape = {
4449
'stype': self.stype,
4550
'scale': self.scale.tolist(),
4651
'filename': self.filename,
4752
'radius': self.radius,
4853
'length': self.length,
49-
't': self.wT.t.tolist(),
50-
'q': r2q(self.wT.R).tolist()
54+
't': fk.t.tolist(),
55+
'q': r2q(fk.R).tolist()
56+
}
57+
58+
return shape
59+
60+
def fk_dict(self):
61+
62+
if self.stype == 'cylinder':
63+
fk = self.wT * SE3.Rx(np.pi/2)
64+
else:
65+
fk = self.wT
66+
67+
shape = {
68+
't': fk.t.tolist(),
69+
'q': r2q(fk.R).tolist()
5170
}
5271

5372
return shape
@@ -62,9 +81,9 @@ def wT(self, T):
6281
T = SE3(T)
6382
self._wT = T * self.base
6483

65-
if _fcl:
84+
if _fcl and self.co is not None:
6685
tf = fcl.Transform(self._wT.R, self._wT.t)
67-
# self.co.setTransform(tf)
86+
self.co.setTransform(tf)
6887

6988
@property
7089
def base(self):

0 commit comments

Comments
 (0)
0