def cutting_plane(mesh, ry=10, rz=-50): plane = Plane(mesh.centroid(), Vector(1, 0, 0)) Ry = Rotation.from_axis_and_angle(Vector(0, 1, 0), radians(ry), plane.point) Rz = Rotation.from_axis_and_angle(Vector(0, 0, 1), radians(rz), plane.point) plane.transform(Rz * Ry) return plane
def test_from_euler_angles(): ea1 = 1.4, 0.5, 2.3 args = False, 'xyz' R1 = Rotation.from_euler_angles(ea1, *args) ea2 = R1.euler_angles(*args) assert allclose(ea1, ea2) alpha, beta, gamma = ea1 xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = Rotation.from_axis_and_angle(xaxis, alpha) Ry = Rotation.from_axis_and_angle(yaxis, beta) Rz = Rotation.from_axis_and_angle(zaxis, gamma) R2 = Rx * Ry * Rz assert np.allclose(R1, R2)
def draw(network, point, vector, s, step, last_node=None): for i, c in enumerate(s): if c == 'A': point = point + vector * step a = network.add_node(x=point.x, y=point.y, z=point.z) if last_node: network.add_edge(last_node, a) last_node = a elif c == '-': R = Rotation.from_axis_and_angle((0, 0, 1), math.radians(60)) vector.transform(R) elif c == '+': R = Rotation.from_axis_and_angle((0, 0, 1), math.radians(-60)) vector.transform(R)
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 calculate(FILE_IN, plot_flag): for file in FILE_IN: #print("Calculating",file) assembly = Assembly.from_json(file) # ============================================================================== # Identify interfaces # ============================================================================== assembly_interfaces_numpy(assembly, tmax=0.02) # ============================================================================== # Equilibrium # ============================================================================== compute_interface_forces_cvx(assembly, solver='CPLEX', verbose=False) #compute_interface_forces_cvx(assembly, solver='ECOS', verbose=True) # ============================================================================== # Export # ============================================================================== assembly.to_json(output_path(file)) if plot_flag: R = Rotation.from_axis_and_angle([1.0, 0, 0], -pi / 2) assembly.transform(R) plotter = AssemblyPlotter(assembly, figsize=(16, 10), tight=True) plotter.draw_nodes(radius=0.05) plotter.draw_edges() plotter.draw_blocks( facecolor={ key: '#ff0000' for key in assembly.nodes_where({'is_support': True}) }) plotter.show()
def create_group_action(self, objs, transformations, times): # TODO: how to start multiple animations at once if len(transformations) != len(times): raise ValueError("Pass equal amount of transformations and times") x, y, z, w = objs[0].quaternion Tinit = Rotation.from_quaternion([w, x, y, z]) * Translation( objs[0].position) positions = [] quaternions = [] for M in transformations: Sc, Sh, R, T, P = (M * Tinit).decomposed() positions.append(list(T.translation)) quaternions.append(R.quaternion.xyzw) position_track = p3js.VectorKeyframeTrack(name='.position', times=times, values=list( flatten(positions))) rotation_track = p3js.QuaternionKeyframeTrack( name='.quaternion', times=times, values=list(flatten(quaternions))) animation_group = p3js.AnimationObjectGroup() animation_group.exec_three_obj_method('add', objs[0]) # this is not working obj_clip = p3js.AnimationClip(tracks=[position_track, rotation_track]) mixer = p3js.AnimationMixer(animation_group) obj_action = p3js.AnimationAction(mixer, obj_clip, animation_group) return obj_action
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 test_axis_and_angle(): axis1 = normalize_vector([-0.043, -0.254, 0.617]) angle1 = 0.1 R = Rotation.from_axis_and_angle(axis1, angle1) axis2, angle2 = R.axis_and_angle assert allclose(axis1, axis2) assert allclose([angle1], [angle2])
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_concatenated(): trans1 = [1, 2, 3] angle1 = [-2.142, 1.141, -0.142] T1 = Translation.from_vector(trans1) R1 = Rotation.from_euler_angles(angle1) M1 = T1.concatenated(R1) assert allclose(M1, T1 * R1)
def test_basis_vectors(): ea1 = 1.4, 0.5, 2.3 args = False, 'xyz' R1 = Rotation.from_euler_angles(ea1, *args) R2 = [[-0.5847122176808724, -0.18803656702967916, 0.789147560317086], [-0.6544178905170501, -0.4655532858863264, -0.5958165511058404]] assert np.allclose(R1.basis_vectors, R2)
def concatenate(): trans1 = [1, 2, 3] angle1 = [-2.142, 1.141, -0.142] T1 = Translation(trans1) R1 = Rotation.from_euler_angles(angle1) M1 = T1.concatenate(R1) assert np.allclose(M1, T1 * R1)
def blocks(self): """Compute the blocks. Returns ------- list A list of blocks defined as simple meshes. Notes ----- This method is used by the ``from_geometry`` constructor of the assembly data structure to create an assembly "from geometry". """ if self.rise > self.span / 2: raise Exception("Not a semicircular arch.") radius = self.rise / 2 + self.span**2 / (8 * self.rise) # base = [0.0, 0.0, 0.0] top = [0.0, 0.0, self.rise] left = [-self.span / 2, 0.0, 0.0] center = [0.0, 0.0, self.rise - radius] vector = subtract_vectors(left, center) springing = angle_vectors(vector, [-1.0, 0.0, 0.0]) sector = radians(180) - 2 * springing angle = sector / self.n a = top b = add_vectors(top, [0, self.depth, 0]) c = add_vectors(top, [0, self.depth, self.thickness]) d = add_vectors(top, [0, 0, self.thickness]) R = Rotation.from_axis_and_angle([0, 1.0, 0], 0.5 * sector, center) bottom = transform_points([a, b, c, d], R) blocks = [] for i in range(self.n): R = Rotation.from_axis_and_angle([0, 1.0, 0], -angle, center) top = transform_points(bottom, R) vertices = bottom + top faces = [[0, 1, 2, 3], [7, 6, 5, 4], [3, 7, 4, 0], [6, 2, 1, 5], [7, 3, 2, 6], [5, 1, 0, 4]] mesh = Mesh.from_vertices_and_faces(vertices, faces) blocks.append(mesh) bottom = top return blocks
def test_rotation(): angle1 = [-2.142, 1.141, -0.142] R = Rotation.from_euler_angles(angle1) r = [[0.41249169135312663, -0.8335562904208867, -0.3674704277413451, 0.0], [-0.05897071585157175, -0.4269749553355485, 0.9023385407861949, 0.0], [-0.9090506362335324, -0.35053715668381935, -0.22527903264048646, 0.0], [0.0, 0.0, 0.0, 1.0]] assert np.allclose(R, r)
def __iter__(self): yield self.axis alphas = arange(self.max_alpha / self.step, self.max_alpha + self.max_alpha / self.step, self.max_alpha / self.step) radii = [math.sin(alpha) for alpha in alphas] x, y = math.cos(alphas[0]), math.sin(alphas[0]) d = math.sqrt((1 - x)**2 + y**2) # get any vector normal to axis axis2 = Frame.from_plane(Plane((0, 0, 0), self.axis)).xaxis for alpha, r in zip(alphas, radii): R1 = Rotation.from_axis_and_angle(axis2, alpha) amount = int(round(2 * math.pi * r / d)) betas = arange(0, 2 * math.pi, 2 * math.pi / amount) for beta in betas: R2 = Rotation.from_axis_and_angle(self.axis, beta) yield self.axis.transformed(R2 * R1)
def test_from_basis_vectors(): xaxis = [0.68, 0.68, 0.27] yaxis = [-0.67, 0.73, -0.15] R = Rotation.from_basis_vectors(xaxis, yaxis) r = [[0.6807833515407016, -0.6687681611113407, -0.29880282253789103, 0.0], [0.6807833515407016, 0.7282315114847181, -0.07882160714891209, 0.0], [0.2703110366411609, -0.14975954908850603, 0.9510541192112079, 0.0], [0.0, 0.0, 0.0, 1.0]] assert np.allclose(R, r)
def transform(self, transformation): R = Rotation.from_quaternion(self.quaternion) R = transformation * R # Due to a bug on COMPAS 0.10.0 # (Fixed on https://github.com/compas-dev/compas/pull/378 but not released atm) # we work around the retrival of the rotation component of R and instead decompose and get it _, _, r, _, _ = R.decomposed() self.quaternion = r.quaternion
def test___str__(): s = '[[ 0.9345, -0.0798, 0.3469, -0.8157],\n' + \ ' [ -0.1624, 0.7716, 0.6150, -1.2258],\n' + \ ' [ -0.3168, -0.6311, 0.7081, 2.4546],\n' + \ ' [ 0.0000, 0.0000, 0.0000, 1.0000]]\n' trans = [1, 2, 3] axes = [-2.142, 1.141, -0.142] angle = 0.7854 R = Rotation.from_axis_and_angle(axes, angle, point=trans) assert s == str(R)
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 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 calculate_continous_transformation(self, position): """Returns a transformation of a continous joint. A continous joint rotates about the axis and has no upper and lower limits. Args: position (float): the angle in radians """ return Rotation.from_axis_and_angle(self.axis.vector, position, self.origin.point)
def transform(self, transformation): """Transform the volume using a :class:`compas.geometry.Transformation`. Parameters ---------- transformation : :class:`compas.geometry.Transformation` """ R = Rotation.from_quaternion(self.quaternion) R = transformation * R self.quaternion = R.rotation.quaternion
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 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 gluepath_creator(int_surf, path_width): def interval_checker(dimension): underflow = dimension % path_width if underflow > 0.2: no_paths = dimension // path_width + 1 new_path_width = dimension / no_paths return new_path_width else: return path_width wid_gap = int_surf[1] - int_surf[0] wid_vec = Vector(wid_gap[0], wid_gap[1], wid_gap[2]) wid = wid_vec.length wid_vec.unitize() len_gap = int_surf[2] - int_surf[1] len_vec = Vector(len_gap[0], len_gap[1], len_gap[2]) len = len_vec.length len_vec.unitize() wid_path = interval_checker(wid) len_path = interval_checker(len) path_dims = [wid_path, len_path] path_points = [] iteration = 0 path_unfinished = True current_pt = int_surf[0] + scale_vector( wid_vec, wid_path / 2) + scale_vector(len_vec, len_path / 2) current_vec = len_vec.unitized() len_left = len - len_path wid_left = wid - wid_path dims_left = [len_left, wid_left] path_points.append(current_pt) R = Rotation.from_axis_and_angle([0, 0, 1], -math.pi / 2) while path_unfinished: current_index = iteration % 2 current_dim = dims_left[current_index] if iteration > 2: current_dim -= path_dims[current_index] dims_left[current_index] = current_dim if current_dim < path_width * 0.95: break current_pt = current_pt + scale_vector(current_vec, current_dim) path_points.append(current_pt) current_vec.transform(R) current_vec.unitize() iteration += 1 if not is_point_in_polygon_xy(current_pt, int_surf): print("Error: Point not in polygon") return path_points
def __iter__(self): if self.start_vector: # correct start_vector self.start_vector = (self.axis.cross( self.start_vector.unitized())).cross(self.axis) for alpha in arange(0, 2 * math.pi, self.angle_step): R = Rotation.from_axis_and_angle(self.axis, alpha) yield self.start_vector.transformed(R) else: f = Frame.from_plane(Plane((0, 0, 0), self.axis)) for alpha in arange(0, 2 * math.pi, self.angle_step): x = math.cos(alpha) y = math.sin(alpha) yield f.to_world_coordinates(Vector(x, y, 0))
def create_action(self, obj, transformations, times): if len(transformations) != len(times): raise ValueError("Pass equal amount of transformations and times") x, y, z, w = obj.quaternion Tinit = Rotation.from_quaternion([w, x, y, z]) * Translation(obj.position) positions = [] quaternions = [] for M in transformations: Sc, Sh, R, T, P = (M * Tinit).decompose() positions.append(list(T.translation)) quaternions.append(R.quaternion.xyzw) position_track = p3js.VectorKeyframeTrack(name='.position', times=times, values=list(flatten(positions))) rotation_track = p3js.QuaternionKeyframeTrack(name='.quaternion', times=times, values=list(flatten(quaternions))) obj_clip = p3js.AnimationClip(tracks=[position_track, rotation_track]) obj_action = p3js.AnimationAction(p3js.AnimationMixer(obj), obj_clip, obj) return obj_action
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 calculate_continuous_transformation(self, position): """Returns a transformation of a continuous joint. A continuous joint rotates about the axis and has no upper and lower limits. Parameters ---------- position : :obj:`float` Angle in radians Returns ------- :class:`Rotation` Transformation of type rotation for the continuous joint. """ return Rotation.from_axis_and_angle(self.current_axis.vector, position, self.current_origin.point)
def visualize_urscript(script): M = [ [-1000, 0, 0, 0], [0, 1000, 0, 0], [0, 0, 1000, 0], [0, 0, 0, 1], ] rgT = matrix_to_rgtransform(M) cgT = Transformation.from_matrix(M) robot = Robot() viz_planes = [] movel_matcher = re.compile(r"^\s*move([lj]).+((-?\d+\.\d+,?\s?){6}).*$") for line in script.splitlines(): mo = re.search(movel_matcher, line) if mo: if mo.group(1) == "l": # movel ptX, ptY, ptZ, rX, rY, rZ = mo.group(2).split(",") pt = Point(float(ptX), float(ptY), float(ptZ)) pt.transform(cgT) frame = Frame(pt, [1, 0, 0], [0, 1, 0]) R = Rotation.from_axis_angle_vector( [float(rX), float(rY), float(rZ)], pt) T = matrix_to_rgtransform(R) plane = cgframe_to_rgplane(frame) plane.Transform(T) viz_planes.append(plane) else: # movej joint_values = mo.group(2).split(",") configuration = Configuration.from_revolute_values( [float(d) for d in joint_values]) frame = robot.forward_kinematics(configuration) plane = cgframe_to_rgplane(frame) plane.Transform(rgT) viz_planes.append(plane) return viz_planes