def surface_area(self):
if self._surface is None:
try:
# triangulate all points that are inside the workspace
delau = _Delaunay(self.inside)
# convex null of this area will be used to determine the
# workspace volume and surface area
hull_faces = delau.convex_hull
# get each vertex
f0 = self.inside[hull_faces[:, 0], :]
f1 = self.inside[hull_faces[:, 1], :]
f2 = self.inside[hull_faces[:, 2], :]
# length of each side
a = _np.linalg.norm(f0 - f1, axis=1)
b = _np.linalg.norm(f1 - f2, axis=1)
c = _np.linalg.norm(f2 - f0, axis=1)
# heron's formula
self._surface = _np.sum(1.0 / 4.0 * _np.sqrt(
(a ** 2 + b ** 2 + c ** 2) ** 2 - 2 * (
a ** 4 + b ** 4 + c ** 4)), axis=0)
except (IndexError, ValueError) as Error:
self._surface = 0
return self._surface
def delaunay(x, y):
"""Returns Delaunay instance for x, y points
:param numpy.ndarray x:
:param numpy.ndarray y:
:rtype: Union[None,scipy.spatial.Delaunay]
"""
# Lazy-loading of Delaunay
try:
from scipy.spatial import Delaunay as _Delaunay
except ImportError: # Fallback using local Delaunay
from silx.third_party.scipy_spatial import Delaunay as _Delaunay
points = numpy.array((x, y)).T
try:
delaunay = _Delaunay(points)
except (RuntimeError, ValueError):
_logger.error("Delaunay tesselation failed: %s", sys.exc_info()[1])
delaunay = None
return delaunay
def volume(self):
if self._volume is None:
try:
delau = _Delaunay(self.inside)
# convex null of this area will be used to determine the
# workspace volume and surface area
hull_faces = delau.convex_hull
# get each vertex
a = self.inside[hull_faces[:, 0], :]
b = self.inside[hull_faces[:, 1], :]
c = self.inside[hull_faces[:, 2], :]
d = _np.zeros((3,))
# | (a - d) . ( (b - d) x (c - d) ) |
# -----------------------------------
# 6
self._volume = _np.sum(_np.abs(
_np.sum((a - d) * _np.cross(b - d, c - d, axis=1),
axis=1))) / 6
except (IndexError, ValueError) as Error:
self._volume = 0
return self._volume
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_platform(self,
platform: _robot.Platform,
*args,
**kwargs):
# platform position and orientation
ppos, prot = platform.pose.position
# 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}',
hoverinfo='text',
hovertext=f'platform {pidx}', )
# otherwise, without a platform geometry, we will triangulate the
# anchor points and plot the platform shape via that
else:
# get trimmed platfrom anchors as (K, M) matrix
panchors = self._extract_coordinates(platform.bi.swapaxes(0, 1))
# in 3 dimensions, we will triangulate using Delaunay triangulation,
# in 2 dimensions, we can simply calculate the convex hull. if
# either of these methods fail, the platform anchors are very likely
# to be coplanar or coincident i.e., the platform is 1T, 2T, 3T
# we cannot plot the platform in a 1D coordinate case as it has no
# extend
if self._NUMBER_OF_COORDINATES == 1:
return
if self._NUMBER_OF_COORDINATES == 3:
try:
delau = _Delaunay(panchors.T)
except QhullError as e:
warnings.warn(RuntimeWarning(e))
return
# and get all vertices and points in the correct sorted order
edges = delau.convex_hull
vertices = delau.points
else:
try:
# calculate convex hull of the platform shape
cv = _ConvexHull(panchors.T)
except QhullError as e:
warnings.warn(RuntimeWarning(e))
return
# and get all vertices and points in the correct sorted order
edges = cv.vertices
vertices = cv.points
# to close the loop of vertices, we will append the first one to
# the list
edges = _np.append(edges, edges[0])
# also rotate and translate the platform anchors
prot_ = prot[0:self._NUMBER_OF_COORDINATES,
0:self._NUMBER_OF_COORDINATES]
vertices = ppos[None, 0:self._NUMBER_OF_COORDINATES] + vertices.dot(prot_.T)
# 3D plot
if self._NUMBER_OF_AXES == 3:
# first, plot the mesh of the platform i.e., its volume
self.figure.add_trace(
self._mesh(
**self._prepare_plot_coordinates(
self._extract_coordinates(vertices.T)),
**self._prepare_plot_coordinates(
edges.T,
('i', 'j', 'k')),
color='rgb(0, 0, 0)',
facecolor=['rgb(178, 178, 178)'] *
edges.shape[0],
flatshading=True,
name='',
hoverinfo='skip',
hovertext='',
)
)
# 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:
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
vertices[edge, :].T),
mode='lines',
line=dict(
color='rgb(13, 13, 13)',
),
name='',
hoverinfo='skip',
hovertext='',
showlegend=False
)
)
# 2D plot
else:
self.figure.add_trace(
self._scatter(
**self._prepare_plot_coordinates(
self._extract_coordinates(
vertices[edges, :].T)),
mode='lines',
fill='toself',
line_color='rgb(13, 13, 13)',
fillcolor='rgb(178, 178, 178)',
name='',
hoverinfo='skip',
hovertext='',
showlegend=False
)
)
# 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(ppos,
name=f'platform {pidx}: center',
hoverinfo='text',
hovertext=f'platform {pidx}: center',
line=dict(
dash='dash' if platform.can_rotate
else 'solid',
)
)
# render rotated coordinate system of platform
if platform.can_rotate:
self.render_coordinate_system(ppos,
prot,
name=f'platform {pidx}',
hovertext=f'platform {pidx}',
hoverinfo='text',
line=dict(
dash='solid',
)
)
def _voronoi_edges(self, coordinates):
dt = _Delaunay(coordinates)
edges = _edges_from_simplices(dt.simplices)
edges = (pandas.DataFrame(numpy.asarray(list(edges))).sort_values(
[0, 1]).drop_duplicates().values)
return edges, dt