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")
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])
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()