8000 Added color support to shapes and methods · Russ76/robotics-toolbox-python@4e3201b · GitHub
[go: up one dir, main page]
Skip to content

Commit 4e3201b

Browse files
jhavljhavl
committed
Added color support to shapes and methods
1 parent 8ea1816 commit 4e3201b

File tree

2 files changed

+101
-16
lines changed

2 files changed

+101
-16
lines changed

‎examples/gimbal.py

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
# #!/usr/bin/env python
2+
# """
3+
# @author Jesse Haviland
4+
# """
5+
6+
import roboticstoolbox as rtb
7+
import spatialmath as sm
8+
import numpy as np
9+
import pathlib
10+
import os
11+
12+
path = os.path.realpath('.')
13+
14+
# Launch the simulator Swift
15+
env = rtb.backends.Swift()
16+
env.launch()
17+
18+
path = pathlib.Path(path) / 'roboticstoolbox' / 'data'
19+
20+
g1 = rtb.Mesh(
21+
filename=str(path / 'gimbal-ring1.stl'),
22+
base=sm.SE3(0, 0, 1.3),
23+
color=[34, 143, 201]
24+
)
25+
# g1.v = [0, 0, 0, 0.4, 0, 0]
26+
27+
g2 = rtb.Mesh(
28+
filename=str(path / 'gimbal-ring2.stl'),
29+
base=sm.SE3(0, 0, 1.3),
30+
color=[31, 184, 72]
31+
)
32+
# g2.v = [0, 0, 0, 0.4, 0.0, 0]
33+
34+
g3 = rtb.Mesh(
35+
filename=str(path / 'gimbal-ring3.stl'),
36+
base=sm.SE3(0, 0, 1.3),
37+
color=[240, 103, 103]
38+
)
39+
# g3.v = [0, 0, 0, 0.4, 0, 0]
40+
41+
env.add(g1)
42+
env.add(g2)
43+
env.add(g3)
44+
45+
46+
def set_one(x):
47+
g1.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.deg2rad(float(x)))
48+
49+
def set_two(x):
50+
g2.base = sm.SE3(0, 0, 1.3) * sm.SE3.Ry(np.deg2rad(float(x)))
51+
52+
def set_three(x):
53+
g3.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rx(np.deg2rad(float(x)))
54+
# g2.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.pi/2) * sm.SE3.Ry(-np.deg2rad(float(x)))
55+
# g2.base = g3.base
56+
57+
58+
env.add_slider(
59+
set_one,
60+
min=-180, max=180,
61+
step=1, value=0,
62+
desc='Ring One')
63+
64+
env.add_slider(
65+
set_two,
66+
min=-180, max=180,
67+
step=1, value=0,
68+
desc='Ring Two')
69+
70+
env.add_slider(
71+
set_three,
72+
min=-180, max=180,
73+
step=1, value=0,
74+
desc='Ring Three')
75+
76+
77+
78+
while(True):
79+
env.process_events()
80+
env.step(0)

‎roboticstoolbox/robot/Shape.py

Lines changed: 21 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -162,14 +162,7 @@ def color(self, value):
162162

163163
default_color = (0.95, 0.5, 0.25, 1.0)
164164

165-
if isinstance(value, tuple):
166-
value = list(value)
167-
168-
if isinstance(value, list):
169-
if len(value) == 3:
170-
value.append(1.0)
171-
value = tuple(value)
172-
elif isinstance(value, str):
165+
if isinstance(value, str):
173166
if _mpl:
174167
try:
175168
value = mpc.to_rgba(value)
@@ -183,6 +176,17 @@ def color(self, value):
183176
'Install using: pip install matplotlib')
184177
elif value is None:
185178
value = default_color
179+
else:
180+
181+
value = np.array(value)
182+
183+
if np.any(value > 1.0):
184+
value = value / 255.0
185+
186+
if value.shape[0] == 3:
187+
value = np.r_[value, 1.0]
188+
189+
value = tuple(value)
186190

187191
self._color = value
188192

@@ -324,10 +328,10 @@ class Mesh(Shape):
324328
325329
"""
326330

327-
def __init__(self, filename=None, scale=[1, 1, 1], base=None):
331+
def __init__(self, filename=None, scale=[1, 1, 1], base=None, color=None):
328332
super(Mesh, self).__init__(
329333
filename=filename, base=base,
330-
scale=scale, stype='mesh')
334+
scale=scale, stype='mesh', color=color)
331335

332336
def _init_pob(self):
333337
name, file_extension = os.path.splitext(self.filename)
@@ -359,10 +363,10 @@ class Cylinder(Shape):
359363
360364
"""
361365

362-
def __init__(self, radius, length, base=None):
366+
def __init__(self, radius, length, base=None, color=None):
363367
super(Cylinder, self).__init__(
364368
base=base, radius=radius, length=length,
365-
stype='cylinder')
369+
stype='cylinder', color=color)
366370

367371
def _init_pob(self):
368372
col = p.createCollisionShape(
@@ -388,9 +392,9 @@ class Sphere(Shape):
388392
389393
"""
390394

391-
def __init__(self, radius, base=None):
395+
def __init__(self, radius, base=None, color=None):
392396
super(Sphere, self).__init__(
393-
base=base, radius=radius, stype='sphere')
397+
base=base, radius=radius, stype='sphere', color=color)
394398

395399
def _init_pob(self):
396400
col = p.createCollisionShape(
@@ -415,8 +419,9 @@ class Box(Shape):
415419
416420
"""
417421

418-
def __init__(self, scale, base=None):
419-
super(Box, self).__init__(base=base, scale=scale, stype='box')
422+
def __init__(self, scale, base=None, color=None):
423+
super(Box, self).__init__(
424+
base=base, scale=scale, stype='box', color=color)
420425

421426
def _init_pob(self):
422427

0 commit comments

Comments
 (0)
0