"""
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()
def test_ccd_1():
def create_robot():
robot = Kuka()
robot.tool = torch3
return robot