def test_get_normals(self):
b = Box(1, 2, 3)
n = b.get_normals(tf_identity)
desired = np.array(
[[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]]
)
assert_almost_equal(n, desired)
def test_polyhedron(self):
b = Box(1, 2, 3)
A, b = b.get_polyhedron(np.eye(4))
Aa = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5])
assert_almost_equal(A, Aa)
assert_almost_equal(b, ba)
def test_get_edges(self):
b = Box(1, 2, 3)
e = b.get_edges(tf_identity)
row, col = e.shape
assert row == 12
assert col == 6
v = b.get_vertices(tf_identity)
# check only one edge
v0 = np.hstack((v[0], v[1]))
assert_almost_equal(v0, e[0])
def test_polyhedron_transformed(self):
b = Box(1, 2, 3)
tf = pose_z(0.3, 0.1, 0.2, -0.3)
A, b = b.get_polyhedron(tf)
Aa = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5])
Aa = np.dot(Aa, tf[:3, :3].T)
ba = ba + np.dot(Aa, tf[:3, 3])
assert_almost_equal(A, Aa)
assert_almost_equal(b, ba)
def test_plot(self):
tf2 = pose_z(np.pi / 6, 9, 0, 0)
soup = Scene([Box(1, 1, 1), Box(2, 2, 0.5)], [tf_identity, tf2])
fig = plt.figure()
ax = fig.gca(projection="3d")
ax.set_xlim([0, 10])
ax.set_ylim([-5, 5])
ax.set_zlim([-5, 5])
soup.plot(ax, c="g")
tf3 = pose_z(-np.pi / 4, 0, 3, 0)
soup.plot(ax, tf=tf3, c="r")
def test_get_vertices(self):
b = Box(1, 2, 3)
v = b.get_vertices(tf_identity)
desired = np.array([
[-0.5, 1, 1.5],
[-0.5, 1, -1.5],
[-0.5, -1, 1.5],
[-0.5, -1, -1.5],
[0.5, 1, 1.5],
[0.5, 1, -1.5],
[0.5, -1, 1.5],
[0.5, -1, -1.5],
])
assert_almost_equal(v, desired)
def test_is_in_collision(self):
b1 = Box(1, 1, 1)
b2 = Box(1, 1, 2)
actual = b1.is_in_collision(tf_identity, b2, tf_identity)
assert actual == True
b3 = Box(1, 2, 1)
T3 = pose_z(np.pi / 4, 0.7, 0.7, 0)
assert b1.is_in_collision(tf_identity, b3, T3) == True
b4 = Box(1, 1, 1)
b5 = Box(1, 1, 2)
T4 = pose_z(0, -1, -1, 0)
T5 = pose_z(np.pi / 4, -2, -2, 0)
assert b4.is_in_collision(T4, b5, T5) == False
def test_kuka_collision(self):
bot = Kuka()
q0 = [0, np.pi / 2, 0, 0, 0, 0]
obj1 = ShapeSoup(
[Box(0.2, 0.3, 0.5), Box(0.1, 0.3, 0.1)],
[pose_x(0, 0.75, 0, 0.5), pose_x(0, 0.75, 0.5, 0.5)],
)
obj2 = ShapeSoup(
[Box(0.2, 0.3, 0.5), Box(0.1, 0.3, 0.1)],
[pose_x(0, 0.3, -0.7, 0.5), pose_x(0, 0.75, 0.5, 0.5)],
)
a1 = bot.is_in_collision(q0, obj1)
assert a1 is True
a2 = bot.is_in_collision(q0, obj2)
assert a2 is False
def test_ccd_3():
table = Box(2, 2, 0.1)
T_table = translation(0, 0, -0.2)
obstacle = Box(0.01, 0.2, 0.2)
T_obs = translation(0, 1.2, 0)
scene = Scene([table, obstacle], [T_table, T_obs])
q_start = np.array([1.0, 1.2, -0.5, 0, 0, 0])
q_goal = np.array([2.0, 1.2, -0.5, 0, 0, 0])
res = robot.is_path_in_collision(q_start, q_goal, scene)
assert res
if DEBUG:
print("resut test 3: ", res)
show_animation(robot, scene, q_start, q_goal)
def test_set_transform(self):
b = Box(1, 2, 3)
tf = np.eye(4)
tf[0, 3] = 10.5
v = b.get_vertices(tf)
desired = np.array([
[10, 1, 1.5],
[10, 1, -1.5],
[10, -1, 1.5],
[10, -1, -1.5],
[11, 1, 1.5],
[11, 1, -1.5],
[11, -1, 1.5],
[11, -1, -1.5],
])
assert_almost_equal(v, desired)
def test_init(self):
dh_params = DHLink(0.1, np.pi / 4, -0.1, np.pi / 6)
geometry = Scene([Box(1, 2, 3)], [np.eye(4)])
link1 = Link(dh_params, JointType.revolute, geometry)
fig = plt.figure()
ax = fig.gca(projection="3d")
link1.plot(ax, np.eye(4))
def test_set_transform2(self):
b = Box(1, 2, 3)
tf = np.eye(4)
# rotate pi / 2 around x-axis
tf[1:3, 1:3] = np.array([[0, -1], [1, 0]])
v = b.get_vertices(tf)
desired = np.array([
[-0.5, -1.5, 1],
[-0.5, 1.5, 1],
[-0.5, -1.5, -1],
[-0.5, 1.5, -1],
[0.5, -1.5, 1],
[0.5, 1.5, 1],
[0.5, -1.5, -1],
[0.5, 1.5, -1],
])
assert_almost_equal(v, desired)
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_polyhedron(self):
b = Box(1, 2, 3)
tf = pose_z(0.3, 0.1, 0.2, -0.3)
col = Scene([b], [tf])
polys = col.get_polyhedrons()
assert len(polys) == 1
Aa = np.array(
[[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]]
)
ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5])
Aa = np.dot(Aa, tf[:3, :3].T)
ba = ba + np.dot(Aa, tf[:3, 3])
assert_almost_equal(polys[0].A, Aa)
assert_almost_equal(polys[0].b, ba)
def parse_link(link):
""" Assume a link has only a single collision object.
Assume this collision object is a box.
Assume the link named "world" has no collision objects.
Parameters
----------
link: a urdfpy.urdf.Link object
"""
assert len(link.collisions) == 1
c = link.collisions[0]
assert c.geometry.box is not None
size = c.geometry.box.size
return Box(size[0], size[1], size[2])
def test_complete_problem():
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(
FreeOrientationPt(
[xi, yi, zi],
[NoTolerance(), NoTolerance(),
NoTolerance()]))
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 = ShapeSoup([table], [table_tf])
robot = Kuka()
# robot.tool = torch
settings = SamplingSetting(
SearchStrategy.INCREMENTAL,
1,
SampleMethod.random_uniform,
500,
tolerance_reduction_factor=2,
)
solve_set = SolverSettings(
SolveMethod.sampling_based,
CostFuntionType.sum_squared,
sampling_settings=settings,
)
setup = PlanningSetup(robot, path_ori_free, scene1)
sol1 = solve(setup, solve_set)
assert sol1.success
s2 = SolverSettings(
SolveMethod.optimization_based,
CostFuntionType.sum_squared,
opt_settings=OptSettings(q_init=np.array(sol1.joint_positions)),
)
sol2 = solve(setup, s2)
assert sol2.success
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_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_scene(position):
""" Welding case."""
obs_h = 0.03
obs_off = 0.04
L = 0.5 # length of all stuff in y-dir
obs_l = L / 6
u_off = 0.005
# U-profile
# obstacle
# table
# side U-profile 1
# side U-profile 2
dims = [
[0.27, L, 0.21],
[0.2, obs_l, obs_h],
[0.7, 0.7, 0.1],
[0.2, 0.1, 0.055],
[0.2, 0.1, 0.055],
]
pos = [
[dims[0][0] / 2, 0, dims[0][2] / 2],
[-(dims[1][0] / 2) - obs_off, 0, obs_h / 2],
[0, 0, -0.10],
[-dims[3][0] / 2, L / 2 - dims[3][1] / 2, dims[3][2] / 2],
[-dims[3][0] / 2, -L / 2 + dims[3][1] / 2, dims[3][2] / 2],
]
pos = [p + position for p in pos]
shapes = [Box(d[0], d[1], d[2]) for d in dims]
tfs = [translation(p[0], p[1], p[2]) for p in pos]
path_start = np.array([0, -L / 2 + dims[3][1] + u_off, 0]) + position
path_stop = np.array([0, L / 2 - dims[3][1] - u_off, 0]) + position
return Scene(shapes, tfs), path_start, path_stop
def test_is_in_collision(self):
soup_1 = Scene([Box(1, 1, 1)], [tf_identity])
soup_2 = Scene([Box(1, 1, 2)], [tf_identity])
assert soup_1.is_in_collision(soup_2) == True
assert soup_1.is_in_collision(soup_2, tf_self=tf_far_away) == False
assert soup_1.is_in_collision(soup_2, tf_other=tf_far_away) == False
soup_3 = Scene(
[Box(1, 1, 1), Box(0.3, 0.2, 0.1)],
[pose_z(0.0, -1.0, -1.0, 0.0), -tf_far_away],
)
soup_4 = Scene(
[Box(1, 1, 2), Box(0.3, 0.2, 0.1)],
[pose_z(np.pi / 4, -2, -2, 0), tf_far_away],
)
assert soup_3.is_in_collision(soup_4) == False
assert soup_4.is_in_collision(soup_3) == False
import time
import numpy as np
from numpy.random import uniform
from acrobotics.shapes import Box
from acrobotics.util import pose_x
box1 = Box(1, 0.5, 2)
box2 = Box(0.7, 1.5, 1)
def check_random_collision():
tf1 = pose_x(uniform(-2, 2), uniform(0, 3), 0, 0)
tf2 = pose_x(uniform(-2, 2), 0, uniform(0, 3), 0)
return box1.is_in_collision(tf1, box2, tf2)
N = 1000
run_times = np.zeros(N)
results = np.zeros(N, dtype=bool)
for i in range(N):
start = time.time()
res = check_random_collision()
end = time.time()
run_times[i] = end - start
results[i] = res
print(f"{N} runs took an average of {np.mean(run_times)*1000} ms per run")
print(f"Not in collion average: {1000 * np.mean(run_times[results == False])}")
# ====================================================== from acrobotics.robot_examples import Kuka robot = Kuka() # ====================================================== # Create planning scene # ====================================================== from acrobotics.geometry import Scene from acrobotics.shapes import Box # A transform matrix is represented by a 4x4 numpy array # Here we create one using a helper function for readability from acrobotics.util import translation table = Box(2, 2, 0.1) T_table = translation(0, 0, -0.2) obstacle = Box(0.2, 0.2, 1.5) T_obs = translation(0, 0.5, 0.55) scene = Scene([table, obstacle], [T_table, T_obs]) # ====================================================== # Linear interpolation path from start to goal # ====================================================== import numpy as np q_start = np.array([0.5, 1.5, -0.3, 0, 0, 0]) q_goal = np.array([2.5, 1.5, 0.3, 0, 0, 0]) q_path = np.linspace(q_start, q_goal, 10)
def test_plot(self):
b1 = Box(1, 2, 3)
fig = plt.figure()
ax = fig.gca(projection="3d")
b1.plot(ax, tf_identity)
assert True
def test_init(self):
Box(1, 2, 3)
