def test_grid(self):
pos = [1, 2, 3]
pos_tol = [NoTolerance()] * 3
rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(np.pi, 3)]
pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol)
tf_samples = pt.sample_grid()
assert_almost_equal(tf_samples[0], tf_samples[2])
assert_almost_equal(tf_samples[1][:3, :3], np.eye(3))
def test_incremental(self):
pos = [1, 2, 3]
pos_tol = [NoTolerance()] * 3
rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(np.pi, 3)]
pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol)
tf_samples = pt.sample_incremental(10, SampleMethod.random_uniform)
euler = [rotation_matrix_to_rpy(tf[:3, :3]) for tf in tf_samples]
for i in range(10):
assert_almost_equal(euler[i][0], 0)
assert_almost_equal(euler[i][1], 0)
assert_in_range(euler[i][2], -np.pi, np.pi)
def path_from_weld_lines(wlines, dist_res, ang_res, shrink_dist):
paths = []
for wl in wlines:
if shrink_dist != None:
wl["start"], wl["goal"] = shrink_path(wl["start"], wl["goal"],
shrink_dist)
trans, rots = tf_interpolations(wl["start"],
wl["goal"],
max_cart_step=dist_res)
constraints = cons_list_to_dict(wl["con"])
path = []
for p, R in zip(trans, rots):
if "xyz" in constraints.keys():
pos_tol = parse_constraints(constraints["xyz"], dist_res,
ang_res)
else:
pos_tol = [NoTolerance()] * 3
if "rpy" in constraints.keys():
rot_tol = parse_constraints(
constraints["rpy"],
dist_res,
ang_res,
angular=True,
convert_angles=True,
)
else:
rot_tol = [NoTolerance()] * 3
path.append(
TolEulerPt(p, Quaternion(matrix=R.as_matrix()), pos_tol,
rot_tol))
paths.append(path)
return paths
def test_transform_to_rel_tolerance_deviation(self):
tol = [SymmetricTolerance(0.1, 3), Tolerance(0.2, 1.0, 3), NoTolerance()]
rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(0.1, 3)]
quat = Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1]))
pt = TolEulerPt([1, 2, 3], quat, tol, rot_tol)
quat2 = Quaternion(angle=np.pi / 2 - 0.05, axis=np.array([0, 0, 1]))
tf = quat2.transformation_matrix
tf[:3, 3] = np.array([1.06, 1.91, 3])
p_rel = pt.transform_to_rel_tolerance_deviation(tf)
assert p_rel.shape == (6,)
p_desired = np.array([-0.09, -0.06, 0.0, 0.0, 0.0, -0.05])
assert_almost_equal(p_rel, p_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_state_cost():
robot = Kuka()
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.00], [0, 0, 0, 1]]
)
scene1 = Scene([table], [table_tf])
# create path
quat = Quaternion(axis=np.array([1, 0, 0]), angle=-3 * np.pi / 4)
pos_tol = 3 * [NoTolerance()]
# rot_tol = 3 * [NoTolerance()]
rot_tol = [
NoTolerance(),
SymmetricTolerance(np.pi / 4, 20),
SymmetricTolerance(np.pi, 20),
]
first_point = TolEulerPt(np.array([0.9, -0.1, 0.2]), quat, pos_tol, rot_tol)
# end_position = np.array([0.9, 0.1, 0.2])
# path = create_line(first_point, end_position, 5)
path = create_arc(
first_point, np.array([0.9, 0.0, 0.2]), np.array([0, 0, 1]), 2 * np.pi, 5
)
planner_settings = SamplingSetting(
SearchStrategy.GRID,
iterations=1,
tolerance_reduction_factor=2,
use_state_cost=True,
state_cost_weight=10.0,
)
solver_settings = SolverSettings(
SolveMethod.sampling_based,
CostFuntionType.sum_squared,
sampling_settings=planner_settings,
)
setup = PlanningSetup(robot, path, scene1)
sol = solve(setup, solver_settings)
assert sol.success
def test_create_arc(self):
pos = [1, 0, 3]
pos_tol = [NoTolerance(), NoTolerance(), NoTolerance()]
rot_tol = [
SymmetricTolerance(0.5, 10),
SymmetricTolerance(0.5, 10),
SymmetricTolerance(0.5, 10),
]
start_pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol)
path = create_arc(start_pt, np.zeros(3), np.array([0, 0, 1]), np.pi / 2, 3)
assert len(path) == 3
p1, p2, p3 = path[0], path[1], path[2]
assert_almost_equal(p1.pos[0], 1)
assert_almost_equal(p1.pos[1], 0)
assert_almost_equal(p1.pos[2], 3)
assert_almost_equal(p2.pos[0], np.sqrt(2) / 2)
assert_almost_equal(p2.pos[1], np.sqrt(2) / 2)
assert_almost_equal(p2.pos[2], 3)
assert_almost_equal(p3.pos[0], 0)
assert_almost_equal(p3.pos[1], 1)
assert_almost_equal(p3.pos[2], 3)
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),
]
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]]