def __init__(self, tcp_frame):
self.model = []
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 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 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 transform(self, transformation):
"""Transform the frame.
Parameters
----------
transformation : :class:`Transformation`
The transformation used to transform the Frame.
Examples
--------
>>> from compas.geometry import Frame
>>> 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
"""
T = transformation * Transformation.from_frame(self)
point = T.translation
xaxis, yaxis = T.basis_vectors
self.point = point
self.xaxis = xaxis
self.yaxis = yaxis
def represent_frame_in_global_coordinates(self, frame):
"""Represents another frame in the local coordinate system in the world
coordinate system.
Parameters
----------
frame: :class:`Frame`
A frame in the local coordinate system.
Returns
-------
:class:`Frame`
A frame in the world coordinate system.
Examples
--------
>>> from compas.geometry import Frame
>>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> pw1 = Frame([1, 1, 1], [0.707, 0.707, 0], [-0.707, 0.707, 0])
>>> pf = f.represent_frame_in_local_coordinates(pw1)
>>> pw2 = f.represent_frame_in_global_coordinates(pf)
>>> allclose(pw1.point, pw2.point)
True
>>> allclose(pw1.xaxis, pw2.xaxis)
True
>>> allclose(pw1.yaxis, pw2.yaxis)
True
"""
T = Transformation.from_frame(self)
f = frame.copy()
f.transform(T)
return f
def get_tool0_transformation(self, T5):
"""Get the transformation to reach tool0_frame.
"""
return T5 * Transformation.from_frame(self.tool0_frame)
def get_tcp_frame_from_tool0_frame(self, frame_tool0):
"""Get the tcp frame from the tool0 frame.
"""
T = Transformation.from_frame(frame_tool0)
return Frame.from_transformation(T * self.transformation_tcp_tool0)
def get_tool0_frame_from_tcp_frame(self, frame_tcp):
"""Get the tool0 frame (frame at robot) from the tool frame (frame_tcp).
"""
T = Transformation.from_frame(frame_tcp)
return Frame.from_transformation(T * self.transformation_tool0_tcp)
def init_transformation(self):
if self.init_origin:
return Transformation.from_frame(self.init_origin)
else:
return Transformation()
def current_transformation(self):
if self.origin:
return Transformation.from_frame(self.origin)
else:
return Transformation()
def _to_xform(m):
m = m if len(m) == 16 else m + [0, 0, 0, 1]
return xform_from_transformation(Transformation.from_list(m))
# ==============================================================================
# Main
# ==============================================================================
if __name__ == '__main__':
f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
R = Rotation.from_frame(f1)
f2 = Frame.from_rotation(R, point=f1.point)
print(f1 == f2)
print(f2)
f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = Transformation.from_frame(f1)
f2 = Frame.from_transformation(T)
print(f1 == f2)
ea1 = [0.5, 0.4, 0.8]
M = matrix_from_euler_angles(ea1)
f = Frame.from_matrix(M)
ea2 = f.euler_angles()
print(allclose(ea1, ea2))
q1 = [0.945, -0.021, -0.125, 0.303]
f = Frame.from_quaternion(q1, point=[1., 1., 1.])
q2 = f.quaternion
print(allclose(q1, q2, tol=1e-03))
aav1 = [-0.043, -0.254, 0.617]