def test_4_faces_transformed(self):
tf = np.eye(4)
tf[:3, 3] = np.array([5, -3, 7])
tf[:3, :3] = rot_y(0.5) @ rot_z(-0.3)
cyl = Cylinder(1, 2, approx_faces=4)
n = cyl.get_normals(tf)
n_desired = np.array([[1, 0, 0], [0, 1, 0], [-1, 0, 0], [0, -1, 0],
[0, 0, 1], [0, 0, -1]])
n_desired = (tf[:3, :3] @ n_desired.T).T
assert_almost_equal(n, n_desired)
v = cyl.get_vertices(tf)
v_desired = np.array([
[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[1, 0, -1],
[0, 1, -1],
[-1, 0, -1],
[0, -1, -1],
])
v_desired = (rot_z(np.pi / 4) @ v_desired.T).T
v_desired = (tf[:3, :3] @ v_desired.T).T + tf[:3, 3]
assert_almost_equal(v, v_desired)
def create_path(start, stop, n_path, rx_samples=5, ry_samples=5, rz_samples=90):
R_pt = rot_z(np.pi / 2) @ rot_x(3 * np.pi / 4)
pos_tol = 3 * [NoTolerance()]
rot_tol = [
SymmetricTolerance(np.deg2rad(10), rx_samples),
SymmetricTolerance(np.deg2rad(15), ry_samples),
Tolerance(-np.pi, np.pi, rz_samples),
]
start_pt = TolEulerPt(start, Quaternion(matrix=R_pt), pos_tol, rot_tol)
return create_line(start_pt, stop, n_path)
def test_4_faces(self):
cyl = Cylinder(1, 2, approx_faces=4)
n = cyl.get_normals(np.eye(4))
n_desired = np.array([[1, 0, 0], [0, 1, 0], [-1, 0, 0], [0, -1, 0],
[0, 0, 1], [0, 0, -1]])
assert_almost_equal(n, n_desired)
v = cyl.get_vertices(np.eye(4))
v_desired = np.array([
[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[1, 0, -1],
[0, 1, -1],
[-1, 0, -1],
[0, -1, -1],
])
v_desired = (rot_z(np.pi / 4) @ v_desired.T).T
assert_almost_equal(v, v_desired)
e = cyl.get_edges(np.eye(4))
e_desired = np.zeros((12, 6))
vd = v_desired
e_desired[0] = np.hstack((vd[3], vd[0]))
e_desired[1] = np.hstack((vd[0], vd[1]))
e_desired[2] = np.hstack((vd[1], vd[2]))
e_desired[3] = np.hstack((vd[2], vd[3]))
e_desired[4] = np.hstack((vd[7], vd[4]))
e_desired[5] = np.hstack((vd[4], vd[5]))
e_desired[6] = np.hstack((vd[5], vd[6]))
e_desired[7] = np.hstack((vd[6], vd[7]))
e_desired[8] = np.hstack((vd[0], vd[4]))
e_desired[9] = np.hstack((vd[1], vd[5]))
e_desired[10] = np.hstack((vd[2], vd[6]))
e_desired[11] = np.hstack((vd[3], vd[7]))
assert e.shape == e_desired.shape
assert_almost_equal(e[0:4], e_desired[0:4])
assert_almost_equal(e[4:8], e_desired[4:8])
assert_almost_equal(e[8:12], e_desired[8:12])
def test_rot_z(self):
A = rot_z(0.6)
B = rot_z(-0.6)
assert_almost_equal(A @ B, np.eye(3))
assert_almost_equal(A[2, :3], np.array([0, 0, 1]))
assert_almost_equal(A[:3, 2], np.array([0, 0, 1]))
from acrobotics.util import get_default_axes3d, plot_reference_frame
from acrobotics.util import rot_z
from acrobotics.path.sampling import SampleMethod
from acrobotics.path.tolerance import (
Tolerance,
NoTolerance,
SymmetricTolerance,
QuaternionTolerance,
)
from acrobotics.path.path_pt import TolPositionPt, TolEulerPt, TolQuatPt
# ==================================================================================
# Position samples
# ==================================================================================
R_pt = rot_z(np.deg2rad(30))
pos_pt = np.array([0.5, 0.5, 0.0])
pos_tol = [Tolerance(-0.1, 0.1, 3), Tolerance(-0.3, 0.3, 5), NoTolerance()]
pt_pos = TolPositionPt(pos_pt, Quaternion(matrix=R_pt), pos_tol)
# ==================================================================================
# Euler constraints samples
# ==================================================================================
R_pt = np.eye(3)
pos_tol = 3 * [NoTolerance()]
rot_tol = [
SymmetricTolerance(np.deg2rad(15), 5),
NoTolerance(),
Tolerance(0, np.pi / 2, 10),