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 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 test___eq__():
i1 = Transformation()
i2 = Transformation()
t = Translation.from_vector([1, 0, 0])
assert i1 == i2
assert not (i1 != i2)
assert i1 != t
assert not (i1 == t)
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, robot, group, function, base_frame=None, tool_frame=None):
# TODO create with class `Tool`, not tool_frame!!
self.group = group
self.joints = robot.get_configurable_joints(group=group)
self.joint_names = robot.get_configurable_joint_names(group=group)
self.function = function
self.base_transformation = Transformation.from_frame(base_frame).inverse() if base_frame else None
self.tool_transformation = Transformation.from_frame(tool_frame).inverse() if tool_frame else None
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 data(self, data):
self.geometry = Geometry.from_data(data['geometry'])
self.origin = Origin.from_data(
data['origin']) if data['origin'] else None
self.name = data['name']
self.attr = _attr_from_data(data['attr'])
self.init_transformation = Transformation.from_data(
data['init_transformation']
) if data['init_transformation'] else None
self.current_transformation = Transformation.from_data(
data['current_transformation']
) if data['current_transformation'] else None
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 data(self, data):
self.attributes.update(data['attributes'] or {})
self.key = data['key']
self.frame.data = data['frame']
self.shape.data = data['shape']
self.features = [(Shape.from_data(shape), operation) for shape, operation in data['features']]
self.transformations = deque([Transformation.from_data(T) for T in data['transformations']])
def test_reflection():
point = [1, 1, 1]
normal = [0, 0, 1]
R1 = Reflection(point, normal)
R2 = Transformation.from_matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 2],
[0, 0, 0, 1]])
assert R1 == R2
def to_local_coordinates(self, obj_in_wcf):
"""Returns the object's coordinates in the local coordinate system of the frame.
Parameters
----------
obj_in_wcf : [float, float, float] | :class:`compas.geometry.Geometry`
An object in the world coordinate frame.
Returns
-------
:class:`compas.geometry.Geometry`
The object in the local coordinate system of the frame.
Notes
-----
If you pass a list of floats, it is assumed to represent a point.
Examples
--------
>>> from compas.geometry import Point
>>> frame = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> pw = Point(2, 2, 2) # point in wcf
>>> pl = frame.to_local_coordinates(pw) # point in frame
>>> frame.to_world_coordinates(pl)
Point(2.000, 2.000, 2.000)
"""
T = Transformation.from_change_of_basis(Frame.worldXY(), self)
if isinstance(obj_in_wcf, (list, tuple)):
return Point(*obj_in_wcf).transformed(T)
return obj_in_wcf.transformed(T)
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 calculate_fixed_transformation(self, position):
"""Returns an identity transformation.
The fixed joint is is not really a joint because it cannot move. All
degrees of freedom are locked.
"""
return Transformation()
def _publish_tf_static_xform(xform=None):
"""Start a docker service advertising a TF2 static transformation.
Parameters
----------
xform : :obj:`list` of :obj:`list` of :obj:`float`, optional
Transformation matrix. Defaults to a zero-matrix.
"""
if xform:
xform = Transformation.from_data(xform)
log.debug("Loading matrix from run_data.")
else:
xform = Transformation()
log.debug("Publishing zero matrix")
publish_static_transform(xform, scale_factor=1000) # mm to m scale factor
def read_mesh_from_filename(self, filename, meshcls):
if not os.path.isfile(filename):
raise FileNotFoundError("No such file: '%s'" % filename)
extension = filename[(filename.rfind(".") + 1):]
if extension == "dae": # no dae support yet
#mesh = Mesh.from_dae(filename)
obj_filename = filename.replace(".dae", ".obj")
if os.path.isfile(obj_filename):
mesh = Mesh.from_obj(obj_filename)
# former DAE files have yaxis and zaxis swapped
# TODO: already fix in conversion to obj
frame = Frame([0,0,0], [1,0,0], [0,0,1])
T = Transformation.from_frame(frame)
mesh_transform(mesh, T)
else:
raise FileNotFoundError("Please convert '%s' into an OBJ file, \
since DAE is currently not supported \
yet." % filename)
elif extension == "obj":
mesh = Mesh.from_obj(filename)
elif extension == "stl":
mesh = Mesh.from_stl(filename)
else:
raise ValueError("%s file types not yet supported" %
extension.upper())
return meshcls(mesh)
def convert_frame_wcf_to_frame_tool0_rcf(self, frame_wcf, base_transformation=None, tool_transformation=None):
T = Transformation.from_frame(frame_wcf)
if base_transformation:
T = base_transformation * T
if tool_transformation:
T = T * tool_transformation
return Frame.from_transformation(T)
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 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 transform(self, T):
"""Transform the frame.
Parameters
----------
T : :class:`compas.geometry.Transformation`
The transformation.
Examples
--------
>>> from compas.geometry import Transformation
>>> f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> T = Transformation.from_frame(f1)
>>> f2 = Frame.worldXY()
>>> f2.transform(T)
>>> f1 == f2
True
"""
# replace this by function call
X = T * Transformation.from_frame(self)
point = X.translation_vector
xaxis, yaxis = X.basis_vectors
self.point = point
self.xaxis = xaxis
self.yaxis = yaxis
def local_to_local_coordinates(frame1, frame2, object_in_frame1):
"""Returns the object's coordinates in frame1 in the local coordinates of frame2.
Parameters
----------
frame1 : :class:`compas.geometry.Frame`
A frame representing one local coordinate system.
frame2 : :class:`compas.geometry.Frame`
A frame representing another local coordinate system.
object_in_frame1 : :class:`compas.geometry.Point` or :class:`compas.geometry.Vector` or :class:`compas.geometry.Frame` or list of float
An object in the coordinate frame1. If you pass a list of float, it is assumed to represent a point.
Returns
-------
:class:`compas.geometry.Point` or :class:`compas.geometry.Vector` or :class:`compas.geometry.Frame`
The object in the local coordinate system of frame2.
Examples
--------
>>> from compas.geometry import Point
>>> frame1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> frame2 = Frame([2, 1, 3], [1., 0., 0.], [0., 1., 0.])
>>> p1 = Point(2, 2, 2) # point in frame1
>>> p2 = Frame.local_to_local_coordinates(frame1, frame2, p1) # point in frame2
>>> Frame.local_to_local_coordinates(frame2, frame1, p2)
Point(2.000, 2.000, 2.000)
"""
T = Transformation.from_change_of_basis(frame1, frame2)
if isinstance(object_in_frame1, list):
return Point(*object_in_frame1).transformed(T)
return object_in_frame1.transformed(T)
def test_reflection():
point = [1, 1, 1]
normal = [0, 0, 1]
plane = point, normal
R1 = Reflection.from_plane(plane)
R2 = Transformation([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 2], [0, 0, 0, 1]])
assert R1 == R2
def _create(self, link, parent_transformation):
"""Private function called during initialisation to transform origins and axes.
Parameters
----------
link : :class:`compas.robots.Link`
Link instance to create.
parent_transformation : :class:`Transformation`
Parent transformation to apply to the link when creating the structure.
"""
if link is None: # some urdfs would fail here otherwise
return
for item in itertools.chain(link.visual, link.collision):
if item.origin:
# transform visual or collision geometry with the transformation specified in origin
transformation = Transformation.from_frame(item.origin)
item.init_transformation = parent_transformation * transformation
else:
item.init_transformation = parent_transformation
for child_joint in link.joints:
child_joint._create(parent_transformation)
# Recursively call creation
self._create(child_joint.child_link,
child_joint.current_transformation)
def to_world_coordinates(self, object_in_lcf):
"""Returns the object's coordinates in the global coordinate frame.
Parameters
----------
object_in_lcf : :class:`compas.geometry.Point` or :class:`compas.geometry.Vector` or :class:`compas.geometry.Frame` or list of float
An object in local coordinate system of the frame.
Returns
-------
:class:`compas.geometry.Point` or :class:`compas.geometry.Vector` or :class:`compas.geometry.Frame`
The object in the world coordinate frame.
Notes
-----
If you pass a list of float, it is assumed to represent a point.
Examples
--------
>>> from compas.geometry import Point
>>> frame = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> pl = Point(1.632, -0.090, 0.573) # point in frame
>>> pw = frame.to_world_coordinates(pl) # point in wcf
>>> frame.to_local_coordinates(pw)
Point(1.632, -0.090, 0.573)
"""
T = Transformation.from_change_of_basis(self, Frame.worldXY())
if isinstance(object_in_lcf, list):
return Point(*object_in_lcf).transformed(T)
else:
return object_in_lcf.transformed(T)
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 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 compute_transformations(self,
joint_state,
link=None,
parent_transformation=None):
"""Recursive function to calculate the transformations of each joint.
Parameters
----------
joint_state : :class:`compas.robots.Configuration` | dict[str, float]
A configuration instance or a dictionary with joint names and joint values in radians and
meters (depending on the joint type).
link : :class:`compas.robots.Link`, optional
Link instance to calculate the child joint's transformation.
parent_transformation : :class:`Transformation`, optional
The transfomation of the parent joint.
Defaults to the identity matrix.
Returns
-------
dict[str, :class:`Transformation`]
A dictionary with the joint names as keys and values are the joint's respective transformation.
Examples
--------
>>> robot = RobotModel.ur5()
>>> config = robot.random_configuration()
>>> transformations = robot.compute_transformations(config)
"""
if link is None:
link = self.root
if parent_transformation is None:
parent_transformation = Transformation()
transformations = {}
for child_joint in link.joints:
if child_joint.name in joint_state.keys(
): # if passive/mimicking joint is in the joint_state, the transformation will be calculated according to this value
position = joint_state[child_joint.name]
transformation = parent_transformation * child_joint.calculate_transformation(
position)
elif child_joint.mimic and child_joint.mimic.joint in joint_state.keys(
):
mimicked_joint_position = joint_state[child_joint.mimic.joint]
position = child_joint.mimic.calculate_position(
mimicked_joint_position)
transformation = parent_transformation * child_joint.calculate_transformation(
position)
else:
transformation = parent_transformation
transformations.update({child_joint.name: transformation})
# call function on child
transformations.update(
self.compute_transformations(joint_state,
child_joint.child_link,
transformation))
return transformations
def create(self, link=None):
"""Recursive function that triggers the drawing of the robot geometry.
This method delegates the geometry drawing to the :meth:`draw_geometry`
method. It transforms the geometry based on the saved initial
transformation from the robot model.
Parameters
----------
link : :class:`compas.robots.Link`, optional
Link instance to create. Defaults to the robot model's root.
Returns
-------
None
"""
if link is None:
link = self.robot.root
for item in itertools.chain(link.visual, link.collision):
# NOTE: Currently, shapes assign their meshes to an
# attribute called `geometry`, but this will change soon to `meshes`.
# This code handles the situation in a forward-compatible
# manner. Eventually, this can be simplified to use only `meshes` attr
if hasattr(item.geometry.shape, 'meshes'):
meshes = item.geometry.shape.meshes
else:
meshes = item.geometry.shape.geometry
if isinstance(meshes, Shape):
meshes = [Mesh.from_shape(meshes)]
if meshes:
# Coerce meshes into an iteratable (a tuple if not natively iterable)
if not hasattr(meshes, '__iter__'):
meshes = (meshes, )
is_visual = hasattr(item, 'get_color')
color = item.get_color() if is_visual else None
native_geometry = []
for i, mesh in enumerate(meshes):
# create native geometry
mesh_type = 'visual' if is_visual else 'collision'
mesh_name = '{}.{}.{}.{}'.format(self.robot.name,
mesh_type, link.name, i)
native_mesh = self.draw_geometry(mesh,
name=mesh_name,
color=color)
# transform native geometry based on saved init transform
self.transform(native_mesh, item.init_transformation)
# append to list
native_geometry.append(native_mesh)
item.native_geometry = native_geometry
item.current_transformation = Transformation()
for child_joint in link.joints:
self.create(child_joint.child_link)
def frame(self) -> compas.geometry.Frame:
location = self.shape.Location()
transformation = location.Transformation()
T = Transformation(
matrix=[[transformation.Value(i, j) for j in range(4)]
for i in range(4)])
frame = Frame.from_transformation(T)
return frame
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 compute_transformations(self,
joint_state,
link=None,
parent_transformation=None):
"""Recursive function to calculate the transformations of each joint.
Parameters
----------
joint_state : dict
A dictionary with the joint names as keys and values in radians and
meters (depending on the joint type).
link : :class:`compas.robots.Link`
Link instance to calculate the child joint's transformation.
parent_transformation : :class:`Transformation`
The transfomation of the parent joint.
Returns
-------
dict of str: :class:`Transformation`
A dictionary with the joint names as keys and values are the joint's respective transformation.
Examples
--------
>>> names = robot.get_configurable_joint_names()
>>> values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.000]
>>> joint_state = dict(zip(names, values))
>>> transformations = robot.compute_transformations(joint_state)
"""
if link is None:
link = self.root
if parent_transformation is None:
parent_transformation = Transformation()
transformations = {}
for child_joint in link.joints:
if child_joint.name in joint_state.keys(
): # if passive/mimicking joint is in the joint_state, the transformation will be calculated according to this value
position = joint_state[child_joint.name]
transformation = parent_transformation * child_joint.calculate_transformation(
position)
elif child_joint.mimic and child_joint.mimic.joint in joint_state.keys(
):
mimicked_joint_position = joint_state[child_joint.mimic.joint]
position = child_joint.mimic.calculate_position(
mimicked_joint_position)
transformation = parent_transformation * child_joint.calculate_transformation(
position)
else:
transformation = parent_transformation
transformations.update({child_joint.name: transformation})
# call function on child
transformations.update(
self.compute_transformations(joint_state,
child_joint.child_link,
transformation))
return transformations
