def test_Kuka_robot(self):
bot = Kuka()
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi)
assert_almost_equal(Tactual, Tdesired)
def test_Kuka_tool_robot(self):
bot = Kuka()
bot.tf_tool_tip = torch.tf_tool_tip
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi)
Tdesired = Tdesired @ torch.tf_tool_tip
assert_almost_equal(Tactual, Tdesired)
def test_generate_envelop():
robot = Kuka()
settings = EnvelopeSettings(1.0, 10, 8)
we = generate_robot_envelope(robot, settings)
max_extension = robot.estimate_max_extension()
num_points = int(2 * max_extension / settings.sample_distance)
assert we.shape == (num_points ** 3, 4)
def test_Kuka_base_robot(self):
bot = Kuka()
bot.tf_base = pose_x(0.5, 0.1, 0.2, 0.3)
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi)
Tdesired = bot.tf_base @ Tdesired
assert_almost_equal(Tactual, Tdesired)
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_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_kuka_random(self):
""" TODO some issues with the numerical accuracy of the ik?"""
bot = Kuka()
N = 20
q_rand = np.random.rand(N, 6) * 2 * PI - PI
for qi in q_rand:
print(qi)
T1 = bot.fk(qi)
resi = bot.ik(T1)
if resi.success:
for q_sol in resi.solutions:
print(q_sol)
T2 = bot.fk(q_sol)
assert_almost_equal(T1, T2, decimal=5)
else:
# somethings is wrong, should be reachable
print(resi)
assert_almost_equal(qi, 0)
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_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_kuka_self_collision(self):
bot = Kuka()
gl = [l.geometry for l in bot.links]
q0 = [0, np.pi / 2, 0, 0, 0, 0]
q_self_collision = [0, 1.5, -1.3, 0, -1.5, 0]
tf1 = bot.fk_all_links(q0)
a1 = bot._check_self_collision(tf1, gl)
assert a1 is False
tf2 = bot.fk_all_links(q_self_collision)
a2 = bot._check_self_collision(tf2, gl)
assert a2 is True
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_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_kuka_tool_random(self):
bot = Kuka()
bot.tf_tool_tip = tool_tip_transform
N = 20
q_rand = np.random.rand(N, 6) * 2 * PI - PI
for qi in q_rand:
print(qi)
T1 = bot.fk(qi)
resi = bot.ik(T1)
if resi.success:
for q_sol in resi.solutions:
print(q_sol)
T2 = bot.fk(q_sol)
assert_almost_equal(T1, T2)
else:
# somethings is wrong, should be reachable
print(resi)
assert_almost_equal(qi, 0)
def test_plot_kuka(self):
robot = Kuka()
fig = plt.figure()
ax = fig.gca(projection="3d")
robot.plot_kinematics(ax, np.ones(robot.ndof))
Try continous collision checking for a simple path through an obstacle.
"""
import time
import fcl
import numpy as np
import matplotlib.pyplot as plt
from acrolib.plotting import get_default_axes3d, plot_reference_frame
from acrolib.geometry import translation
from acrobotics.robot_examples import Kuka
from acrobotics.tool_examples import torch2
from acrobotics.geometry import Scene
from acrobotics.shapes import Box
robot = Kuka()
robot.tool = torch2
DEBUG = False
def show_animation(robot, scene, qa, qb):
q_path = np.linspace(qa, qb, 10)
fig, ax = get_default_axes3d([-0.8, 0.8], [0, 1.6], [-0.2, 1.4])
ax.set_axis_off()
ax.view_init(elev=31, azim=-15)
scene.plot(ax, c="green")
robot.animate_path(fig, ax, q_path)
plt.show()
""" Getting started example from readme.txt This example shows how to create a planning scene, visualize a robot and check for collision between the two. Also, forward and inverse kinematics are nice. """ # ====================================================== # Get a ready to go robot implementation # ====================================================== 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])
def create_robot():
robot = Kuka()
robot.tool = torch3
return robot