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_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 plot_kinematics(self, ax, q, *arg, **kwarg):
# base frame (0)
plot_reference_frame(ax, self.tf_base)
# link frames (1-ndof)
tf_links = self.fk_all_links(q)
points = [tf[0:3, 3] for tf in tf_links]
points = np.array(points)
points = np.vstack((self.tf_base[0:3, 3], points))
ax.plot(points[:, 0], points[:, 1], points[:, 2], "o-")
for tfi in tf_links:
plot_reference_frame(ax, tfi)
# tool tip frame
if self.tf_tool_tip is not None:
tf_tt = np.dot(tf_links[-1], self.tf_tool_tip)
plot_reference_frame(ax, tf_tt)
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)
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()
R_pt = np.eye(4)
pos_tol = 3 * [NoTolerance()]
quat_tol = QuaternionTolerance(0.1)
pt_quat = TolQuatPt(np.zeros(3), Quaternion(matrix=R_pt), pos_tol, quat_tol)
# ==================================================================================
# Create plot and save it
# ==================================================================================
fig = plt.figure(figsize=plt.figaspect(1 / 3))
axes = [fig.add_subplot(1, 3, i, projection="3d") for i in [1, 2, 3]]
# all axis show the path point reference frame and have no coordinate axes
for pt, ax in zip([pt_pos, pt_eul, pt_quat], axes):
ax.set_axis_off()
plot_reference_frame(ax, tf=pt.transformation_matrix, arrow_length=0.2)
# here we plot the samples for each specific point
ax1, ax2, ax3 = axes[0], axes[1], axes[2]
plot_reference_frame(ax1, arrow_length=0.3)
for tf in pt_pos.sample_grid():
ax1.scatter(tf[0, 3], tf[1, 3], tf[2, 3], "o", c="black")
for tf in pt_eul.sample_grid():
plot_reference_frame(ax2, tf, arrow_length=0.1)
for tf in pt_quat.sample_incremental(50, SampleMethod.random_uniform):
plot_reference_frame(ax3, tf, arrow_length=0.1)
# here we tweak the view a bit to make it look nice