def test_to_transform(self):
distance = 0.1
q = Quaternion()
pos_tol = 3 * [NoTolerance()]
distance = 0.1
point = TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(distance))
tf = point.to_transform([1, 2, 3], q)
assert_almost_equal(
[[1, 0, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]], tf
)
def test_sample_incremental(self):
method = SampleMethod.random_uniform
q = Quaternion()
pos_tol = 3 * [NoTolerance()]
distance = 0.1
point = TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(distance))
samples = point.sample_incremental(100, method)
for tf in samples:
newquat = Quaternion(matrix=tf)
assert Quaternion.distance(q, newquat) <= distance
def test_tol_quat_pt_with_weights():
path_ori_free = []
for s in np.linspace(0, 1, 3):
xi = 0.8
yi = s * 0.2 + (1 - s) * (-0.2)
zi = 0.2
path_ori_free.append(
TolQuatPt(
[xi, yi, zi],
Quaternion(axis=[1, 0, 0], angle=np.pi),
[NoTolerance(), NoTolerance(), NoTolerance()],
QuaternionTolerance(2.0),
)
)
table = Box(0.5, 0.5, 0.1)
table_tf = np.array(
[[1, 0, 0, 0.80], [0, 1, 0, 0.00], [0, 0, 1, 0.12], [0, 0, 0, 1]]
)
scene1 = Scene([table], [table_tf])
robot = Kuka()
# robot.tool = torch
setup = PlanningSetup(robot, path_ori_free, scene1)
# weights to express the importance of the joints in the cost function
joint_weights = [10.0, 5.0, 1.0, 1.0, 1.0, 1.0]
settings = SamplingSetting(
SearchStrategy.INCREMENTAL,
sample_method=SampleMethod.random_uniform,
num_samples=500,
iterations=2,
tolerance_reduction_factor=2,
weights=joint_weights,
)
solve_set = SolverSettings(
SolveMethod.sampling_based,
CostFuntionType.weighted_sum_squared,
sampling_settings=settings,
)
sol = solve(setup, solve_set)
assert sol.success
for qi, s in zip(sol.joint_positions, np.linspace(0, 1, 3)):
xi = 0.8
yi = s * 0.2 + (1 - s) * (-0.2)
zi = 0.2
fk = robot.fk(qi)
pos_fk = fk[:3, 3]
assert_almost_equal(pos_fk, np.array([xi, yi, zi]))
def test_create(self):
q = Quaternion()
pos_tol = 3 * [NoTolerance()]
TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(0.5))
R_pt = np.eye(3)
pos_tol = 3 * [NoTolerance()]
rot_tol = [
SymmetricTolerance(np.deg2rad(15), 5),
NoTolerance(),
Tolerance(0, np.pi / 2, 10),
]
pt_eul = TolEulerPt(np.zeros(3), Quaternion(matrix=R_pt), pos_tol, rot_tol)
# ==================================================================================
# Quaternion constraints samples
# ==================================================================================
R_pt = np.eye(4)
pos_tol = 3 * [NoTolerance()]
quat_tol = QuaternionTolerance(0.1)
pt_quat = TolQuatPt(np.zeros(3), Quaternion(matrix=R_pt), pos_tol, quat_tol)
# ==================================================================================
# Create plot and save it
# ==================================================================================
fig = plt.figure(figsize=plt.figaspect(1 / 3))
axes = [fig.add_subplot(1, 3, i, projection="3d") for i in [1, 2, 3]]
# all axis show the path point reference frame and have no coordinate axes
for pt, ax in zip([pt_pos, pt_eul, pt_quat], axes):
ax.set_axis_off()
plot_reference_frame(ax, tf=pt.transformation_matrix, arrow_length=0.2)
# here we plot the samples for each specific point