def create_new_pt(
fk_transform: np.ndarray,
q: np.ndarray,
prev_pt: PathPt,
tolerance_reduction_factor=2.0,
):
if isinstance(prev_pt, TolQuatPt):
new_pos = fk_transform[:3, 3]
new_quat = Quaternion(matrix=fk_transform)
new_tol = deepcopy(prev_pt.tol)
ref_values = prev_pt.transform_to_rel_tolerance_deviation(fk_transform)
for tolerance, ref in zip(new_tol, ref_values):
tolerance.reduce_tolerance(tolerance_reduction_factor, ref)
return TolQuatPt(new_pos, new_quat, new_tol[:3], new_tol[3])
elif isinstance(prev_pt, TolEulerPt):
new_pos = fk_transform[:3, 3]
new_quat = Quaternion(matrix=fk_transform)
new_tol = deepcopy(prev_pt.tol)
ref_values = prev_pt.transform_to_rel_tolerance_deviation(fk_transform)
for tolerance, ref in zip(new_tol, ref_values):
tolerance.reduce_tolerance(tolerance_reduction_factor, ref)
return TolEulerPt(new_pos, new_quat, new_tol[:3], new_tol[3:])
else:
raise NotImplementedError()
def test_random_near():
q = Quaternion()
for _ in range(5):
q_near = q.random_near(0.11)
assert quat_distance(q, q_near) <= 0.11
assert np.any(np.not_equal(q.rotation_matrix, q_near.rotation_matrix))
for _ in range(5):
q_near = q.random_near(0.01)
assert quat_distance(q, q_near) <= 0.01
assert np.any(np.not_equal(q.rotation_matrix, q_near.rotation_matrix))
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 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 test_sample_grid(self):
q = Quaternion()
pos_tol = [Tolerance(-0.5, 0.5, 2), Tolerance(-0.5, 0.5, 2), NoTolerance()]
point = TolPositionPt([0.5, 0.5, 1], q, pos_tol)
grid = point.sample_grid()
position_samples = [[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]]
for pos, T in zip(position_samples, grid):
assert_almost_equal(pos, T[:3, 3])
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_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_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 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_transform_to_rel_tolerance_deviation(self):
tol = [SymmetricTolerance(0.1, 3), Tolerance(0.2, 1.0, 3), NoTolerance()]
quat = Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1]))
pt = TolPositionPt([1, 2, 3], quat, tol)
tf = quat.transformation_matrix
tf[:3, 3] = np.array([1.06, 1.91, 3])
p_rel = pt.transform_to_rel_tolerance_deviation(tf)
p_desired = np.array([-0.09, -0.06, 0.0])
assert_almost_equal(p_rel, p_desired)
def test_sample_grid_2(self):
"""
Rotate with pi / 2 around z, give a tolerance along x,
expect a grid sampled along y
"""
tol = [SymmetricTolerance(0.1, 3), NoTolerance(), NoTolerance()]
pt = TolPositionPt(
[1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol
)
tf_samples = pt.sample_grid()
g = [tf[:3, 3] for tf in tf_samples]
g_desired = np.array([[1, 1.9, 3], [1, 2, 3], [1, 2.1, 3]])
assert_almost_equal(g, g_desired)
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 test_sample_incremental_2(self):
tol = [SymmetricTolerance(0.1, 3), Tolerance(0.2, 1.0, 3), NoTolerance()]
pt = TolPositionPt(
[1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol
)
g = pt.sample_incremental(10, SampleMethod.random_uniform)
pos_samples = [tf[:3, 3] for tf in g]
for sample in pos_samples:
assert_almost_equal(sample[2], 3)
assert_in_range(sample[0], 0, 3) # 1 + (-1, 2)
assert_in_range(sample[1], 1.9, 2.1) # 2 + (-0.1, 0.1)
def test_to_joint_solutions():
for search_strategy in SearchStrategy:
tol = [SymmetricTolerance(0.1, 3), NoTolerance(), NoTolerance()]
pt = TolPositionPt(
[1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol
)
robot = DummyRobot(is_colliding=True)
settings = setting_generator(search_strategy)
if search_strategy is not SearchStrategy.MIN_INCREMENTAL:
joint_solutions = pt.to_joint_solutions(robot, settings)
assert len(joint_solutions) == 0
else:
with pytest.raises(Exception) as info:
joint_solutions = pt.to_joint_solutions(robot, settings)
msg = "Maximum iterations reached in to_joint_solutions."
assert msg in str(info.value)
def test_create_line(self):
pos = [1, 2, 3]
tol = [
SymmetricTolerance(1, 10),
SymmetricTolerance(1, 10),
SymmetricTolerance(1, 10),
]
start_pt = TolPositionPt(pos, Quaternion.random(), tol)
path = create_line(start_pt, np.array([2, 2, 3]), 3)
assert path[-1].pos[0] == 2
assert_almost_equal(path[1].pos[0], 1.5)
assert path[-1].pos[1] == pos[1]
assert path[-1].pos[1] == pos[1]
assert path[-1].pos[2] == pos[2]
assert path[-1].pos[2] == pos[2]
assert path[-1].pos[0] == pos[0] + 1
assert path[-1].pos[0] == pos[0] + 1
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_sample_incremental_1(self):
"""
Rotate with pi / 2 around z, give a tolerance along x,
expect a grid sampled along y
"""
tol = [SymmetricTolerance(0.1, 3), NoTolerance(), NoTolerance()]
pt = TolPositionPt(
[1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol
)
g = pt.sample_incremental(10, SampleMethod.random_uniform)
pos_samples = [tf[:3, 3] for tf in g]
for sample in pos_samples:
assert_almost_equal(sample[0], 1)
assert_almost_equal(sample[2], 3)
assert_in_range(sample[1], 1.9, 2.1)
def test_tol_position_pt_planning_problem():
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.12], [0, 0, 0, 1]]
)
scene1 = Scene([table], [table_tf])
# create path
quat = Quaternion(axis=np.array([1, 0, 0]), angle=np.pi)
tolerance = [NoTolerance(), SymmetricTolerance(0.05, 10), NoTolerance()]
first_point = TolPositionPt(np.array([0.9, -0.2, 0.2]), quat, tolerance)
# end_position = np.array([0.9, 0.2, 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)
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
for qi, pt in zip(sol.joint_positions, path):
fk = robot.fk(qi)
pos_fk = fk[:3, 3]
pos_pt = pt.pos
R_pt = pt.rotation_matrix
pos_error = R_pt.T @ (pos_fk - pos_pt)
assert_almost_equal(pos_error[0], 0)
assert_almost_equal(pos_error[2], 0)
assert pos_error[1] <= (0.05 + 1e-6)
assert pos_error[1] >= -(0.05 + 1e-6)
def create_arc(
start_pt: PathPt, mid_point: np.ndarray, rotation_axis, angle, num_points
):
"""Copy a given toleranced PathPt along an arc with a given mid point and rotation axis."""
check_num_points(num_points)
rotation_axis = rotation_axis / norm(rotation_axis)
rotating_vector = start_pt.pos - mid_point
a = np.linspace(angle / (num_points - 1), angle, num_points - 1)
path = [deepcopy(start_pt)]
for ai in a:
rot_mat = Quaternion(angle=ai, axis=rotation_axis).rotation_matrix
offset = (rot_mat @ rotating_vector) - rotating_vector
new_pt = deepcopy(start_pt)
new_pt.translate(offset)
new_pt.rotate(rot_mat)
path.append(new_pt)
return path
def test_exceptions():
settings = SamplingSetting(
SearchStrategy.INCREMENTAL,
sample_method=SampleMethod.random_uniform,
num_samples=500,
iterations=2,
tolerance_reduction_factor=2,
)
solve_set = SolverSettings(
SolveMethod.sampling_based,
CostFuntionType.weighted_sum_squared,
sampling_settings=settings,
)
setup = PlanningSetup(None, None, None)
with pytest.raises(Exception) as e:
solve(setup, solve_set)
assert (
str(e.value)
== "No weights specified in SamplingSettings for the weighted cost function."
)
robot = Kuka()
scene = Scene([], [])
pos = np.array([1000, 0, 0])
quat = Quaternion(axis=np.array([1, 0, 0]), angle=-3 * np.pi / 4)
path = [TolPositionPt(pos, quat, 3 * [NoTolerance()])]
solve_set2 = SolverSettings(
SolveMethod.sampling_based,
CostFuntionType.sum_squared,
sampling_settings=settings,
)
setup2 = PlanningSetup(robot, path, scene)
with pytest.raises(Exception) as e:
solve(setup2, solve_set2)
assert str(e.value) == f"No valid joint solutions for path point {0}."
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)
def test_random_near_large_dist():
q = Quaternion()
for _ in range(5):
q_near = q.random_near(10.0)
assert quat_distance(q, q_near) <= (0.5 * np.pi)
assert np.any(np.not_equal(q.rotation_matrix, q_near.rotation_matrix))
def test_create(self):
TolPositionPt([1, 2, 3], Quaternion(), 3 * [NoTolerance()])
def rotate(self, R):
R_new = R @ self.quat.rotation_matrix
self.quat = Quaternion(matrix=R_new)
def test_quat_distance(self):
q1 = Quaternion()
assert_almost_equal(quat_distance(q1, q1), 0.0)
q2 = Quaternion(axis=[0, 0, 1], angle=np.pi)
assert_almost_equal(quat_distance(q1, q2), np.pi / 2)
def test_create(self):
q = Quaternion()
pos_tol = 3 * [NoTolerance()]
TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(0.5))
def test_matrix_to_rxyz(self):
for _ in range(100):
R_random = Quaternion.random().rotation_matrix
rpy = rotation_matrix_to_rpy(R_random)
R_converted = rpy_to_rot_mat(rpy)
assert_almost_equal(R_random, R_converted)
def test_max_distance():
for _ in range(10):
dist = quat_distance(Quaternion.random(), Quaternion.random())
assert dist <= (0.5 * np.pi)
def test_create_extended_quat():
q = Quaternion()
assert isinstance(q, Quaternion)