def render_workspace_grid(self, workspace: _grid.Result, *args, **kwargs):
# option to plot only the points inside the workspace
only_inside = kwargs.pop('only_inside', False)
# plot the points inside the workspace
_mlab.points3d(
*self._prepare_plot_coordinates(
self._extract_coordinates(workspace.inside.T)),
**update_recursive(
dict(
name='workspace: inside',
color=(0, 1, 0),
scale_factor=0.10,
resolution=3,
), kwargs))
if not only_inside:
# plot the points outside the workspace
_mlab.points3d(
*self._prepare_plot_coordinates(
self._extract_coordinates(workspace.outside.T)),
**update_recursive(
dict(
name='workspace: outside',
color=(1, 0, 0),
scale_factor=0.10,
resolution=3,
), kwargs))
def render_polyhedron(self, polyhedron: _geometry.Polyhedron, *args,
**kwargs):
# render the polyehedron as mesh
self.figure.add_trace(
self._mesh(
**self._prepare_plot_coordinates(
self._extract_coordinates(
polyhedron.vertices.T)),
**self._prepare_plot_coordinates(polyhedron.faces.T,
('i', 'j', 'k')),
**update_recursive(
dict(
facecolor=['rgb(178, 178, 178)'] *
polyhedron.faces.shape[0],
vertexcolor=['rgb(255, 0, 0)'] *
polyhedron.vertices.shape[
0],
flatshading=True,
opacity=0.75,
contour=dict(
show=True,
color='rgb(0, 0, 0)',
),
name='polyhedron',
hoverinfo='skip',
hovertext='',
),
kwargs.pop('mesh', {})
)
)
)
# and render each edge
for face, vertices in polyhedron:
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(vertices.T)),
**update_recursive(
dict(
mode='lines',
line=dict(
color='rgb(0, 0, 0)',
),
name='polyhedron face',
hoverinfo='skip',
hovertext='',
showlegend=False,
),
kwargs.pop('lines', {})
)
)
)
def render_coordinate_system(self,
position: Vector = None,
dcm: Matrix = None,
**kwargs):
# default position value
if position is None:
position = _np.zeros((3, ))
# default orientation of the coordinate system
if dcm is None:
dcm = _np.eye(3)
marker_style = kwargs.pop('marker', {})
axes_styles = kwargs.pop('axes', {})
# draw center of the coordinate system
_mlab.points3d(
*self._prepare_plot_coordinates(
self._extract_coordinates(position)),
**update_recursive(
dict(
color=(0, 0, 0),
scale_factor=0.10,
resolution=3,
), marker_style))
# scaling of coordiante axis length
scale = kwargs.pop('scale', 0.25)
# plot each coordinate axis i.e., x, y, z or which of them are available
name = kwargs.pop('name', 'axis {}')
for idx in range(self._NUMBER_OF_COORDINATES):
try:
name_ = name.format(self.AXES_NAMES[idx],
axis=self.AXES_NAMES[idx])
except KeyError:
name_ = f'{name}: {self.AXES_NAMES[idx]}'
_mlab.quiver3d(
*self._prepare_plot_coordinates(
self._extract_coordinates(
_np.vstack(
(position, position + scale *
dcm.dot(self.COORDINATE_DIRECTIONS[idx]))).T)),
**update_recursive(
dict(
color=self.COORDINATE_DIRECTIONS[idx],
line_width=0.10,
name=name_,
), axes_styles))
def render_cuboid(self, cuboid: _geometry.Cuboid, *args, **kwargs):
"""
Render a cuboid by rendering it as a rectangle with only the width
and depth used because the height is assumed to be along the vertical
axis with is perpendicular to the plane of a planar robot
Parameters
----------
cuboid
args
kwargs
Returns
-------
"""
# get transformation to apply
transform = kwargs.get('transformation', _HomogenousTransformation())
faces = cuboid.faces
vertices = cuboid.vertices
# scale vertices to account for the dimensions
vertices = transform.apply((vertices).T).T
_mlab.triangular_mesh(
*self._prepare_plot_coordinates(
self._extract_coordinates(vertices.T)), faces,
**update_recursive(
dict(
name='cuboid',
representation='surface',
color=self._RGB2rgb((178, 178, 178)),
line_width=0.10,
), kwargs.pop('mesh', {})))
def render_platform_anchor(
self,
anchor: _robot.PlatformAnchor,
*args,
transformation: _HomogenousTransformation = None,
**kwargs):
# default value for transformation, if the platform has no pose
if transformation is None:
transformation = _HomogenousTransformation()
# anchor index from outside
aidx = kwargs.pop('loop_index', -1)
# platform index from outside
pidx = kwargs.pop('platform_index', -1)
_mlab.points3d(
*self._prepare_plot_coordinates(
self._extract_coordinates(
transformation.apply(anchor.linear.position))),
**update_recursive(
dict(
name=f'platform {pidx}: anchor {aidx}',
color=(1, 0, 0),
scale_factor=0.10,
resolution=3,
), kwargs))
def render_platform_anchor(self,
anchor: _robot.PlatformAnchor,
*args,
transformation: _HomogenousTransformation = None,
**kwargs):
# default value for transformation, if the platform has no pose
if transformation is None:
transformation = _HomogenousTransformation()
# anchor index from outside
aidx = kwargs.pop('loop_index', -1)
# platform index from outside
pidx = kwargs.pop('platform_index', -1)
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(
transformation.apply(
anchor.linear.position))),
**update_recursive(dict(
mode='markers',
marker=dict(
color='Red',
size=3 if self._NUMBER_OF_AXES == 3
else 5,
),
name=f'platform {pidx}: anchor {aidx}',
hoverinfo='text',
hovertext=f'platform {pidx}: anchor {aidx}',
showlegend=False,
), kwargs)
)
)
def draw(self, *args, **kwargs):
super().draw(*args,
**update_recursive(
dict(
yaxis=dict(
scaleanchor="x",
scaleratio=1,
)
),
kwargs)
)
def draw(self, *args, **kwargs):
super().draw(*args,
**update_recursive(
dict(
yaxis=dict(
scaleanchor="x",
scaleratio=1,
showline=False,
showticklabels=False,
showgrid=False,
)
),
kwargs)
)
def render_polyhedron(self, polyhedron: _geometry.Polyhedron, *args,
**kwargs):
# render the polyehedron as mesh
_mlab.triangular_mesh(
*self._prepare_plot_coordinates(
self._extract_coordinates(polyhedron.vertices.T)),
polyhedron.faces,
**update_recursive(
dict(
color=self._RGB2rgb((178, 178, 178)),
line_width=0.10,
), kwargs.pop('mesh', {})))
# and render each edge
for face, vertices in polyhedron:
_mlab.plot3d(
*self._prepare_plot_coordinates(
self._extract_coordinates(vertices.T)),
**update_recursive(
dict(
color=(0, 0, 0),
line_width=0.10,
tube_radius=None,
), kwargs.pop('lines', {})))
def render_platform(self, platform: _robot.Platform, *args, **kwargs):
# platform position and orientation
position = platform.pose.linear.position
dcm = platform.pose.angular.dcm
# temporary platform loop index
pidx = kwargs.pop('loop_index', -1)
# if the platform has a geometry assigned, we will plot the geometry
# and only add the anchor points to it
if platform.geometry is not None:
self.render(platform.geometry,
transformation=platform.pose.transformation,
name=f'platform {pidx}')
# otherwise, without a platform geometry, we will triangulate the
# anchor points and plot the platform shape via that
else:
# render bounding box of platform
if platform.is_cuboid:
# get original anchors as (K,M) matrix
anchors = self._extract_coordinates(platform.bi.swapaxes(0, 1))
# in 3D, we perform delaunay triangulation of the corners and
# retrieve the convex hull from there
try:
delau = _Delaunay(anchors.T)
except QhullError as e:
warnings.warn(RuntimeWarning(e))
return
edges = delau.convex_hull
vertices = delau.points
# ensure vertices are (N,3) arrays
vertices = _np.pad(vertices,
((0, 0), (0, 3 - vertices.shape[1])))
# also rotate and translate the platform anchors
vertices = (position[:, _np.newaxis] + dcm.dot(vertices.T)).T
# 3D plot
# first, plot the mesh of the platform i.e., its volume
_mlab.triangular_mesh(
*self._prepare_plot_coordinates(
self._extract_coordinates(vertices.T)), edges,
**update_recursive(
dict(
color=(0, 0, 0),
line_width=0.10,
), kwargs))
# close all edges by appending the first column
edges = _np.hstack((edges, edges[:, 0, _np.newaxis]))
# and loop over each edge to plot
for edge in edges:
_mlab.plot3d(
*self._prepare_plot_coordinates(vertices[edge, :].T),
**update_recursive(
dict(
color=self._RGB2rgb((13, 13, 13)),
line_width=0.10,
), kwargs))
# render all anchors
self._render_component_list(
platform,
'anchors',
transformation=platform.pose.transformation,
platform_index=pidx,
**kwargs)
# render reference coordinate system of platform
self.render_coordinate_system(position,
name=f'platform {pidx}: center')
# render rotated coordinate system of platform
if platform.can_rotate:
self.render_coordinate_system(position,
dcm,
name=f'platform {pidx}')
def render_ellipsoid(self, ellipsoid: _geometry.Ellipsoid, *args,
**kwargs):
# get transformation to apply
transform = kwargs.get('transformation', _HomogenousTransformation())
# quicker access to width, depth, and height of cuboid
radii = ellipsoid.radius
az_ = _np.linspace(-_np.pi, _np.pi, num=36, endpoint=True)
el_ = _np.linspace(-_np.pi / 2, _np.pi / 2, num=18, endpoint=True)
az, el = _np.meshgrid(az_, el_)
x = radii[0] * _np.cos(el) * _np.cos(az)
y = radii[1] * _np.cos(el) * _np.sin(az)
z = radii[2] * _np.sin(el)
vertices = _np.stack((x, y, z), axis=1).T
# apply transformation to the vertices
try:
vertices = transform.apply(vertices)
except ValueError:
vertices = _np.asarray(
[transform.apply(page) for page in vertices.T]).T
vertices = _np.stack(
[transform.apply(page) for page in vertices.T], axis=0)
# outside surface
_mlab.surf(
*self._prepare_plot_coordinates(
self._extract_coordinates(_np.swapaxes(vertices, 0, 1))),
**update_recursive(
dict(
name='ellipsoid',
color=self._RGB2rgb((178, 178, 178)),
line_width=0.10,
), kwargs))
# create a new linearly spaced elevation vector which spans from
# all the way the south pole to the north pole
el_ = _np.linspace(-_np.pi, _np.pi, num=36, endpoint=True)
# calculate the circles of the principal planes (XY, YZ, XZ)
circles = [(_np.vstack((
radii[0] * _np.cos(az_),
radii[1] * _np.sin(az_),
_np.zeros_like(az_),
)), 'rgb(0, 0, 255)'),
(_np.vstack((
_np.zeros_like(el_),
radii[1] * _np.cos(el_),
radii[2] * _np.sin(el_),
)), 'rgb(255, 0, 0)'),
(_np.vstack((
radii[0] * _np.cos(el_),
_np.zeros_like(el_),
radii[2] * _np.sin(el_),
)), 'rgb(0, 255, 0)')]
for circle, color in circles:
_mlab.surf(
*self._prepare_plot_coordinates(
self._extract_coordinates(transform.apply(circle))), )
def render_cuboid(self,
cuboid: _geometry.Cuboid,
*args,
**kwargs):
"""
Render a cuboid by rendering it as a rectangle with only the width
and depth used because the height is assumed to be along the vertical
axis with is perpendicular to the plane of a planar robot
Parameters
----------
cuboid
args
kwargs
Returns
-------
"""
# get transformation to apply
transform = kwargs.get('transformation', _HomogenousTransformation())
if self._NUMBER_OF_COORDINATES == 1:
faces = _np.asarray(
[0, 1, 0]
)
elif self._NUMBER_OF_COORDINATES == 2:
faces = _np.asarray(
[0, 1, 2, 3, 0]
)
else:
faces = cuboid.faces
vertices = cuboid.vertices
# scale vertices to account for the dimensions
vertices = transform.apply(vertices.T).T
if self._NUMBER_OF_AXES == 3:
self.figure.add_trace(
self._mesh(
**self._prepare_plot_coordinates(
self._extract_coordinates(vertices.T)),
**self._prepare_plot_coordinates(faces.T,
('i', 'j', 'k')),
**update_recursive(dict(
facecolor=['rgb(178, 178, 178)']
* faces.shape[0],
vertexcolor=['rgb(0, 0, 0)']
* vertices.shape[0],
flatshading=True,
opacity=0.75,
showscale=False,
name='cuboid',
hoverinfo='skip',
hovertext='',
), kwargs.pop('mesh', {}))
)
)
# close faces
faces = _np.append(faces, faces[:, 0, _np.newaxis], axis=1)
# draw each edge separately
for face in faces:
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(
vertices[face, :].T)),
**update_recursive(dict(
mode='lines',
line=dict(
color='rgb(0, 0, 0)',
),
name='cuboid face',
hoverinfo='skip',
hovertext='',
showlegend=False,
), kwargs.pop('faces', {})),
)
)
else:
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(
vertices.T[:, faces])),
**update_recursive(dict(
mode='lines',
fill='toself',
line=dict(
color='rgb(13, 13, 13)'
),
fillcolor='rgb(178, 178, 178)',
showlegend=False,
name='cuboid',
hoverinfo='skip',
hovertext='',
), kwargs.pop('mesh', {})),
)
)
def render_coordinate_system(self,
position: Vector = None,
dcm: Matrix = None,
**kwargs):
# default position value
if position is None:
position = _np.zeros((3,))
# default orientation of the coordinate system
if dcm is None:
dcm = _np.eye(3)
# draw center of the coordinate system
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(position)),
**update_recursive(dict(
mode='markers',
marker=dict(
color='Black',
size=3 if self._NUMBER_OF_AXES == 3
else 5,
),
showlegend=False,
), kwargs)
)
)
# scaling of coordiante axis length
scale = kwargs.pop('scale', 0.25)
# plot each coordinate axis i.e., x, y, z or which of them are available
for idx in range(self._NUMBER_OF_COORDINATES):
# convert rgb in range of [0...1] to RGB in range of [0...255]
rgb = self._rgb2RGB(self.COORDINATE_DIRECTIONS[idx])
# plot the axis as a line (currently, a quiver isn't supported in
# 2D... no idea why, plot.ly?!)
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(
_np.vstack((position,
position + scale *
dcm.dot(
self.COORDINATE_DIRECTIONS[
idx]))).T)),
**update_recursive(dict(
mode='lines',
line=dict(
color=f'rgb({rgb[0]},{rgb[1]},'
f'{rgb[2]})',
width=4 if self._NUMBER_OF_AXES
== 3 else 2,
),
name=f'{kwargs.get("name")}: axis {idx}',
hoverinfo='text',
hovertext=f'{kwargs.get("name")}: axis '
f'{idx}',
showlegend=False,
), kwargs)
)
)
