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