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)
