8000 Can remove robots and shapes now · yobzhuu/robotics-toolbox-python@eb87a33 · GitHub
[go: up one dir, main page]
Skip to content

Commit eb87a33

Browse files
jhavljhavl
committed
Can remove robots and shapes now
1 parent 7bc43f5 commit eb87a33

File tree

1 file changed

+81
-35
lines changed

1 file changed

+81
-35
lines changed

‎roboticstoolbox/backends/Swift/Swift.py

Lines changed: 81 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
_sw = None
1414
sw = None
1515

16+
SHAPE_ADD = 10000
1617

1718
def _import_swift(): # pragma nocover
1819
import importlib
@@ -258,29 +259,70 @@ def add(
258259
id = len(self.robots)
259260

260261
self.robots.append(robot_object)
261-
return id
262+
return int(id)
262263
elif isinstance(ob, rp.Shape):
263264
shape = ob.to_dict()
264265
if self.display:
265266
id = self._send_socket('shape', shape)
266267
else:
267-
id = len(self.shapes)
268+
id = len(self.shapes) + SHAPE_ADD
268269
self.shapes.append(ob)
269-
return id
270+
return int(id)
270271

271-
def remove(self):
272+
def remove(self, id):
272273
"""
273-
Remove a robot to the graphical scene
274+
Remove a robot/shape from the graphical scene
274275
275276
``env.remove(robot)`` removes the ``robot`` from the graphical
276277
environment.
277-
"""
278278
279-
# TODO - shouldn't this have an id argument? which robot does it remove
280-
# TODO - it can remove any entity?
279+
:param id: the id of the object as returned by the ``add`` method,
280+
or the instance of the object
281+
:type id: Int, Robot or Shape
282+
"""
281283

282284
super().remove()
283285

286+
# ob to remove
287+
idd = None
288+
code = None
289+
290+
if isinstance(id, rp.ERobot):
291+
292+
for i in range(len(self.robots)):
293+
if self.robots[i] is not None and id == self.robots[i]['ob']:
294+
idd = i
295+
code = 'remove_robot'
296+
self.robots[idd] = None
297+
break
298+
299+
elif isinstance(id, rp.Shape):
300+
301+
for i in range(len(self.shapes)):
302+
if self.shapes[i] is not None and id == self.shapes[i]:
303+
idd = i
304+
code = 'remove_shape'
305+
self.shapes[idd] = None
306+
break
307+
308+
elif id >= SHAPE_ADD:
309+
# Number corresponding to shape ID
310+
idd = id - SHAPE_ADD
311+
code = 'remove_shape'
312+
self.shapes[idd] = None
313+
else:
314+
# Number corresponding to robot ID
315+
idd = id
316+
code = 'remove_robot'
317+
self.robots[idd] = None
318+
319+
if idd is None:
320+
raise ValueError(
321+
'the id argument does not correspond with '
322+
'a robot or shape in Swift')
323+
324+
self._send_socket(code, idd)
325+
284326
def hold(self): # pragma: no cover
285327
'''
286328
hold() keeps the browser tab open i.e. stops the browser tab from
@@ -344,52 +386,56 @@ def stop_recording(self):
344386
def _step_robots(self, dt):
345387

346388
for robot_object in self.robots:
347-
robot = robot_object['ob']
389+
if robot_object is not None:
390+
robot = robot_object['ob']
348391

349-
if robot_object['readonly'] or robot.control_type == 'p':
350-
pass # pragma: no cover
392+
if robot_object['readonly'] or robot.control_type == 'p':
393+
pass # pragma: no cover
351394

352-
elif robot.control_type == 'v':
395+
elif robot.control_type == 'v':
353396

354-
for i in range(robot.n):
355-
robot.q[i] += robot.qd[i] * (dt)
397+
for i in range(robot.n):
398+
robot.q[i] += robot.qd[i] * (dt)
356399

357-
if np.any(robot.qlim[:, i] != 0) and \
358-
not np.any(np.isnan(robot.qlim[:, i])):
359-
robot.q[i] = np.min([robot.q[i], robot.qlim[1, i]])
360-
robot.q[i] = np.max([robot.q[i], robot.qlim[0, i]])
400+
if np.any(robot.qlim[:, i] != 0) and \
401+
not np.any(np.isnan(robot.qlim[:, i])):
402+
robot.q[i] = np.min([robot.q[i], robot.qlim[1, i]])
403+
robot.q[i] = np.max([robot.q[i], robot.qlim[0, i]])
361404

362-
elif robot.control_type == 'a':
363-
pass
405+
elif robot.control_type == 'a':
406+
pass
364407

365-
else: # pragma: no cover
366-
# Should be impossible to reach
367-
raise ValueError(
368-
'Invalid robot.control_type. '
369-
'Must be one of \'p\', \'v\', or \'a\'')
408+
else: # pragma: no cover
409+
# Should be impossible to reach
410+
raise ValueError(
411+
'Invalid robot.control_type. '
412+
'Must be one of \'p\', \'v\', or \'a\'')
370413

371414
def _step_shapes(self, dt):
372415

373416
for shape in self.shapes:
417+
if shape is not None:
374418

375-
T = shape.base
376-
t = T.t.astype('float64')
377-
r = T.rpy('rad').astype('float64')
419+
T = shape.base
420+
t = T.t.astype('float64')
421+
r = T.rpy('rad').astype('float64')
378422

379-
t += shape.v[:3] * dt
380-
r += shape.v[3:] * dt
423+
t += shape.v[:3] * dt
424+
r += shape.v[3:] * dt
381425

382-
shape.base = sm.SE3(t) * sm.SE3.RPY(r)
426+
shape.base = sm.SE3(t) * sm.SE3.RPY(r)
383427

384428
def _draw_all(self):
385429

386430
for i in range(len(self.robots)):
387-
self._send_socket(
388-
'robot_poses', [i, self.robots[i]['ob'].fk_dict()])
431+
if self.robots[i] is not None:
432+
self._send_socket(
433+
'robot_poses', [i, self.robots[i]['ob'].fk_dict()])
389434

390435
for i in range(len(self.shapes)):
391-
self._send_socket(
392-
'shape_poses', [i, self.shapes[i].fk_dict()])
436+
if self.shapes[i] is not None:
437+
self._send_socket(
438+
'shape_poses', [i, self.shapes[i].fk_dict()])
393439

394440
def _send_socket(self, code, data=None):
395441
msg = [code, data]

0 commit comments

Comments
 (0)
0