def test_scale(): S = Scale.from_factors([1, 2, 3]) assert S.matrix == [[1.0, 0.0, 0.0, 0.0], [0.0, 2.0, 0.0, 0.0], [0.0, 0.0, 3.0, 0.0], [0.0, 0.0, 0.0, 1.0]] S = Scale.from_factors([2.] * 3, Frame((2, 5, 0), (1, 0, 0), (0, 1, 0))) assert S.matrix == [[2.0, 0.0, 0.0, -2.0], [0.0, 2.0, 0.0, -5.0], [0.0, 0.0, 2.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
def append_collision_mesh(self, collision_mesh, scale=False): """Appends a collision mesh that already exists in the planning scene. If the does not exist, it is added. Parameters ---------- collision_mesh : :class:`compas_fab.robots.CollisionMesh` The collision mesh we want to append. scale : bool, optional If `True`, the mesh will be scaled according to the robot's scale factor. Returns ------- None Examples -------- >>> scene = PlanningScene(robot) >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) >>> cm = CollisionMesh(mesh, 'floor') >>> scene.append_collision_mesh(cm) # doctest: +SKIP """ self.ensure_client() collision_mesh.root_name = self.robot.root_name if scale: S = Scale([1. / self.robot.scale_factor] * 3) collision_mesh.scale(S) self.robot.client.append_collision_mesh(collision_mesh)
def test_remeshing(): FILE = os.path.join(HERE, '../..', 'data', 'Bunny.ply') # ============================================================================== # Get the bunny and construct a mesh # ============================================================================== bunny = TriMesh.from_ply(FILE) bunny.cull_vertices() # ============================================================================== # Move the bunny to the origin and rotate it upright. # ============================================================================== vector = scale_vector(bunny.centroid, -1) T = Translation.from_vector(vector) S = Scale.from_factors([100, 100, 100]) R = Rotation.from_axis_and_angle(Vector(1, 0, 0), math.radians(90)) bunny.transform(R * S * T) # ============================================================================== # Remesh # ============================================================================== length = bunny.average_edge_length bunny.remesh(4 * length) bunny.to_mesh()
def add_attached_collision_mesh(self, attached_collision_mesh, scale=False): """Adds an attached collision object to the planning scene. Parameters ---------- attached_collision_mesh : :class:`compas_fab.robots.AttachedCollisionMesh` scale : bool, optional If `True`, the mesh will be scaled according to the robot's scale factor. Returns ------- None Examples -------- >>> scene = PlanningScene(robot) >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) >>> cm = CollisionMesh(mesh, 'tip') >>> ee_link_name = 'ee_link' >>> touch_links = ['wrist_3_link', 'ee_link'] >>> acm = AttachedCollisionMesh(cm, ee_link_name, touch_links) >>> scene.add_attached_collision_mesh(acm) # doctest: +SKIP """ self.ensure_client() if scale: S = Scale([1. / self.robot.scale_factor] * 3) attached_collision_mesh.collision_mesh.scale(S) self.client.add_attached_collision_mesh(attached_collision_mesh)
def vertex_xyz(self): """dict : The view coordinates of the volmesh object.""" origin = Point(0, 0, 0) stack = [] if self.scale != 1.0: S = Scale.from_factors([self.scale] * 3) stack.append(S) if self.rotation != [0, 0, 0]: R = Rotation.from_euler_angles(self.rotation) stack.append(R) if self.location != origin: if self.anchor is not None: xyz = self.volmesh.vertex_attributes(self.anchor, 'xyz') point = Point(*xyz) T1 = Translation.from_vector(origin - point) stack.insert(0, T1) T2 = Translation.from_vector(self.location) stack.append(T2) if stack: X = reduce(mul, stack[::-1]) volmesh = self.volmesh.transformed(X) else: volmesh = self.volmesh vertex_xyz = { vertex: volmesh.vertex_attributes(vertex, 'xyz') for vertex in volmesh.vertices() } return vertex_xyz
def test_sphere_scaling(): s = Sphere(Point(1, 1, 1), 10) s.transform(Scale.from_factors([2, 3, 4])) assert s.point.x == 2 assert s.point.y == 3 assert s.point.z == 4 assert s.radius == 40
def node_xyz(self): """dict : The view coordinates of the mesh object.""" origin = Point(0, 0, 0) stack = [] if self.scale != 1.0: S = Scale.from_factors([self.scale] * 3) stack.append(S) if self.rotation != [0, 0, 0]: R = Rotation.from_euler_angles(self.rotation) stack.append(R) if self.location != origin: if self.anchor is not None: xyz = self.network.vertex_attributes(self.anchor, 'xyz') point = Point(*xyz) T1 = Translation.from_vector(origin - point) stack.insert(0, T1) T2 = Translation.from_vector(self.location) stack.append(T2) if stack: X = reduce(mul, stack[::-1]) network = self.network.transformed(X) else: network = self.network node_xyz = { network: network.node_attributes(node, 'xyz') for node in network.nodes() } return node_xyz
def draw_halfedges( self, halfedges: Optional[List[Tuple[int, int]]] = None, color: Union[str, Color, List[Color], Dict[int, Color]] = (0.7, 0.7, 0.7), distance: float = 0.05, width: float = 0.01, shrink: float = 0.8, ) -> None: """Draw a selection of halfedges. Parameters ---------- edges : list, optional A list of halfedges to draw. The default is ``None``, in which case all halfedges are drawn. color : rgb-tuple or dict of rgb-tuples, optional The color specification for the halfedges. Returns ------- None """ self.clear_halfedges() self._halfedgecollection = [] if color: self.halfedge_color = color if halfedges: self.halfedges = halfedges for u, v in self.halfedges: face = self.mesh.halfedge_face(u, v) if face is None: normal = self.mesh.face_normal(self.mesh.halfedge_face(v, u)) else: normal = self.mesh.face_normal(face) a, b = self.mesh.edge_coordinates(u, v) line = Line(*offset_line((a, b), distance, normal)) frame = Frame(line.midpoint, [1, 0, 0], [0, 1, 0]) scale = Scale.from_factors([shrink, shrink, shrink], frame=frame) line.transform(scale) artist = self.plotter.axes.arrow(line.start[0], line.start[1], line.vector[0], line.vector[1], width=width, head_width=10 * width, head_length=10 * width, length_includes_head=True, shape='right', color=self.halfedge_color.get( (u, v), self.default_halfedgecolor), zorder=10000) self._halfedgecollection.append(artist)
def add_attached_collision_mesh(self, id_name, mesh, group=None, touch_links=[], scale=False): """Attaches a collision mesh to the robot's end-effector. Parameters ---------- id_name (str): The identifier of the object. mesh (:class:`Mesh`): A triangulated COMPAS mesh. group (str, optional): The planning group on which's end-effector the object should be attached. Defaults to the robot's main planning group. touch_links(str list): The list of link names that the attached mesh is allowed to touch by default. The end-effector link name is already considered. """ if not group: group = self.main_group_name # ensure semantics ee_link_name = self.get_end_effector_link_name(group) if scale: S = Scale([1. / self.scale_factor] * 3) mesh = mesh_transformed(mesh, S) if ee_link_name not in touch_links: touch_links.append(ee_link_name) self.client.attached_collision_mesh(id_name, ee_link_name, mesh, 0, touch_links)
def vertex_xyz(self): """dict : The view coordinates of the mesh object.""" origin = Point(0, 0, 0) if self.anchor is not None: xyz = self.mesh.vertex_attributes(self.anchor, 'xyz') point = Point(* xyz) T1 = Translation.from_vector(origin - point) S = Scale.from_factors([self.scale] * 3) R = Rotation.from_euler_angles(self.rotation) T2 = Translation.from_vector(self.location) X = T2 * R * S * T1 else: S = Scale.from_factors([self.scale] * 3) R = Rotation.from_euler_angles(self.rotation) T = Translation.from_vector(self.location) X = T * R * S mesh = self.mesh.transformed(X) vertex_xyz = {vertex: mesh.vertex_attributes(vertex, 'xy') + [0.0] for vertex in mesh.vertices()} return vertex_xyz
def scale(self, factor): from compas.geometry import Scale S = Scale([factor, factor, factor]) for item in self.visual: item.geometry.shape.transform(S) for item in self.collision: item.geometry.shape.transform(S) for joint in self.joints: joint.scale(factor) joint.child_link.scale(factor)
def scale(self, scale_factor): """Scale the volume uniformly. Parameters ---------- scale_factor : :obj:`float` Scale factor to use in scaling operation. """ S = Scale.from_factors([scale_factor] * 3) self.transform(S)
def scale(self, scale_factor): """Scales the collision mesh uniformly. Parameters ---------- scale_factor : :obj:`float` Scale factor. """ S = Scale([scale_factor] * 3) self.mesh.transform(S)
def test_basis_vectors(): trans1 = [1, 2, 3] angle1 = [-2.142, 1.141, -0.142] scale1 = [0.123, 2, 0.5] T1 = Translation(trans1) R1 = Rotation.from_euler_angles(angle1) S1 = Scale(scale1) M = (T1 * R1) * S1 x, y = M.basis_vectors assert np.allclose(x, Vector(0.41249169135312663, -0.05897071585157175, -0.9090506362335324)) assert np.allclose(y, Vector(-0.8335562904208867, -0.4269749553355485, -0.35053715668381935))
def test_decomposed(): trans1 = [1, 2, 3] angle1 = [-2.142, 1.141, -0.142] scale1 = [0.123, 2, 0.5] T1 = Translation(trans1) R1 = Rotation.from_euler_angles(angle1) S1 = Scale(scale1) M = (T1 * R1) * S1 Sc, Sh, R, T, P = M.decomposed() assert S1 == Sc assert R1 == R assert T1 == T
def attach_collision_mesh_to_robot_end_effector(self, collision_mesh, scale=False, group=None): """Attaches a collision mesh to the robot's end-effector. Parameters ---------- collision_mesh: :class:`compas_fab.robots.CollisionMesh` The collision mesh. scale : bool, optional If `True`, the mesh will be scaled according to the robot's scale factor. group : str The planning group to which we want to attach the mesh to. Defaults to the robot's main planning group. Returns ------- None Examples -------- >>> scene = PlanningScene(robot) >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) >>> cm = CollisionMesh(mesh, 'tip') >>> group = robot.main_group_name >>> scene.attach_collision_mesh_to_robot_end_effector(cm, group=group) >>> # wait for it to be attached >>> time.sleep(1) >>> # wait for it to be removed >>> scene.remove_attached_collision_mesh('tip') >>> time.sleep(1) >>> # check if it's really gone >>> planning_scene = robot.client.get_planning_scene() >>> objects = [c.object['id'] for c in planning_scene.robot_state.attached_collision_objects] >>> 'tip' in objects False """ self.ensure_client() if scale: S = Scale([1. / self.robot.scale_factor] * 3) collision_mesh.scale(S) ee_link_name = self.robot.get_end_effector_link_name(group) touch_links = [ee_link_name] acm = AttachedCollisionMesh(collision_mesh, ee_link_name, touch_links) self.add_attached_collision_mesh(acm)
def node_xyz(self): """dict : The view coordinates of the mesh object.""" origin = Point(0, 0, 0) if self.anchor is not None: xyz = self.network.node_attributes(self.anchor, 'xyz') point = Point(*xyz) T1 = Translation.from_vector(origin - point) S = Scale.from_factors([self.scale] * 3) R = Rotation.from_euler_angles(self.rotation) T2 = Translation.from_vector(self.location) X = T2 * R * S * T1 else: S = Scale.from_factors([self.scale] * 3) R = Rotation.from_euler_angles(self.rotation) T = Translation.from_vector(self.location) X = T * R * S network = self.network.transformed(X) node_xyz = { network: network.node_attributes(node, 'xyz') for node in network.nodes() } return node_xyz
def scale(self, factor): """Scales the robot geometry by factor (absolute). Parameters ---------- factor : float The factor to scale the robot with. """ relative_factor = factor / self.scale_factor # relative scaling factor self.scale_factor = factor transformation = Scale( [relative_factor, relative_factor, relative_factor]) self.scale_links(transformation)
def decomposed(self): """Decompose the `Transformation` into its components. Returns ------- :class:`compas.geometry.Scale` The scale component of the current transformation. :class:`compas.geometry.Shear` The shear component of the current transformation. :class:`compas.geometry.Rotation` The rotation component of the current transformation. :class:`compas.geometry.Translation` The translation component of the current transformation. :class:`compas.geometry.Projection` The projection component of the current transformation. Examples -------- >>> from compas.geometry import Scale, Translation, Rotation >>> trans1 = [1, 2, 3] >>> angle1 = [-2.142, 1.141, -0.142] >>> scale1 = [0.123, 2, 0.5] >>> T1 = Translation.from_vector(trans1) >>> R1 = Rotation.from_euler_angles(angle1) >>> S1 = Scale.from_factors(scale1) >>> M = T1 * R1 * S1 >>> S, H, R, T, P = M.decomposed() >>> S1 == S True >>> R1 == R True >>> T1 == T True """ from compas.geometry import Scale # noqa: F811 from compas.geometry import Shear from compas.geometry import Rotation # noqa: F811 from compas.geometry import Translation # noqa: F811 from compas.geometry import Projection s, h, a, t, p = decompose_matrix(self.matrix) S = Scale.from_factors(s) H = Shear.from_entries(h) R = Rotation.from_euler_angles(a, static=True, axes='xyz') T = Translation.from_vector(t) P = Projection.from_entries(p) return S, H, R, T, P
def scale(self, factor): """Scales the robot geometry by factor (absolute). Parameters ---------- factor : float The factor to scale the robot with. Returns ------- None """ self.robot.scale(factor) # scale the model relative_factor = factor / self.scale_factor # relative scaling factor transformation = Scale([relative_factor] * 3) self.scale_link(self.robot.root, transformation) self.scale_factor = factor
def scale(self, factor): """Scales the robot model's geometry by factor (absolute). Parameters ---------- factor : float The factor to scale the robot with. Returns ------- None """ self.model.scale(factor) relative_factor = factor / self.scale_factor transformation = Scale.from_factors([relative_factor] * 3) self.scale_link(self.model.root, transformation) self.scale_factor = factor
def add_collision_mesh_to_planning_scene(self, id_name, mesh, scale=False): """Adds a collision mesh to the robot's planning scene. Parameters ---------- id_name (str): The identifier of the collision mesh. mesh (:class:`Mesh`): A triangulated COMPAS mesh. Examples -------- """ self.ensure_client() root_link_name = self.model.root.name if scale: S = Scale([1. / self.scale_factor] * 3) mesh = mesh_transformed(mesh, S) self.client.collision_mesh(id_name, root_link_name, mesh, 0)
def create_collision_mesh_attached_to_end_effector(self, id_name, mesh, group=None, scale=False): """Creates a collision object that is added to the end effector's tcp. """ if not group: group = self.main_group_name # ensure semantics ee_link_name = self.get_end_effector_link_name(group) if scale: S = Scale([1. / self.scale_factor] * 3) mesh = mesh_transformed(mesh, S) last_link_with_geometry = self.get_links_with_geometry(group)[-1] touch_links = [last_link_with_geometry.name] return self.client.build_attached_collision_mesh( ee_link_name, id_name, mesh, operation=0, touch_links=touch_links)
def decomposed(self): """Decompose the ``Transformation`` into its ``Scale``, ``Shear``, ``Rotation``, ``Translation`` and ``Perspective`` components. Returns ------- 5-tuple of Transformation The scale, shear, rotation, tranlation, and projection components of the current transformation. Examples -------- >>> trans1 = [1, 2, 3] >>> angle1 = [-2.142, 1.141, -0.142] >>> scale1 = [0.123, 2, 0.5] >>> T1 = Translation(trans1) >>> R1 = Rotation.from_euler_angles(angle1) >>> S1 = Scale(scale1) >>> M = (T1 * R1) * S1 >>> Sc, Sh, R, T, P = M.decomposed() >>> S1 == Sc True >>> R1 == R True >>> T1 == T True """ from compas.geometry import Scale # noqa: F811 from compas.geometry import Shear from compas.geometry import Rotation # noqa: F811 from compas.geometry import Translation # noqa: F811 from compas.geometry import Projection sc, sh, a, t, p = decompose_matrix(self.matrix) Sc = Scale(sc) Sh = Shear.from_entries(sh) R = Rotation.from_euler_angles(a, static=True, axes='xyz') T = Translation(t) P = Projection.from_entries(p) return Sc, Sh, R, T, P
import math import compas_libigl as igl from compas.datastructures import Mesh from compas.utilities import Colormap, rgb_to_hex from compas.geometry import Line, Polyline, Rotation, Scale from compas_viewers.objectviewer import ObjectViewer # ============================================================================== # Input geometry # ============================================================================== mesh = Mesh.from_off(igl.get_beetle()) Rx = Rotation.from_axis_and_angle([1, 0, 0], math.radians(90)) Rz = Rotation.from_axis_and_angle([0, 0, 1], math.radians(90)) S = Scale.from_factors([10, 10, 10]) mesh.transform(S * Rz * Rx) # ============================================================================== # Isolines # ============================================================================== scalars = mesh.vertices_attribute('z') vertices, edges = igl.trimesh_isolines(mesh.to_vertices_and_faces(), scalars, 10) isolines = igl.groupsort_isolines(vertices, edges) # ============================================================================== # Visualisation # ==============================================================================
from compas.geometry import Translation from compas.geometry import Scale from compas.geometry import Reflection from compas.geometry import Projection from compas.geometry import Shear axis, angle = [0.2, 0.4, 0.1], 0.3 R = Rotation.from_axis_and_angle(axis, angle) print("Rotation:\n", R) translation_vector = [5, 3, 1] T = Translation(translation_vector) print("Translation:\n", T) scale_factors = [0.1, 0.3, 0.4] S = Scale(scale_factors) print("Scale:\n", S) point, normal = [0.3, 0.2, 1], [0.3, 0.1, 1] R = Reflection(point, normal) print("Reflection:\n", R) point, normal = [0, 0, 0], [0, 0, 1] perspective = [1, 1, 0] P = Projection.perspective(point, normal, perspective) print("Perspective projection:\n", R) angle, direction = 0.1, [0.1, 0.2, 0.3] point, normal = [4, 3, 1], [-0.11, 0.31, -0.17] S = Shear(angle, direction, point, normal) print("Shear:\n", S)
from compas.datastructures import Mesh from compas.geometry import Translation, Scale, Point tetra = Mesh.from_polyhedron(4) hexa = Mesh.from_polyhedron(6) octa = Mesh.from_polyhedron(8) dodeca = Mesh.from_polyhedron(12) icosa = Mesh.from_polyhedron(20) o = Point(0, 0, 0) T = Translation.from_vector([2.5, 0, 0]) p = Point(* tetra.vertex_coordinates(tetra.get_any_vertex())) s = 1 / (p - o).length S = Scale.from_factors([s, s, s]) tetra.transform(S) tetra.dual() p = Point(* hexa.vertex_coordinates(hexa.get_any_vertex())) s = 1 / (p - o).length S = Scale.from_factors([s, s, s]) hexa.transform(T * S) hexa.dual() p = Point(* octa.vertex_coordinates(octa.get_any_vertex())) s = 1 / (p - o).length S = Scale.from_factors([s, s, s])
"""Example: pre- vs. post- multiplication """ import math from compas.geometry import Translation from compas.geometry import Rotation from compas.geometry import Scale R = Rotation.from_axis_and_angle([0, 0, 1], math.radians(30)) T = Translation([2, 0, 0]) S = Scale([0.5] * 3) X1 = S * T * R X2 = R * T * S print(X1) print(X2) print(X1 == X2) # should not be the same!
cylinder = Cylinder(circle, 0.7 * height) circle = [[0, 0, 0.7 * height], [0, 0, 1]], 0.1 cone = Cone(circle, 0.3 * height) for node in network.nodes(): a = network.node_attributes(node, 'xyz') for nbr in network.neighbors(node): edge = node, nbr if not network.has_edge(*edge): edge = nbr, node b = network.node_attributes(nbr, 'xyz') force = network.edge_attribute(edge, 'f') direction = normalize_vector(subtract_vectors(b, a)) frame = Frame.from_plane([a, direction]) X = Transformation.from_frame_to_frame(world, frame) S = Scale.from_factors([force, 1, 1]) X = X * S shaft = cylinder.transformed(X) tip = cone.transformed(X) artist = CylinderArtist(shaft, layer=layer, color=(255, 0, 0)) artist.draw(u=16) artist = ConeArtist(tip, layer=layer, color=(255, 0, 0)) artist.draw(u=16)
from compas.datastructures import Mesh from compas.geometry import Point, Box from compas.geometry import Translation, Scale from compas_view2 import app box = Box.from_diagonal([(0.0, 0.0, 0.0), (1.0, 1.0, 1.0)]) mesh = Mesh.from_shape(box) trimesh = Mesh.from_polyhedron(4) tribox = Box.from_bounding_box(trimesh.bounding_box()) S = tribox.width / box.width trimesh.transform(Scale.from_factors([S, S, S])) trimesh.transform( Translation.from_vector(Point(0.5, 0.5, 0.5) - Point(0, 0, 0))) tri = mesh.subdivide(k=3, scheme='tri') quad = mesh.subdivide(k=3, scheme='quad') corner = mesh.subdivide(k=3, scheme='corner') ck = mesh.subdivide(k=3, scheme='catmullclark') doosabin = mesh.subdivide(k=3, scheme='doosabin') frames = mesh.subdivide(offset=0.2, scheme='frames') loop = trimesh.subdivide(k=3, scheme='loop') corner.transform(Translation.from_vector([1.2 * 1, 0.0, 0.0])) loop.transform(Translation.from_vector([1.2 * 2, 0.0, 0.0])) quad.transform(Translation.from_vector([1.2 * 3, 0.0, 0.0])) ck.transform(Translation.from_vector([1.2 * 4, 0.0, 0.0])) doosabin.transform(Translation.from_vector([1.2 * 5, 0.0, 0.0])) frames.transform(Translation.from_vector([1.2 * 6, 0.0, 0.0]))