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 create_cc(opti, robot: Robot, scene: Scene, q):
poly_robot = []
for link in robot.links:
poly_robot.extend(link.geometry.get_polyhedrons())
if robot.geometry_tool is not None:
poly_robot.extend(robot.geometry_tool.get_polyhedrons())
poly_scene = scene.get_polyhedrons()
S = len(poly_robot[0].b)
nobs = len(poly_scene)
N, _ = q.shape
robs = len(poly_robot)
# dual variables arranged in convenient lists to acces with indices
lam = [
[[opti.variable(S) for j in range(nobs)] for i in range(robs)] for k in range(N)
]
mu = [
[[opti.variable(S) for j in range(nobs)] for i in range(robs)] for k in range(N)
]
cons = []
for k in range(N):
cons.extend(
create_cc_for_joint_pose(
robot, poly_robot, poly_scene, q[k, :], lam[k], mu[k]
)
)
return cons
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_kuka_collision(self):
bot = Kuka()
q0 = [0, np.pi / 2, 0, 0, 0, 0]
obj1 = Scene(
[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 = Scene(
[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_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_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_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_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_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 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 import_urdf(name, path):
# add '.urdf' extension if it was not added by the user
if not name.endswith(".urdf"):
name += ".urdf"
urdf = URDF.load("{}/{}".format(path, name))
root = urdf.base_link.name
shapes = {}
for link in urdf.links:
if len(link.collisions) == 0:
continue
else:
shapes[link.name] = parse_link(link)
tfs = []
final_shapes = []
for joint in urdf.joints:
if joint.parent == root:
tfs.append(joint.origin)
final_shapes.append(shapes[joint.child])
return Scene(final_shapes, tfs)
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
# 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) # ====================================================== # Show which parts of the path are in collision # ====================================================== print([robot.is_in_collision(q, scene) for q in q_path])