def set_base(self, base_frame): # move to UR !!!! ??? self.base_frame = base_frame # transformation matrix from world coordinate system to robot coordinate system self.transformation_WCS_RCS = Transformation.from_frame_to_frame( Frame.worldXY(), self.base_frame) # transformation matrix from robot coordinate system to world coordinate system self.transformation_RCS_WCS = Transformation.from_frame_to_frame( self.base_frame, Frame.worldXY())
def __init__(self, tcp_frame): self.model = [] #mesh self.model_breps = [] #breps self.tool0_frame = Frame.worldXY() self.tcp_frame = tcp_frame self.transformation_tool0_tcp = Transformation.from_frame_to_frame( self.tcp_frame, self.tool0_frame) self.transformation_tcp_tool0 = Transformation.from_frame_to_frame( self.tool0_frame, self.tcp_frame)
def to_global_geometry(self, world_frame=Frame.worldXY()): geometry = self.to_local_geometry_xy() transformed_geometry = [] gusset_local_frame = Frame([0, 0, 0], [1, 0, 0], [0, 1, 0]) T1 = Transformation.from_frame_to_frame(gusset_local_frame, self.frame) T2 = Transformation.from_frame_to_frame(self.frame, world_frame) transformed_part = [] # default for part in geometry: transformed_part = [] for point in part: p_point = Point(point['x'], point['y'], point['z']) transformed_part.append(p_point.transformed(T1)) transformed_geometry.append(transformed_part) return transformed_geometry
def get_next_frame(self, place_cylinder): """Get next frame to pick cylinder at. Parameters ---------- cylinder : :class:`compas_rcf.fab_data.Claycylinder` cylinder to place Returns ------- :class:`compas.geometry.Frame` """ idx = self.counter % self.n_pick_frames self.counter += 1 log.debug("Counter at: {}, Frame index at {}".format( self.counter, idx)) pick_location = self.pick_frames[idx] T = Transformation.from_frame_to_frame(place_cylinder.location, pick_location) # Copy place_cylinder to get same height properties pick_cylinder = place_cylinder.copy() pick_cylinder.location.transform(T) return pick_cylinder
def get_next_pick_elem(self, place_element): """Get next pick element. Parameters ---------- place_element : :class:`rapid_clay_formations_fab.fab_data.FabricationElement` Element to place. Returns ------- :class:`rapid_clay_formations_fab.fab_data.FabricationElement` """ idx = self.counter % self.n_pick_frames self.counter += 1 log.debug("Counter at: {}, Frame index at {}".format(self.counter, idx)) pick_location = self.pick_frames[idx] T = Transformation.from_frame_to_frame(place_element.location, pick_location) # Copy place_cylinder to get same height properties pick_cylinder = place_element.copy() pick_cylinder.location.transform(T) return pick_cylinder
def attach_tool_model(self, tool_model): """Attach a tool to the robot artist for visualization. Parameters ---------- tool_model : :class:`compas.robots.ToolModel` The tool that should be attached to the robot's flange. """ self.attached_tool_model = tool_model self.create(tool_model.root, 'attached_tool') if not tool_model.link_name: link = self.model.get_end_effector_link() tool_model.link_name = link.name else: link = self.model.get_link_by_name(tool_model.link_name) ee_frame = link.parent_joint.origin.copy() initial_transformation = Transformation.from_frame_to_frame(Frame.worldXY(), ee_frame) sample_geometry = link.collision[0] if link.collision else link.visual[0] if link.visual else None if hasattr(sample_geometry, 'current_transformation'): relative_transformation = sample_geometry.current_transformation else: relative_transformation = Transformation() transformation = relative_transformation.concatenated(initial_transformation) self.update_tool(transformation=transformation) tool_model.parent_joint_name = link.parent_joint.name
def icp_numpy(source, target, tol=1e-3): """Align two point clouds using the Iterative Closest Point (ICP) method. Parameters ---------- source : list of point The source data. target : list of point The target data. tol : float, optional Tolerance for finding matches. Default is ``1e-3``. Returns ------- The transformed points Notes ----- First we align the source with the target cloud using the frames resulting from a PCA of each of the clouds, simply by calculating a frame-to-frame transformation. This initial alignment is used to establish an initial correspondence between the points of the two clouds. Then we iteratively improve the alignment by computing successive "best-fit" transformations using SVD of the cross-covariance matrix of the two data sets. During this iterative process, we continuously update the correspondence between the point clouds by finding the closest point in the target to each of the source points. The algorithm terminates when the alignment error is below a specified tolerance. Examples -------- >>> """ A = asarray(source) B = asarray(target) origin, axes, _ = pca_numpy(A) A_frame = Frame(origin, axes[0], axes[1]) origin, axes, _ = pca_numpy(B) B_frame = Frame(origin, axes[0], axes[1]) X = Transformation.from_frame_to_frame(A_frame, B_frame) A = transform_points_numpy(A, X) for i in range(20): D = cdist(A, B, 'euclidean') closest = argmin(D, axis=1) if norm(normrow(A - B[closest])) < tol: break X = bestfit_transform(A, B[closest]) A = transform_points_numpy(A, X) return A, X
def update(self, joint_state, visual=True, collision=True): """Triggers the update of the robot geometry. Parameters ---------- joint_state : :class:`compas.robots.Configuration` | dict[str, float] A dictionary with joint names as keys and joint positions as values. visual : bool, optional If True, the visual geometry will also be updated. collision : bool, optional If True, the collision geometry will also be updated. Returns ------- None """ _ = self._update(self.model, joint_state, visual, collision) if self.attached_tool_model: frame = self.model.forward_kinematics( joint_state, link_name=self.attached_tool_model.link_name) self.update_tool(visual=visual, collision=collision, transformation=Transformation.from_frame_to_frame( Frame.worldXY(), frame))
def from_t0cf_to_tcf(self, frames_t0cf): """Converts frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame). Parameters ---------- frames_t0cf : list[:class:`compas.geometry.Frame`] Frames (in WCF) at the robot's flange (tool0). Returns ------- list[:class:`compas.geometry.Frame`] Frames (in WCF) at the robot's tool tip (tcf). Examples -------- >>> import compas >>> from compas.datastructures import Mesh >>> from compas.geometry import Frame >>> mesh = Mesh.from_stl(compas.get('cone.stl')) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = ToolModel(mesh, frame) >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] >>> tool.from_t0cf_to_tcf(frames_t0cf) [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] """ Te = Transformation.from_frame_to_frame(Frame.worldXY(), self.frame) return [ Frame.from_transformation(Transformation.from_frame(f) * Te) for f in frames_t0cf ]
def three_pts_localization( rcs_coords, wcs_coords): # type: (List[Point], List[Point]) -> Frame """Get the robot base frame in WCS using three points method. Parameters ---------- rcs_coords List of the RCS coordinates used for measurements. wcs_coords List of the WCS coordinates used for measurements. Returns ------- The base frame of the robot in WCS. """ recorded_frame_rcs = _coerce_frame(rcs_coords) recorded_frame_wcs = _coerce_frame(wcs_coords) T = Transformation.from_frame_to_frame(recorded_frame_rcs, recorded_frame_wcs) wcs_robot_base = Frame.worldXY() wcs_robot_base.transform(T) return wcs_robot_base
def offset_bbox_xy(pts, dist): bbox = pca_numpy(pts) frame1 = Frame(bbox[0], bbox[1][0], bbox[1][1]) xform = Transformation.from_frame_to_frame(frame1, Frame.worldXY()) pts = transform_points(pts, xform) bbox = bounding_box_xy(pts) bbox = offset_polygon(bbox, dist) return bbox, xform
def RunScript(self, robot, group, configuration, attached_collision_meshes, show_visual, show_collision, show_frames, show_base_frame, show_end_effector_frame, show_acm): visual = None collision = None attached_meshes = None frames = None base_frame = None ee_frame = None if robot: show_visual = True if show_visual is None else show_visual configuration = configuration or robot.zero_configuration() robot.update(configuration, visual=show_visual, collision=show_collision) compas_frames = robot.transformed_frames(configuration, group) if show_visual: visual = robot.artist.draw_visual() if show_collision: collision = robot.artist.draw_collision() if show_base_frame: base_compas_frame = compas_frames[0] artist = FrameArtist(base_compas_frame) base_frame = artist.draw() if show_end_effector_frame: ee_compas_frame = robot.forward_kinematics( configuration, group, options=dict(solver='model')) artist = FrameArtist(ee_compas_frame) ee_frame = artist.draw() if show_frames: frames = [] for compas_frame in compas_frames[1:]: artist = FrameArtist(compas_frame) frame = artist.draw() frames.append(frame) if show_acm: attached_meshes = [] for acm in attached_collision_meshes: frame = robot.forward_kinematics(configuration, options=dict( solver='model', link=acm.link_name)) T = Transformation.from_frame_to_frame( acm.collision_mesh.frame, frame) mesh = acm.collision_mesh.mesh.transformed(T) attached_meshes.append(MeshArtist(mesh).draw()) return (visual, collision, attached_meshes, frames, base_frame, ee_frame)
def oabb_numpy(points): origin, (xaxis, yaxis, zaxis), values = pca_numpy(points) frame = Frame(origin, xaxis, yaxis) world = Frame.worldXY() X = Transformation.from_frame_to_frame(frame, world) points = transform_points_numpy(points, X) bbox = bounding_box(points) bbox = transform_points_numpy(bbox, X.inverse()) return bbox
def to_global_geometry(self, world_frame=Frame.worldXY()): geometry = self.to_local_geometry_xy() transformed_geometry = [] T = Transformation.from_frame_to_frame(self.frame, world_frame) for part in geometry: transformed_part = [] for point in part: p_point = Point(point['x'], point['y'], point['z']) transformed_part.append(p_point.transformed(T)) transformed_geometry.append(transformed_part) return transformed_geometry
def offset_polygon_xy(points, dist, planarize=False): if len(points) < 3: return None frame = Frame.from_plane( Plane.from_three_points(points[0], points[1], points[2])) xform = Transformation.from_frame_to_frame(frame, Frame.worldXY()) if planarize: points = [point_on_plane(point, frame) for point in points] points = transform_points(points, xform) points = offset_polygon(points, dist) points = transform_points(points, xform.inverse()) return points
def geometry(self): # TODO: this is a temp solution # TODO: add memoization or some other kind of caching A = Mesh.from_shape(self.shape) for shape, operation in self.features: A.quads_to_triangles() B = Mesh.from_shape(shape) B.quads_to_triangles() A = Part.operations[operation](A.to_vertices_and_faces(), B.to_vertices_and_faces()) geometry = Shape(*A) T = Transformation.from_frame_to_frame(Frame.worldXY(), self.frame) geometry.transform(T) return geometry
def transformation_WCF_RCF(self, group=None): """Returns the transformation from the world coordinate system (WCF) to the robot's coordinate system (RCF). Parameters ---------- group : str The name of the planning group. Defaults to `None`. Returns ------- :class:`compas.geometry.Transformation` """ base_frame = self.get_base_frame(group) return Transformation.from_frame_to_frame(base_frame, Frame.worldXY())
def update(self, joint_state, visual=True, collision=True): """Triggers the update of the robot geometry. Parameters ---------- joint_state : :obj:`dict` or :class:`compas.robots.Configuration` A dictionary with joint names as keys and joint positions as values. visual : bool, optional ``True`` if the visual geometry should be also updated, otherwise ``False``. Defaults to ``True``. collision : bool, optional ``True`` if the collision geometry should be also updated, otherwise ``False``. Defaults to ``True``. """ _ = self._update(self.model, joint_state, visual, collision) if self.attached_tool_model: frame = self.model.forward_kinematics(joint_state, link_name=self.attached_tool_model.link_name) self.update_tool(visual=visual, collision=collision, transformation=Transformation.from_frame_to_frame(Frame.worldXY(), frame))
def represent_frame_in_transformed_RCF(self, frame_WCF, configuration, group=None): """Returns the frame's representation the current robot's coordinate frame (RCF). Parameters ---------- frame_WCF (:class:`Frame`): A frame in the world coordinate frame. configuration (:class:`Configuration`): The (full) configuration from which the group's base frame is calculated. group (str, optional): The planning group. Defaults to the robot's main planning group. Examples -------- """ self.ensure_client() if not group: group = self.main_group_name # ensure semantics base_link = self.get_base_link_name(group) joint_names = self.get_configurable_joint_names() if len(joint_names) != len(configuration.values): raise ValueError("Please pass a configuration with %d values" % len(joint_names)) joint_positions = configuration.values joint_positions = self.scale_joint_values(joint_positions, 1. / self.scale_factor) response = self.client.forward_kinematics(joint_positions, base_link, group, joint_names, base_link) base_frame_RCF = response.pose_stamped[ 0].pose.frame # the group's transformed base_frame in RCF base_frame = self.get_base_frame( group) # the group's original base_frame T = Transformation.from_frame(base_frame) base_frame = base_frame_RCF.transformed(T) # the current base frame T = Transformation.from_frame_to_frame(base_frame, Frame.worldXY()) return frame_WCF.transformed(T)
def icp_numpy(d1, d2, tol=1e-3): """Align two point clouds using the Iterative Closest Point (ICP) method. Parameters ---------- d1 : list of point Point cloud 1. d2 : list of point Point cloud 2. tol : float, optional Tolerance for finding matches. Default is ``1e-3``. Returns ------- Notes ----- Examples -------- References ---------- """ d1 = asarray(d1) d2 = asarray(d2) point, axes, spread = pca_numpy(d1) frame1 = Frame(point, axes[0], axes[1]) point, axes, spread = pca_numpy(d2) frame2 = Frame(point, axes[0], axes[1]) T = Transformation.from_frame_to_frame(frame1, frame2) transform_points_numpy(d1, T) y = cdist(d1, d2, 'eucledian') closest = argmin(y, axes=1)
def attach_tool(self, tool): """Attach a tool to the robot artist. Parameters ---------- tool : :class:`compas_fab.robots.Tool` The tool that should be attached to the robot's flange. """ name = '{}.visual.attached_tool'.format(self.robot.name) native_geometry = self.draw_geometry( tool.visual, name=name) # TODO: only visual, collision would be great link = self.robot.get_link_by_name( tool.attached_collision_mesh.link_name) ee_frame = link.parent_joint.origin.copy() T = Transformation.from_frame_to_frame(Frame.worldXY(), ee_frame) self.transform(native_geometry, T) tool.native_geometry = [native_geometry] tool.current_transformation = Transformation() self.attached_tool = tool
def move_mesh_to_point(mesh, target_point): """Moves (translates) a mesh to a target point. Parameters ---------- mesh: :class:`compas.datastructures.Mesh` A compas mesh. target_point: :class:`compas.geometry.Point` The point to move the mesh to. """ mesh_center_pt = get_mid_pt_base(mesh) # transform mesh mesh_frame = Frame(mesh_center_pt, (1, 0, 0), (0, 1, 0)) target_frame = Frame(target_point, (1, 0, 0), (0, 1, 0)) T = Transformation.from_frame_to_frame(mesh_frame, target_frame) mesh.transform(T) logger.info("Mesh moved to: " + str(target_point)) return mesh
def oabb_numpy(points): """Oriented bounding box of a set of points. Parameters ---------- points : array_like[point] XYZ coordinates of the points. Returns ------- list[[float, float, float]] XYZ coordinates of 8 points defining a box. """ origin, (xaxis, yaxis, zaxis), values = pca_numpy(points) frame = Frame(origin, xaxis, yaxis) world = Frame.worldXY() X = Transformation.from_frame_to_frame(frame, world) points = transform_points_numpy(points, X) bbox = bounding_box(points) bbox = transform_points_numpy(bbox, X.inverse()) return bbox
def _get_current_transformation_WCF_RCF(self, full_configuration, group): """Returns the group's current WCF to RCF transformation, if the robot is in full_configuration. The base_frame of a planning group can change if a parent joint was transformed. This function performs a forward kinematic request with the full configuration to retrieve the (possibly) transformed base_frame of planning group. This function is only used in plan_motion since other services, such as ik or plan_cartesian_motion, do not use the transformed base_frame as the group's local coordinate system. Parameters ---------- full_configuration : :class:`compas_fab.robots.Configuration` The (full) configuration from which the group's base frame is calculated. group : str The planning group for which we want to get the transformed base frame. Returns ------- :class:`compas.geometry.Transformation` """ base_frame = self._get_current_base_frame(full_configuration, group) return Transformation.from_frame_to_frame(base_frame, Frame.worldXY())
world = Frame.worldXY() for guid in guids: surface = RhinoSurface.from_guid(guid) block = surface.to_compas() block.name = str(guid) bottom = sorted( block.faces(), key=lambda face: dot_vectors(block.face_normal(face), [0, 0, -1]))[-1] plane = block.face_centroid(bottom), block.face_normal(bottom) frame = Frame.from_plane(plane) T = Transformation.from_frame_to_frame(frame, world) block.transform(T) blank = Box.from_bounding_box(block.bounding_box()) blank.xsize += 25 blank.ysize += 25 blank.zsize += 25 block.attributes['blank'] = blank block.attributes['bottom'] = bottom blocks.append(block) # ============================================================================== # Export
def test_from_frame(): f1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26]) f2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) T = Transformation.from_frame_to_frame(f1, f2) f1.transform(T) assert f1 == f2
def test_json_xform(): before = Transformation.from_frame_to_frame(Frame.worldXY(), Frame.worldXY()) after = compas.json_loads(compas.json_dumps(before)) assert before.dtype == after.dtype assert all(a == b for a, b in zip(before, after))
def unroll(zone): unrolled = [] for quadmesh, trimesh in zone: flatmesh = trimesh.copy() fkeys = list( trimesh.faces_where_predicate(partial(endswith_NAME, '-00'))) for fkey in fkeys: nbrs = trimesh.face_neighbors(fkey) if len(nbrs) == 1: root = fkey break root = None for key in trimesh.face_vertices(root): if trimesh.vertex_degree(key) == 2: corner = key break corner = None origin = trimesh.vertex_coordinates(corner) zaxis = trimesh.face_normal(root, unitized=True) xaxis = trimesh.edge_direction( corner, trimesh.face_vertex_descendant(root, corner)) yaxis = normalize_vector(cross_vectors(zaxis, xaxis)) frame = Frame(origin, xaxis, yaxis) frame_to = Frame.worldXY() T = Transformation.from_frame_to_frame(frame, frame_to) for key in trimesh.face_vertices(root): x, y, z = trimesh.vertex_coordinates(key) point = Point(x, y, z) point.transform(T) flatmesh.set_vertex_attributes(key, 'xyz', point) tovisit = deque([root]) visited = set([root]) while tovisit: fkey = tovisit.popleft() for u, v in trimesh.face_halfedges(fkey): nbr = trimesh.halfedge[v][u] if nbr is None: continue if nbr in visited: continue tovisit.append(nbr) visited.add(nbr) origin = trimesh.vertex_coordinates(v) zaxis = trimesh.face_normal(nbr, unitized=True) xaxis = trimesh.edge_direction(v, u) yaxis = normalize_vector(cross_vectors(xaxis, zaxis)) frame = Frame(origin, xaxis, yaxis) origin = flatmesh.vertex_coordinates(v) zaxis = [0, 0, 1] xaxis = flatmesh.edge_direction(v, u) yaxis = normalize_vector(cross_vectors(xaxis, zaxis)) frame_to = Frame(origin, xaxis, yaxis) T = Transformation.from_frame_to_frame(frame, frame_to) w = trimesh.face_vertex_ancestor(nbr, v) x, y, z = trimesh.vertex_coordinates(w) point = Point(x, y, z) point.transform(T) flatmesh.set_vertex_attributes(w, 'xyz', point) for key, attr in quadmesh.vertices(True): x, y, z = flatmesh.vertex_coordinates(key) attr['x'] = x attr['y'] = y attr['z'] = z unrolled.append(quadmesh) return unrolled
from compas.geometry import Transformation from compas.geometry import Box from compas_rhino.artists import BoxArtist from compas_rhino.artists import MeshArtist from compas.datastructures import Mesh # Box in the world coordinate system frame = Frame([1, 0, 0], [-0.45, 0.1, 0.3], [1, 0, 0]) width, length, height = 1, 1, 1 box = Box(frame, width, length, height) # Frame F representing a coordinate system F = Frame([2, 2, 2], [0.978, 0.010, -0.210], [0.090, 0.882, 0.463]) # Get transformation between frames and apply transformation on box. T = Transformation.from_frame_to_frame(Frame.worldXY(), F) box_transformed = box.transformed(T) # make Mesh from box mesh_transformed = Mesh.from_shape(box_transformed) # create Projection P = Projection.from_plane_and_direction(Plane((0, 0, 0), (0, 0, 1)), (3, 5, 5)) P = Projection.from_plane(Plane((0, 0, 0), (0, 0, 1))) P = Projection.from_plane_and_point(Plane((0, 0, 0), (0, 0, 1)), (3, 5, 5)) # apply transformation on mesh mesh_projected = mesh_transformed.transformed(P) # create artists artist1 = BoxArtist(box_transformed)
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)