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 show_path(robot, path, scene, solution: Solution):
fig2, ax2 = get_default_axes3d(
xlim=[0, 1.5], ylim=[-0.75, 0.75], zlim=[-0.75, 0.75]
)
scene.plot(ax2, c="g")
for pt in path:
plot_reference_frame(ax2, pt.transformation_matrix, 0.1)
if solution.success:
robot.animate_path(fig2, ax2, solution.joint_path)
else:
print("Cannot show solution of a failed solver solution.")
def test_plot_reference_frame(self):
_, 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)
def test_create_axes_3d(self):
fig, ax = get_default_axes3d()
assert isinstance(fig, matplotlib.pyplot.Figure)
assert isinstance(ax, mpl_toolkits.mplot3d.Axes3D)
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()
start_pt = TolEulerPt(start, Quaternion(matrix=R_pt), pos_tol, rot_tol)
return create_line(start_pt, stop, n_path)
def show_path(robot, path, scene, solution: Solution):
fig2, ax2 = get_default_axes3d(
xlim=[0, 1.5], ylim=[-0.75, 0.75], zlim=[-0.75, 0.75]
)
scene.plot(ax2, c="g")
for pt in path:
plot_reference_frame(ax2, pt.transformation_matrix, 0.1)
if solution.success:
robot.animate_path(fig2, ax2, solution.joint_path)
else:
print("Cannot show solution of a failed solver solution.")
if __name__ == "__main__":
# show the planning setup
robot = create_robot()
scene, path_start, path_stop = create_scene(np.array([0.85, 0, 0]))
path = create_path(path_start, path_stop, 20)
_, ax = get_default_axes3d()
robot.plot(ax, [0, 1.5, 0, 0, 0, 0], c="black")
scene.plot(ax, c="green")
for pt in path:
plot_reference_frame(ax, tf=pt.transformation_matrix)
plt.show()