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()
def test_torch_model():
fig, ax = get_default_axes3d([-0.10, 0.20], [0, 0.30], [-0.15, 0.15])
plot_reference_frame(ax, torch.tf_tool_tip)
torch.plot(ax, tf=np.eye(4), c="k")
for tf in torch.tf_s:
plot_reference_frame(ax, tf)
plot_reference_frame(ax, torch.tf_tool_tip)
def test_create_axes_3d():
fig, ax = get_default_axes3d()
assert isinstance(fig, matplotlib.pyplot.Figure)
assert isinstance(ax, mpl_toolkits.mplot3d.Axes3D)
# ======================================================
# 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 acrolib.plotting import get_default_axes3d
fig, ax = get_default_axes3d([-0.8, 0.8], [-0.8, 0.8], [-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)
# robot.animation.save("examples/robot_animation.gif", writer="imagemagick", fps=10)
plt.show()
def test_plot_reference_frame():
_, ax = get_default_axes3d()
plot_reference_frame(ax)
plot_reference_frame(ax, tf=np.eye(4))
plot_reference_frame(ax, tf=np.eye(4), arrow_length=0.3)