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) ) )