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 test_axis_and_angle_from_matrix():
axis1 = normalize_vector([-0.043, -0.254, 0.617])
angle1 = 0.1
R = matrix_from_axis_and_angle(axis1, angle1)
axis2, angle2 = axis_and_angle_from_matrix(R)
assert allclose(axis1, axis2)
assert allclose([angle1], [angle2])
def intersection_line_box_xy(line, box, tol=1e-6):
"""Compute the intersection between a line and a box in the XY plane.
Parameters
----------
line : [point, point] | :class:`compas.geometry.Line`
A line defined by two points, with at least XY coordinates.
box : [point, point, point, point]
A box defined by 4 points, with at least XY coordinates.
tol : float, optional
A tolerance value for point comparison.
Returns
-------
tuple[[float, float, 0.0], [float, float, 0.0]] | [float, float, 0.0] | None
Two points if the line goes through the box.
One point if the line goes through one of the box vertices only.
None otherwise.
"""
points = []
for segment in pairwise(box + box[:1]):
x = intersection_line_segment_xy(line, segment, tol=tol)
if x:
points.append(x)
if len(points) < 3:
return tuple(points)
if len(points) == 3:
a, b, c = points
if allclose(a, b, tol=tol):
return a, c
if allclose(b, c, tol=tol):
return a, b
return b, c
def intersection_line_box_xy(line, box, tol=1e-6):
"""Compute the intersection between a line and a box in the XY plane.
Parameters
----------
line : list of 2 points or :class:`compas.geometry.Line`
box : list of 4 points
tol : float, optional
A tolerance value for point comparison.
Default is ``1e-6``.
Returns
-------
list
A list of at most two intersection points.
"""
points = []
for segment in pairwise(box + box[:1]):
x = intersection_line_segment_xy(line, segment, tol=tol)
if x:
points.append(x)
if len(points) < 3:
return points
if len(points) == 3:
a, b, c = points
if allclose(a, b, tol=tol):
return [a, c]
if allclose(b, c, tol=tol):
return [a, b]
return [b, c]
def test_basis_vectors_from_matrix():
f = Frame([0, 0, 0], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
R = matrix_from_frame(f)
xaxis, yaxis = basis_vectors_from_matrix(R)
assert allclose(
xaxis, [0.6807833515407016, 0.6807833515407016, 0.2703110366411609])
assert allclose(
yaxis, [-0.6687681911461376, 0.7282315441900513, -0.14975955581430114])
def test_matrix_inverse(R, T):
assert allclose(matrix_inverse(R.matrix),
[[1.0, -0.0, 0.0, -0.0],
[-0.0, -0.4480736161291701, 0.8939966636005579, 0.0],
[0.0, -0.8939966636005579, -0.4480736161291701, -0.0],
[-0.0, 0.0, -0.0, 1.0]])
assert allclose(matrix_inverse(T.matrix),
[[1.0, -0.0, 0.0, -1.0], [-0.0, 1.0, -0.0, -2.0],
[0.0, -0.0, 1.0, -3.0], [-0.0, 0.0, -0.0, 1.0]])
def test_basis_vectors():
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
x, y = M.basis_vectors
assert allclose(x, Vector(0.41249169135312663, -0.05897071585157175, -0.9090506362335324))
assert allclose(y, Vector(-0.8335562904208867, -0.4269749553355485, -0.35053715668381935))
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 allclose(R1, R2)
def __init__(self, matrix=None):
if matrix:
_, _, _, translation, _ = decompose_matrix(matrix)
check = matrix_from_translation(translation)
if not allclose(flatten(matrix), flatten(check)):
raise ValueError('This is not a proper translation matrix.')
super(Translation, self).__init__(matrix=matrix)
def __init__(self, matrix=None):
if matrix:
_, _, angles, _, _ = decompose_matrix(matrix)
check = matrix_from_euler_angles(angles)
if not allclose(flatten(matrix), flatten(check)):
raise ValueError('This is not a proper rotation matrix.')
super(Rotation, self).__init__(matrix=matrix)
def basis_vectors_from_matrix(R):
"""Returns the basis vectors from the rotation matrix R.
Raises
------
ValueError
If rotation matrix is invalid.
Returns
-------
list of two vectors
The X and Y basis vectors of the rotation.
Examples
--------
>>> from compas.geometry import Frame
>>> f = Frame([0, 0, 0], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> R = matrix_from_frame(f)
>>> xaxis, yaxis = basis_vectors_from_matrix(R)
"""
xaxis = [R[0][0], R[1][0], R[2][0]]
yaxis = [R[0][1], R[1][1], R[2][1]]
zaxis = [R[0][2], R[1][2], R[2][2]]
if not allclose(zaxis, cross_vectors(xaxis, yaxis)):
raise ValueError("Matrix is invalid rotation matrix.")
return [xaxis, yaxis]
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 allclose(R1.basis_vectors, R2)
def __init__(self, matrix=None):
if matrix:
_, _, _, _, perspective = decompose_matrix(matrix)
check = matrix_from_perspective_entries(perspective)
if not allclose(flatten(matrix), flatten(check)):
raise ValueError('This is not a proper projection matrix.')
super(Projection, self).__init__(matrix=matrix)
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 intersection_line_box_xy(line, box, tol=1e-6):
points = []
for segment in pairwise(box + box[:1]):
x = intersection_line_segment_xy(line, segment, tol=tol)
if x:
points.append(x)
if len(points) < 3:
return points
if len(points) == 3:
a, b, c = points
if allclose(a, b, tol=tol):
return [a, c]
if allclose(b, c, tol=tol):
return [a, b]
return [a, b]
return [a, c]
def test_matrix_from_orthogonal_projection():
point = [0, 0, 0]
normal = [0, 0, 1]
P = matrix_from_orthogonal_projection((point, normal))
p = [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]
assert allclose(P, p)
def __init__(self, matrix=None):
if matrix:
scale, _, _, _, _ = decompose_matrix(matrix)
check = matrix_from_scale_factors(scale)
if not allclose(flatten(matrix), flatten(check)):
raise ValueError('This is not a proper scale matrix.')
super(Scale, self).__init__(matrix=matrix)
def test_shear():
angle = 1
direction = [1, 0, 0]
point = [1, 1, 1]
normal = [0, 0, 1]
S = Shear.from_angle_direction_plane(angle, direction, (point, normal))
s = [[1.0, 0.0, 1.557407724654902, -1.557407724654902], [0.0, 1.0, 0.0, -0.0], [0.0, 0.0, 1.0, -0.0], [0.0, 0.0, 0.0, 1.0]]
assert allclose(S.matrix, s)
def test_matrix_from_quaternion():
q1 = [0.945, -0.021, -0.125, 0.303]
R = matrix_from_quaternion(q1)
r = [[0.7853252073134178, -0.5669097811969227, -0.2487521230892197, 0.0],
[0.5774003396942752, 0.8156659006893796, -0.0360275751823359, 0.0],
[0.22332300929163754, -0.11533619742231993, 0.9678968927964832, 0.0],
[0.0, 0.0, 0.0, 1.0]]
assert allclose(R, r)
def test_matrix_from_perspective_projection():
point = [0, 0, 0]
normal = [0, 0, 1]
perspective = [1, 1, 0]
P = matrix_from_perspective_projection((point, normal), perspective)
p = [[0.0, 0.0, -1.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0]]
assert allclose(P, p)
def test_matrix_from_parallel_projection():
point = [0, 0, 0]
normal = [0, 0, 1]
direction = [1, 1, 1]
P = matrix_from_parallel_projection((point, normal), direction)
p = [[1.0, 0.0, -1.0, 0.0], [0.0, 1.0, -1.0, 0.0], [0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]
assert allclose(P, p)
def test_matrix_from_frame():
f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = matrix_from_frame(f)
t = [[0.6807833515407016, -0.6687681911461376, -0.29880283595731283, 1.0],
[0.6807833515407016, 0.7282315441900513, -0.0788216106888398, 1.0],
[0.2703110366411609, -0.14975955581430114, 0.9510541619236438, 1.0],
[0.0, 0.0, 0.0, 1.0]]
assert allclose(T, t)
def test_rotation():
angle1 = [-2.142, 1.141, -0.142]
R = Rotation.from_euler_angles(angle1)
matrix = [[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 allclose(R.matrix, matrix)
def test_oriented_bounding_box_numpy(coords, expected):
if compas.IPY:
return
from compas.geometry import oriented_bounding_box_numpy
results = oriented_bounding_box_numpy(coords).tolist()
for result, expected_values in zip(results, expected):
assert allclose(result, expected_values, tol=1e-3)
def test_kinematic_functions():
kin = UR5eKinematics() # one UR is enough, they are all the same
q = [0.2, 0.5, 1.4, 1.3, 2.6, 2.3]
frame = kin.forward(q)
sol = kin.inverse(frame)
assert(allclose(sol[0], q))
kin = Staubli_TX260LKinematics()
q = [0.2, 0.5, 1.4, 1.3, 2.6, 2.3]
frame = kin.forward(q)
sol = kin.inverse(frame)
assert(allclose(sol[0], q))
kin = ABB_IRB4600_40_255Kinematics()
q = [0.2, 0.5, 1.4, 1.3, 2.6, 2.3]
frame = kin.forward(q)
sol = kin.inverse(frame)
assert(allclose(sol[0], q))
def test_matrix_from_basis_vectors():
xaxis = [0.68, 0.68, 0.27]
yaxis = [-0.67, 0.73, -0.15]
R = matrix_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 allclose(R, r)
def test_matrix_from_euler_angles():
ea1 = 1.4, 0.5, 2.3
args = True, 'xyz'
R = matrix_from_euler_angles(ea1, *args)
r = [[-0.5847122176808724, -0.4415273357486694, 0.6805624396639868, 0.0],
[0.6544178905170501, 0.23906322244658262, 0.7173464994301357, 0.0],
[-0.479425538604203, 0.8648134986574489, 0.14916020070358058, 0.0],
[0.0, 0.0, 0.0, 1.0]]
assert allclose(R, r)
def test_switch_package():
with Proxy('numpy', python='python') as proxy:
A = proxy.array([[1, 2], [3, 4]])
proxy.package = 'scipy.linalg'
r = proxy.inv(A)
assert allclose(r, [[-2, 1], [1.5, -0.5]])
def test_matrix_from_axis_angle_vector():
axis1 = normalize_vector([-0.043, -0.254, 0.617])
angle1 = 0.1
R = matrix_from_axis_and_angle(axis1, angle1)
r = [[0.9950248278789664, -0.09200371122722178, -0.03822183963195913, 0.0],
[0.09224781823368366, 0.9957251324831573, 0.004669108322156158, 0.0],
[
0.037628871037522216, -0.008171760019527692, 0.9992583701939277,
0.0
], [0.0, 0.0, 0.0, 1.0]]
assert allclose(R, r)
def test_external_axes():
ea = rrc.ExternalAxes()
assert list(ea) == []
ea = rrc.ExternalAxes(30)
assert list(ea) == [30]
ea = rrc.ExternalAxes(30, 10, 0)
assert list(ea) == [30, 10, 0]
ea = rrc.ExternalAxes([30, 10, 0])
assert list(ea) == [30, 10, 0]
ea = rrc.ExternalAxes(iter([30, 10, 0]))
assert list(ea) == [30, 10, 0]
j = rrc.ExternalAxes(30, 45, 0)
c = j.to_configuration_primitive([0, 1, 2], ['j1', 'j2', 'j3'])
assert c.joint_values == [math.pi/6, math.pi/4, 0.0]
assert c.joint_names == ['j1', 'j2', 'j3']
j = rrc.ExternalAxes(30, -45, 100)
c = j.to_configuration_primitive([0, 1, 2])
assert c.joint_values == [math.pi/6, -math.pi/4, 0.1]
assert len(c.joint_names) == 0
config = Configuration([2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6], [0, 0, 0, 0, 0, 0])
rj = rrc.ExternalAxes.from_configuration_primitive(config)
assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
config = Configuration(
[0, 2*math.pi, math.pi, math.pi/2, math.pi/3, math.pi/4, math.pi/6 ],
[0, 0, 0, 0, 0, 0, 0],
['a', 'b', 'c', 'd', 'e', 'f', 'g'])
rj = rrc.ExternalAxes.from_configuration_primitive(config, ['b', 'c', 'd', 'e', 'f', 'g'])
assert allclose(rj.values, [360, 180, 90, 60, 45, 30])
j = rrc.RobotJoints(30, 10, 0)
config = j.to_configuration_primitive([0, 0, 0])
new_j = rrc.ExternalAxes.from_configuration_primitive(config)
assert allclose(j.values, new_j.values)
