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_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_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_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_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_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_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)
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])
# ======================================================
# Calculate forward and inverse kinematics
# ======================================================
# forward kinematics are available by default
T_fk = robot.fk([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
# inverse kinematics are implemented for specific robots
ik_solution = robot.ik(T_fk)
print(f"Inverse kinematics successful? {ik_solution.success}")
for q in ik_solution.solutions:
print(q)
# ======================================================
# Animate path and planning scene
# ======================================================
import matplotlib.pyplot as plt
from acrobotics.util import get_default_axes3d
fig, ax = get_default_axes3d([-0.8, 0.8], [-0.8, 0.8], [-0.2, 1.4])